bool KinematicsMetrics::getManipulability(const robot_state::RobotState &state,
const robot_model::JointModelGroup *joint_model_group,
double &manipulability,
bool translation) const
{
// state.getJacobian() only works for chain groups.
if(!joint_model_group->isChain())
{
return false;
}
// Get joint limits penalty
double penalty = getJointLimitsPenalty(state, joint_model_group);
if (translation)
{
Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3,jacobian.cols()));
Eigen::MatrixXd singular_values = svdsolver.singularValues();
for (int i = 0; i < singular_values.rows(); ++i)
logDebug("moveit.kin_metrics: Singular value: %d %f",i,singular_values(i,0));
manipulability = penalty * singular_values.minCoeff()/singular_values.maxCoeff();
}
else
{
Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
Eigen::MatrixXd singular_values = svdsolver.singularValues();
for(int i=0; i < singular_values.rows(); ++i)
logDebug("moveit.kin_metrics: Singular value: %d %f",i,singular_values(i,0));
manipulability = penalty * singular_values.minCoeff()/singular_values.maxCoeff();
}
return true;
}
double TRAC_IK::ManipValue2(const KDL::JntArray& arr) {
KDL::Jacobian jac(arr.data.size());
jacsolver->JntToJac(arr,jac);
Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
Eigen::MatrixXd singular_values = svdsolver.singularValues();
return singular_values.minCoeff()/singular_values.maxCoeff();
}
double TRAC_IK::ManipValue1(const KDL::JntArray& arr) {
KDL::Jacobian jac(arr.data.size());
jacsolver->JntToJac(arr,jac);
Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
Eigen::MatrixXd singular_values = svdsolver.singularValues();
double error = 1.0;
for(unsigned int i=0; i < singular_values.rows(); ++i)
error *= singular_values(i,0);
return error;
}
