void drawStepCycles(float p_phi, float p_yOffset, LegFrame p_frame, int legFrameId)
{
for (int i = 0; i < LegFrame.c_legCount; i++)
{
StepCycle cycle = p_frame.m_tuneStepCycles[i];
if (cycle != null)
{
// DRAW!
float timelineLen = 300.0f;
float xpad = 10.0f;
float offset = cycle.m_tuneStepTrigger;
float len = cycle.m_tuneDutyFactor;
float lineStart = xpad;
float lineEnd = lineStart + timelineLen;
float dutyEnd = lineStart + timelineLen * (offset + len);
float w = 4.0f;
float y = p_yOffset + (float)i * w * 2.0f;
bool stance = cycle.isInStance(p_phi);
// Draw back
Color ucol = Color.white * 0.5f + new Color((float)(legFrameId % 2), (float)(i % 2), 1 - (float)(i % 2), 1.0f);
int h = (int)w / 2;
Drawing.DrawLine(new Vector2(lineStart - 1, y - h - 1), new Vector2(lineEnd + 1, y - h - 1), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart - 1, y + h), new Vector2(lineEnd + 1, y + h), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart - 1, y - h - 1), new Vector2(lineStart - 1, y + h + 1), Color.black, 1);
Drawing.DrawLine(new Vector2(lineEnd + 1, y - h - 1), new Vector2(lineEnd + 1, y + h), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineEnd, y), new Color(1.0f, 1.0f, 1.0f, 1.0f), w);
// Color depending on stance
Color currentCol = Color.black;
float phase = cycle.getStancePhase(p_phi);
if (stance)
{
currentCol = Color.Lerp(ucol, Color.black, phase * phase);
}
// draw df
Drawing.DrawLine(new Vector2(lineStart + timelineLen * offset, y), new Vector2(Mathf.Min(lineEnd, dutyEnd), y), currentCol, w);
// draw rest if out of bounds
if (offset + len > 1.0f)
{
Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineStart + timelineLen * (offset + len - 1.0f), y), currentCol, w);
}
// Draw current time marker
Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi - 1, y), new Vector2(lineStart + timelineLen * p_phi + 3, y),
Color.red, w);
Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi, y), new Vector2(lineStart + timelineLen * p_phi + 2, y),
Color.green * 2, w);
}
}
}
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 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);
}
void drawStepCycles(float p_phi,float p_yOffset,LegFrame p_frame, int legFrameId)
{
for (int i = 0; i < LegFrame.c_legCount; i++)
{
StepCycle cycle = p_frame.m_tuneStepCycles[i];
if (cycle!=null)
{
// DRAW!
float timelineLen = 300.0f;
float xpad = 10.0f;
float offset = cycle.m_tuneStepTrigger;
float len = cycle.m_tuneDutyFactor;
float lineStart = xpad;
float lineEnd = lineStart + timelineLen;
float dutyEnd = lineStart + timelineLen * (offset + len);
float w = 4.0f;
float y = p_yOffset+(float)i*w*2.0f;
bool stance = cycle.isInStance(p_phi);
// Draw back
Color ucol = Color.white*0.5f+new Color((float)(legFrameId%2), (float)(i%2), 1-(float)(i%2),1.0f);
int h = (int)w / 2;
Drawing.DrawLine(new Vector2(lineStart-1, y-h-1), new Vector2(lineEnd+1, y-h-1), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart-1, y+h), new Vector2(lineEnd+1, y+h), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart-1, y-h-1), new Vector2(lineStart-1, y+h+1), Color.black, 1);
Drawing.DrawLine(new Vector2(lineEnd+1, y-h-1), new Vector2(lineEnd+1, y+h), Color.black, 1);
Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineEnd, y), new Color(1.0f,1.0f,1.0f,1.0f), w);
// Color depending on stance
Color currentCol = Color.black;
float phase = cycle.getStancePhase(p_phi);
if (stance)
currentCol = Color.Lerp(ucol, Color.black, phase*phase);
// draw df
Drawing.DrawLine(new Vector2(lineStart + timelineLen * offset, y), new Vector2(Mathf.Min(lineEnd, dutyEnd), y), currentCol, w);
// draw rest if out of bounds
if (offset + len > 1.0f)
Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineStart + timelineLen * (offset + len - 1.0f), y), currentCol, w);
// Draw current time marker
Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi-1, y), new Vector2(lineStart + timelineLen * p_phi + 3, y),
Color.red, w);
Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi, y), new Vector2(lineStart + timelineLen * p_phi + 2, y),
Color.green*2, w);
}
}
}
