| public static Transpose ( CMatrix, p_m ) : CMatrix, | ||
| p_m | CMatrix, | |
| Résultat | CMatrix, | |
public static void updateJacobianTranspose(List <Joint> p_joints, Vector3 p_targetPos, Vector3 p_axis)
{
int linkCount = p_joints.Count;
if (linkCount == 0)
{
return;
}
// Calculate Jacobian matrix
CMatrix J = calculateJacobian(p_joints, linkCount, p_targetPos, -p_axis);
// Calculate Jacobian transpose
CMatrix Jt = CMatrix.Transpose(J);
// Calculate error matrix
CMatrix e = new CMatrix(3, 1);
e[0, 0] = p_joints[linkCount - 1].m_endPoint.x - p_targetPos.x;
e[1, 0] = p_joints[linkCount - 1].m_endPoint.y - p_targetPos.y;
e[2, 0] = p_joints[linkCount - 1].m_endPoint.z - p_targetPos.z;
float error = CMatrix.Dot(e, e);
if (error < 0.0001f)
{
return;
}
// Calculate mu for inverse estimation
// ie. a small scalar constant used as step size
float mudiv = CMatrix.Dot(J * Jt * e, J * Jt * e);
if (mudiv == 0.0f)
{
return;
}
float mu = CMatrix.Dot(e, J * Jt * e) / mudiv;
// Step matrix
CMatrix deltaAngle = Jt * (mu * e);
// Make sure the matrix is correct
if (deltaAngle.m_rows != linkCount)
{
Debug.Log("Not correct amount of rows! (" + deltaAngle.m_rows + ") correct is " + linkCount);
}
if (deltaAngle.m_cols != 1)
{
Debug.Log("Not correct amount of cols! (" + deltaAngle.m_cols + ") correct is 1");
}
for (int i = 0; i < linkCount; i++)
{
p_joints[i].m_angle += p_axis * deltaAngle[i, 0];
}
}
void updateChain()
{
// Just copy from objects
Vector3 end = transform.position;
for (int i = 0; i < m_chain.Count; i++)
{
Joint current = m_chain[i];
GameObject currentObj = m_chainObjs[i];
current.length = currentObj.transform.localScale.x;
current.m_position = currentObj.transform.position - currentObj.transform.right * current.length * 0.5f;
current.m_endPoint = currentObj.transform.position + currentObj.transform.right * current.length * 0.5f;
end = current.m_endPoint;
}
//CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward);
CMatrix J = Jacobian.calculateJacobian(m_chain, m_chainObjs, m_dofs, m_dofJointId, end + m_virtualForce);
CMatrix Jt = CMatrix.Transpose(J);
CMatrix force = new CMatrix(3, 1);
force[0, 0] = m_virtualForce.x;
force[1, 0] = m_virtualForce.y;
force[2, 0] = m_virtualForce.z;
//CMatrix torqueSet = Jt*force;
Debug.Log(Jt.m_rows + "x" + Jt.m_cols);
//Debug.Log(torqueSet.m_rows+"x"+torqueSet.m_cols);
//for (int i = 0; i < m_chain.Count; i++)
//{
// // store torque
// m_torques[i] = Vector3.forward*Vector3.Dot(new Vector3(Jt[i,0],Jt[i,1],Jt[i,2]),m_virtualForce);
// //Debug.Log(m_torques[i].ToString());
//}
// This could either be a new kernel, or continuation of the old
// Here in GPGPU, we could zero the torque position each time...
for (int i = 0; i < m_chain.Count; i++)
{
m_torques[i] = Vector3.zero; // for each dof, for its joint id (as if it had been in the next loop essentially)
}
// ... but force sync here, so the following add-op isn't overwritten:
for (int i = 0; i < m_dofs.Count; i++)
{
// store torque
int x = m_dofJointId[i];
m_torques[x] += /*m_chainObjs[x].transform.TransformDirection(m_dofs[i])*/ m_dofs[i] * Vector3.Dot(new Vector3(Jt[i, 0], Jt[i, 1], Jt[i, 2]), m_virtualForce);
//Debug.Log(m_torques[i].ToString());
}
// Come to think of it, the jacobian and torque could be calculated in the same
// kernel as it lessens write to global memory and the need to fetch joint matrices several time (transform above)
}
// Compute the torque of all applied virtual forces
Vector3[] computeVFTorques(float p_phi, float p_dt)
{
Vector3[] newTorques = new Vector3[m_jointTorques.Length];
if (m_useVFTorque)
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
lf.calculateNetLegVF(p_phi, p_dt, m_currentVelocity, m_desiredVelocity);
// Calculate torques using each leg chain
for (int n = 0; n < LegFrame.c_legCount; n++)
{
// get the joints
int legFrameRoot = lf.m_id;
//legFrameRoot = -1;
int legRoot = lf.m_neighbourJointIds[n];
int legSegmentCount = LegFrame.c_legSegments; // hardcoded now
// Use joint ids to get dof ids
// Start in chain
int legFrameRootDofId = -1; // if we have separate root as base link
if (legFrameRoot != -1)
{
legFrameRootDofId = m_chain[legFrameRoot].m_dofListIdx;
}
// otherwise, use first in chain as base link
int legRootDofId = m_chain[legRoot].m_dofListIdx;
// end in chain
int lastDofIdx = legRoot + legSegmentCount - 1;
int legDofEnd = m_chain[lastDofIdx].m_dofListIdx + m_chain[lastDofIdx].m_dof.Length;
//
// get force for the leg
Vector3 VF = lf.m_netLegBaseVirtualForces[n];
// Calculate torques for each joint
// Start by updating joint information based on their gameobjects
Vector3 end = transform.localPosition;
//Debug.Log("legroot "+legRoot+" legseg "+legSegmentCount);
int jointstart = legRoot;
if (legFrameRoot != -1)
{
jointstart = legFrameRoot;
}
for (int x = jointstart; x < legRoot + legSegmentCount; x++)
{
if (legFrameRoot != -1 && x < legRoot && x != legFrameRoot)
{
x = legRoot;
}
Joint current = m_chain[x];
GameObject currentObj = m_chainObjs[x];
//Debug.Log("joint pos: " + currentObj.transform.localPosition);
// Update Joint
current.length = currentObj.transform.localScale.y;
current.m_position = currentObj.transform.position /*- (-currentObj.transform.up) * current.length * 0.5f*/;
current.m_endPoint = currentObj.transform.position + (-currentObj.transform.up) * current.length /* * 0.5f*/;
//m_chain[i] = current;
//Debug.DrawLine(current.m_position, current.m_endPoint, Color.red);
//Debug.Log(x+" joint pos: " + current.m_position + " = " + m_chain[x].m_position);
end = current.m_endPoint;
}
//foreach(Joint j in m_chain)
// Debug.Log("joint pos CC: " + j.m_position);
//CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward);
CMatrix J = Jacobian.calculateJacobian(m_chain, // Joints (Joint script)
m_chainObjs, // Gameobjects in chain
m_dofs, // Degrees Of Freedom (Per joint)
m_dofJointId, // Joint id per DOF
end + VF, // Target position
legRootDofId, // Starting link id in chain (start offset)
legDofEnd, // End of chain of link (ie. size)
legFrameRootDofId); // As we use the leg frame as base, we supply it separately (it will be actual root now)
CMatrix Jt = CMatrix.Transpose(J);
//Debug.DrawLine(end, end + VF, Color.magenta, 0.3f);
int jIdx = 0;
int extra = 0;
int start = legRootDofId;
if (legFrameRootDofId >= 0)
{
start = legFrameRootDofId;
extra = m_chain[legFrameRoot].m_dof.Length;
}
for (int g = start; g < legDofEnd; g++)
{
if (extra > 0)
{
extra--;
}
else if (g < legRootDofId)
{
g = legRootDofId;
}
// store torque
int x = m_dofJointId[g];
Vector3 addT = m_dofs[g] * Vector3.Dot(new Vector3(Jt[jIdx, 0], Jt[jIdx, 1], Jt[jIdx, 2]), VF);
Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + VF, new Color(0.0f, 153.0f / 256.0f, 0.0f));
newTorques[x] += addT;
jIdx++;
//Vector3 drawTorque = new Vector3(0.0f, 0.0f, -addT.x);
//Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + drawTorque*0.1f, Color.cyan);
}
// Come to think of it, the jacobian and torque could be calculated in the same
// kernel as it lessens write to global memory and the need to fetch joint matrices several time (transform above)
}
}
}
return(newTorques);
}
// Compute the torque needed on swing legs to compensate for gravity
Vector3[] computeCGVFTorques(float p_phi, float p_dt)
{
Vector3[] newTorques = new Vector3[m_jointTorques.Length];
if (m_useVFTorque)
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
// Calculate torques using each leg chain
for (int n = 0; n < LegFrame.c_legCount; n++) // for each leg
{
if (!lf.isInControlledStance(n, m_player.m_gaitPhase)) // only on swing
{
for (int m = 0; m < LegFrame.c_legSegments; m++) // for each segment in leg
{
// get the joints
int segIdx = n * LegFrame.c_legSegments + m + 1; // get segment index in list (+1 to offset for LF)
//Debug.Log("sidx: " + segIdx);
Rigidbody segment = m_joints[segIdx];
lf.calculateFgravcomp(segIdx - 1, segment);
//
// Calculate jacobian
//
// get the joints
int legFrameRoot = lf.m_id;
//legFrameRoot = -1;
int legRoot = lf.m_neighbourJointIds[n];
int legSegmentCount = m + 1; // the amount of segments decreases the further in in the hierarchy we get
// Use joint ids to get dof ids
// Start in chain
int legFrameRootDofId = -1; // if we have separate root as base link
if (legFrameRoot != -1)
{
legFrameRootDofId = m_chain[legFrameRoot].m_dofListIdx;
}
// otherwise, use first in chain as base link
int legRootDofId = m_chain[legRoot].m_dofListIdx;
// end in chain
int lastDofIdx = legRoot + m;
int legDofEnd = m_chain[lastDofIdx].m_dofListIdx + m_chain[lastDofIdx].m_dof.Length;
//
// get force for the leg
Vector3 VF = lf.m_legSegmentGravityCompVirtualForces[segIdx - 1];
// Calculate torques for each joint
// Start by updating joint information based on their gameobjects
Vector3 end = segment.transform.TransformPoint(segment.centerOfMass);
//foreach(Joint j in m_chain)
// Debug.Log("joint pos CC: " + j.m_position);
//CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward);
CMatrix J = Jacobian.calculateJacobian(m_chain, // Joints (Joint script)
m_chainObjs, // Gameobjects in chain
m_dofs, // Degrees Of Freedom (Per joint)
m_dofJointId, // Joint id per DOF
end + VF, // Target position
legRootDofId, // Starting link id in chain (start offset)
legDofEnd, // End of chain of link (ie. size)
legFrameRootDofId); // As we use the leg frame as base, we supply it separately (it will be actual root now)
CMatrix Jt = CMatrix.Transpose(J);
//Debug.DrawLine(end, end + VF * 0.4f, Color.magenta);
/*Color idxCol = new Color((float)n / (float)LegFrame.c_legCount, (float)m / (float)LegFrame.c_legSegments, (float)segIdx / (float)(LegFrame.c_legCount * LegFrame.c_legSegments));
* Debug.DrawLine(end, end + VF, idxCol);
* Debug.DrawLine(end, end + VF, idxCol);*/
int jIdx = 0;
int extra = 0;
int start = legRootDofId;
if (legFrameRootDofId >= 0)
{
start = legFrameRootDofId;
extra = m_chain[legFrameRoot].m_dof.Length;
}
for (int g = start; g < legDofEnd; g++)
{
if (extra > 0)
{
extra--;
}
else if (g < legRootDofId)
{
g = legRootDofId;
}
// store torque
int x = m_dofJointId[g];
Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + VF, new Color(1.0f, 153.0f / 255.0f, 153.0f / 255.0f));
Vector3 addT = m_dofs[g] * Vector3.Dot(new Vector3(Jt[jIdx, 0], Jt[jIdx, 1], Jt[jIdx, 2]), VF);
newTorques[x] += addT;
jIdx++;
Vector3 drawTorque = addT;
//Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + drawTorque * 0.5f, idxCol);
}
} // endfor legsegments
} // endif not-stance
} // endfor legs
}
}
return(newTorques);
}
