robManipulator::Errno mtsIntuitiveResearchKitECM::InverseKinematics(vctDoubleVec & jointSet,
const vctFrm4x4 & cartesianGoal)
{
// re-align desired frame to 4 axis direction to reduce free space
vctDouble3 shaft = cartesianGoal.Translation();
shaft.NormalizedSelf();
const vctDouble3 z = cartesianGoal.Rotation().Column(2).Ref<3>(); // last column of rotation matrix
vctDouble3 axis;
axis.CrossProductOf(z, shaft);
const double angle = acos(vctDotProduct(z, shaft));
const vctMatRot3 reAlign(vctAxAnRot3(axis, angle, VCT_NORMALIZE));
vctFrm4x4 newGoal;
newGoal.Translation().Assign(cartesianGoal.Translation());
newGoal.Rotation().ProductOf(reAlign, cartesianGoal.Rotation());
if (Manipulator.InverseKinematics(jointSet, newGoal) == robManipulator::ESUCCESS) {
// find closest solution mod 2 pi
const double difference = JointGet[3] - jointSet[3];
const double differenceInTurns = nearbyint(difference / (2.0 * cmnPI));
jointSet[3] = jointSet[3] + differenceInTurns * 2.0 * cmnPI;
// make sure we are away from RCM point, this test is
// simplistic
if (jointSet[2] < 40.0 * cmn_mm) {
jointSet[2] = 40.0 * cmn_mm;
}
#if 0
vctFrm4x4 forward = Manipulator.ForwardKinematics(jointSet);
vctDouble3 diff;
diff.DifferenceOf(forward.Translation(), newGoal.Translation());
std::cerr << cmnInternalTo_mm(diff.Norm()) << "mm ";
#endif
return robManipulator::ESUCCESS;
}
return robManipulator::EFAILURE;
}
