/**
This method is used to compute the desired orientation and angular velocity for a parent RB and a child RB, relative to the grandparent RB and
parent RB repsectively. The input is:
- the index of the joint that connects the grandparent RB to the parent RB, and the index of the joint between parent and child
- the distance from the parent's joint with its parent to the location of the child's joint, expressed in parent coordinates
- two rotation normals - one that specifies the plane of rotation for the parent's joint, expressed in grandparent coords,
and the other specifies the plane of rotation between the parent and the child, expressed in parent's coordinates.
- The position of the end effector, expressed in child's coordinate frame
- The desired position of the end effector, expressed in world coordinates
- an estimate of the desired position of the end effector, in world coordinates, some dt later - used to compute desired angular velocities
*/
void IKVMCController::computeIKQandW(int parentJIndex, int childJIndex, const Vector3d& parentAxis, const Vector3d& parentNormal, const Vector3d& childNormal, const Vector3d& childEndEffector, const Point3d& wP, bool computeAngVelocities, const Point3d& futureWP, double dt) {
//this is the joint between the grandparent RB and the parent
Joint* parentJoint = character->joints[parentJIndex];
//this is the grandparent - most calculations will be done in its coordinate frame
ArticulatedRigidBody* gParent = parentJoint->getParent();
//this is the reduced character space where we will be setting the desired orientations and ang vels.
ReducedCharacterState rs(&desiredPose);
//the desired relative orientation between parent and grandparent
Quaternion qParent;
//and the desired relative orientation between child and parent
Quaternion qChild;
TwoLinkIK::getIKOrientations(parentJoint->getParentJointPosition(), gParent->getLocalCoordinates(wP), parentNormal, parentAxis, childNormal, childEndEffector, &qParent, &qChild);
controlParams[parentJIndex].relToFrame = false;
controlParams[childJIndex].relToFrame = false;
rs.setJointRelativeOrientation(qChild, childJIndex);
rs.setJointRelativeOrientation(qParent, parentJIndex);
Vector3d wParentD(0, 0, 0);
Vector3d wChildD(0, 0, 0);
if (computeAngVelocities) {
//the joint's origin will also move, so take that into account, by subbing the offset by which it moves to the
//futureTarget (to get the same relative position to the hip)
Vector3d velOffset = gParent->getAbsoluteVelocityForLocalPoint(parentJoint->getParentJointPosition());
Quaternion qParentF;
Quaternion qChildF;
TwoLinkIK::getIKOrientations(parentJoint->getParentJointPosition(), gParent->getLocalCoordinates(futureWP + velOffset * -dt), parentNormal, parentAxis, childNormal, childEndEffector, &qParentF, &qChildF);
Quaternion qDiff = qParentF * qParent.getComplexConjugate();
wParentD = qDiff.v * 2 / dt;
//the above is the desired angular velocity, were the parent not rotating already - but it usually is, so we'll account for that
wParentD -= gParent->getLocalCoordinates(gParent->getAngularVelocity());
qDiff = qChildF * qChild.getComplexConjugate();
wChildD = qDiff.v * 2 / dt;
//make sure we don't go overboard with the estimates, in case there are discontinuities in the trajectories...
boundToRange(&wChildD.x, -5, 5);
boundToRange(&wChildD.y, -5, 5);
boundToRange(&wChildD.z, -5, 5);
boundToRange(&wParentD.x, -5, 5);
boundToRange(&wParentD.y, -5, 5);
boundToRange(&wParentD.z, -5, 5);
}
rs.setJointRelativeAngVelocity(wChildD, childJIndex);
rs.setJointRelativeAngVelocity(wParentD, parentJIndex);
}
/**
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);
}
}
