Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState &state,
                                               const robot_model::JointModelGroup *joint_model_group,
                                               double &manipulability_index,
                                               bool translation) const
{
  // state.getJacobian() only works for chain groups.
  if(!joint_model_group->isChain())
  {
    return false;
  }

  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
  // Get joint limits penalty
  double penalty = getJointLimitsPenalty(state, joint_model_group);
  if (translation)
  {
    Eigen::MatrixXd jacobian_2 = jacobian.topLeftCorner(3,jacobian.cols());
    Eigen::MatrixXd matrix = jacobian_2*jacobian_2.transpose();
    // Get manipulability index
    manipulability_index = penalty * sqrt(matrix.determinant());
  }
  else
  {
    Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
    // Get manipulability index
    manipulability_index = penalty * sqrt(matrix.determinant());
  }
  return true;
}
Exemplo n.º 3
0
bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState &state,
                                                   const robot_model::JointModelGroup *joint_model_group,
                                                   Eigen::MatrixXcd &eigen_values,
                                                   Eigen::MatrixXcd &eigen_vectors) const
{
  // state.getJacobian() only works for chain groups.
  if(!joint_model_group->isChain())
  {
    return false;
  }

  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
  Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
  Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(matrix.block(0, 0, 3, 3));
  eigen_values = eigensolver.eigenvalues();
  eigen_vectors = eigensolver.eigenvectors();
  return true;
}