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; }