| public isInControlledStance ( int p_idx, float p_phi ) : bool | ||
| p_idx | int | |
| p_phi | float | |
| return | bool | |
void debugColorLegs()
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
for (int n = 0; n < lf.m_tuneStepCycles.Length; n++)
{
StepCycle cycle = lf.m_tuneStepCycles[n];
Rigidbody current = m_joints[lf.m_neighbourJointIds[n]];
if (lf.isInControlledStance(n, m_player.m_gaitPhase))
{
current.gameObject.GetComponentInChildren <Renderer>().material.color = Color.yellow;
}
else
{
current.gameObject.GetComponentInChildren <Renderer>().material.color = Color.white;
}
}
}
}
// Compute the torque of all PD-controllers in the joints
Vector3[] computePDTorques(float p_phi)
{
// This loop might have to be rewritten into something a little less cumbersome
Vector3[] newTorques = new Vector3[m_jointTorques.Length];
if (m_usePDTorque)
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
newTorques[lf.m_id] = m_jointTorques[lf.m_id];
// All hip joints
for (int n = 0; n < lf.m_tuneStepCycles.Length; n++)
{
StepCycle cycle = lf.m_tuneStepCycles[n];
int jointID = lf.m_neighbourJointIds[n];
if (lf.isInControlledStance(i, m_player.m_gaitPhase))
{
newTorques[jointID] = Vector3.zero;
// m_jointTorques[jointID];
//Vector3.zero;
//
}
else if (m_desiredJointTorquesPD.Length > 0)
{
newTorques[jointID] = m_desiredJointTorquesPD[jointID].m_vec;
}
}
// All other joints
for (int n = 0; n < lf.m_legJointIds.Length; n++)
{
int jointID = lf.m_legJointIds[n];
if (jointID > -1)
{
newTorques[jointID] = m_desiredJointTorquesPD[jointID].m_vec;
}
}
}
}
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);
}