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); }