Eigen::MatrixXf CoMIK::getJacobianOfCoM(RobotNodePtr node)
{
// Get number of degrees of freedom
size_t nDoF = rns->getAllRobotNodes().size();
// Create matrices for the position and the orientation part of the jacobian.
Eigen::MatrixXf position = Eigen::MatrixXf::Zero(3, nDoF);
const std::vector<RobotNodePtr> parentsN = bodyNodeParents[node];
// Iterate over all degrees of freedom
for (size_t i = 0; i < nDoF; i++)
{
RobotNodePtr dof = rns->getNode(i);// bodyNodes[i];
// Check if the tcp is affected by this DOF
if (find(parentsN.begin(), parentsN.end(), dof) != parentsN.end())
{
// Calculus for rotational joints is different as for prismatic joints.
if (dof->isRotationalJoint())
{
// get axis
boost::shared_ptr<RobotNodeRevolute> revolute
= boost::dynamic_pointer_cast<RobotNodeRevolute>(dof);
THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
// todo: find a better way of handling different joint types
Eigen::Vector3f axis = revolute->getJointRotationAxis(coordSystem);
// For CoM-Jacobians only the positional part is necessary
Eigen::Vector3f toTCP = node->getCoMLocal() + node->getGlobalPose().block(0, 3, 3, 1)
- dof->getGlobalPose().block(0, 3, 3, 1);
position.block(0, i, 3, 1) = axis.cross(toTCP);
}
else if (dof->isTranslationalJoint())
{
// -> prismatic joint
boost::shared_ptr<RobotNodePrismatic> prismatic
= boost::dynamic_pointer_cast<RobotNodePrismatic>(dof);
THROW_VR_EXCEPTION_IF(!prismatic, "Internal error: expecting prismatic joint");
// todo: find a better way of handling different joint types
Eigen::Vector3f axis = prismatic->getJointTranslationDirection(coordSystem);
position.block(0, i, 3, 1) = axis;
}
}
}
if (target.rows() == 2)
{
Eigen::MatrixXf result(2, nDoF);
result.row(0) = position.row(0);
result.row(1) = position.row(1);
return result;
}
else if (target.rows() == 1)
{
VR_INFO << "One dimensional CoMs not supported." << endl;
}
return position;
}
