/**
This method is used to compute torques for the stance leg that help achieve a desired speed in the sagittal and lateral planes
*/
void IKVMCController::computeLegTorques(int ankleIndex, int kneeIndex, int hipIndex, DynamicArray<ContactPoint> *cfs) {
Vector3d fA = computeVirtualForce();
Vector3d r;
Point3d p = comPosition;
r.setToVectorBetween(character->joints[ankleIndex]->getChild()->getWorldCoordinates(character->joints[ankleIndex]->getChildJointPosition()), p);
Vector3d ankleTorque = r.crossProductWith(fA);
preprocessAnkleVTorque(ankleIndex, cfs, &ankleTorque);
torques[ankleIndex] += ankleTorque;
r.setToVectorBetween(character->joints[kneeIndex]->getChild()->getWorldCoordinates(character->joints[kneeIndex]->getChildJointPosition()), p);
torques[kneeIndex] += r.crossProductWith(fA);
r.setToVectorBetween(character->joints[hipIndex]->getChild()->getWorldCoordinates(character->joints[hipIndex]->getChildJointPosition()), p);
torques[hipIndex] += r.crossProductWith(fA);
//the torque on the stance hip is cancelled out, so pass it in as a torque that the root wants to see!
ffRootTorque -= r.crossProductWith(fA);
int lBackIndex = character->getJointIndex("pelvis_lowerback");
r.setToVectorBetween(character->joints[lBackIndex]->getChild()->getWorldCoordinates(character->joints[lBackIndex]->getChildJointPosition()), p);
torques[lBackIndex] += r.crossProductWith(fA) / 10;
int mBackIndex = character->getJointIndex("lowerback_torso");
r.setToVectorBetween(character->joints[mBackIndex]->getChild()->getWorldCoordinates(character->joints[mBackIndex]->getChildJointPosition()), p);
torques[mBackIndex] += r.crossProductWith(fA) / 10;
}
/**
This method is used to compute the torques that mimick the effect of applying a force on
a rigid body, at some point. It works best if the end joint is connected to something that
is grounded, otherwise (I think) this is just an approximation.
This function works by making use of the formula:
t = J' * f, where J' is dp/dq, where p is the position where the force is applied, q is
'sorta' the relative orientation between links. It makes the connection between the velocity
of the point p and the relative angular velocities at each joint. Here's an example of how to compute it.
Assume: p = pBase + R1 * v1 + R2 * v2, where R1 is the matrix from link 1 to whatever pBase is specified in,
and R2 is the rotation matrix from link 2 to whatever pBase is specified in, v1 is the point from link 1's
origin to link 2's origin (in link 1 coordinates), and v2 is the vector from origin of link 2 to p
(in link 2 coordinates).
dp/dt = d(R1 * v1)/dt + d(R2 * v2)/dt = d R1/dt * v1 + d R2/dt * v2, and dR/dt = wx * R, where wx is
the cross product matrix associated with the angular velocity w
so dp/dt = w1x * R1 * v1 + w2x * R2 * v2, and w2 = w1 + wRel
= [-(R1*v1 + R2*v2)x -(R2*v1)x ] [w1 wRel]', so the first matrix is the Jacobian.
The first entry is the cross product matrix of the vector (in 'global' coordinates) from the
origin of link 1 to p, and the second entry is the vector (in 'global' coordinates) from
the origin of link 2 to p (and therein lies the general way of writing this).
*/
void VirtualModelController::computeJointTorquesEquivalentToForce(Joint* start, const Point3d& pLocal, const Vector3d& fGlobal, Joint* end){
//starting from the start joint, going towards the end joint, get the origin of each link, in world coordinates,
//and compute the vector to the global coordinates of pLocal.
Joint* currentJoint = start;
Vector3d tmpV;
Point3d pGlobal = start->getChild()->getWorldCoordinates(pLocal);
while (currentJoint != end){
if (currentJoint == NULL)
throwError("VirtualModelController::computeJointTorquesEquivalentToForce --> end was not a parent of start...");
tmpV = Vector3d(currentJoint->getParent()->getWorldCoordinates(currentJoint->getParentJointPosition()), pGlobal);
Vector3d tmpT = tmpV.crossProductWith(fGlobal);
torques[currentJoint->getID()] -= tmpT;
currentJoint = currentJoint->getParent()->getParentJoint();
}
//and we just have to do it once more for the end joint, if it's not NULL
if (end != NULL){
tmpV = Vector3d(currentJoint->getParent()->getWorldCoordinates(currentJoint->getParentJointPosition()), pGlobal);
torques[currentJoint->getID()] -= tmpV.crossProductWith(fGlobal);
}
}
/**
Assume that the current quaternion represents the relative orientation between two coordinate frames A and B.
This method decomposes the current relative rotation into a twist of frame B around the axis v passed in as a
parameter, and another more arbitrary rotation.
AqB = AqT * TqB, where T is a frame that is obtained by rotating frame B around the axis v by the angle
that is returned by this function.
In the T coordinate frame, v is the same as in B, and AqT is a rotation that aligns v from A to that
from T.
It is assumed that vB is a unit vector!! This method returns TqB, which represents a twist about
the axis vB.
*/
Quaternion Quaternion::decomposeRotation(const Vector3d vB) const{
//we need to compute v in A's coordinates
Vector3d vA = this->rotate(vB);
vA.toUnit();
double temp = 0;
//compute the rotation that aligns the vector v in the two coordinate frames (A and T)
Vector3d rotAxis = vA.crossProductWith(vB);
rotAxis.toUnit();
double rotAngle = -safeACOS(vA.dotProductWith(vB));
Quaternion TqA = Quaternion::getRotationQuaternion(rotAngle, rotAxis*(-1));
return TqA * (*this);
}
/**
This method is used to rotate the vector that is passed in as a parameter by the current quaternion (which is assumed to be a
unit quaternion).
*/
Vector3d Quaternion::inverseRotate(const Vector3d& u) const{
//uRot = q * (0, u) * q' = (s, -v) * (0, u) * (s, v)
//working it out manually, we get:
Vector3d t = u * s + u.crossProductWith(v);
return v*u.dotProductWith(v) + t * s + t.crossProductWith(v);
}