kinematics_metrics.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta */
00036 
00037 #include <moveit/kinematics_metrics/kinematics_metrics.h>
00038 #include <Eigen/Eigenvalues>
00039 #include <boost/math/constants/constants.hpp>
00040 
00041 namespace kinematics_metrics
00042 {
00043 double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& state,
00044                                                 const robot_model::JointModelGroup* joint_model_group) const
00045 {
00046   if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
00047     return 1.0;
00048   double joint_limits_multiplier(1.0);
00049   const std::vector<const robot_model::JointModel*>& joint_model_vector = joint_model_group->getJointModels();
00050   for (std::size_t i = 0; i < joint_model_vector.size(); ++i)
00051   {
00052     if (joint_model_vector[i]->getType() == robot_model::JointModel::REVOLUTE)
00053     {
00054       const robot_model::RevoluteJointModel* revolute_model =
00055           static_cast<const robot_model::RevoluteJointModel*>(joint_model_vector[i]);
00056       if (revolute_model->isContinuous())
00057         continue;
00058     }
00059     if (joint_model_vector[i]->getType() == robot_model::JointModel::PLANAR)
00060     {
00061       const robot_model::JointModel::Bounds& bounds = joint_model_vector[i]->getVariableBounds();
00062       if (bounds[0].min_position_ == -std::numeric_limits<double>::max() ||
00063           bounds[0].max_position_ == std::numeric_limits<double>::max() ||
00064           bounds[1].min_position_ == -std::numeric_limits<double>::max() ||
00065           bounds[1].max_position_ == std::numeric_limits<double>::max() ||
00066           bounds[2].min_position_ == -boost::math::constants::pi<double>() ||
00067           bounds[2].max_position_ == boost::math::constants::pi<double>())
00068         continue;
00069     }
00070     if (joint_model_vector[i]->getType() == robot_model::JointModel::FLOATING)
00071     {
00072       // Joint limits are not well-defined for floating joints
00073       continue;
00074     }
00075     const double* joint_values = state.getJointPositions(joint_model_vector[i]);
00076     const robot_model::JointModel::Bounds& bounds = joint_model_vector[i]->getVariableBounds();
00077     std::vector<double> lower_bounds, upper_bounds;
00078     for (std::size_t j = 0; j < bounds.size(); ++j)
00079     {
00080       lower_bounds.push_back(bounds[j].min_position_);
00081       upper_bounds.push_back(bounds[j].max_position_);
00082     }
00083     double lower_bound_distance = joint_model_vector[i]->distance(joint_values, &lower_bounds[0]);
00084     double upper_bound_distance = joint_model_vector[i]->distance(joint_values, &upper_bounds[0]);
00085     double range = lower_bound_distance + upper_bound_distance;
00086     if (range <= boost::math::tools::epsilon<double>())
00087       continue;
00088     joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance / (range * range));
00089   }
00090   return (1.0 - exp(-penalty_multiplier_ * joint_limits_multiplier));
00091 }
00092 
00093 bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state, const std::string& group_name,
00094                                                double& manipulability_index, bool translation) const
00095 {
00096   const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
00097   if (joint_model_group)
00098     return getManipulabilityIndex(state, joint_model_group, manipulability_index, translation);
00099   else
00100     return false;
00101 }
00102 
00103 bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state,
00104                                                const robot_model::JointModelGroup* joint_model_group,
00105                                                double& manipulability_index, bool translation) const
00106 {
00107   // state.getJacobian() only works for chain groups.
00108   if (!joint_model_group->isChain())
00109   {
00110     return false;
00111   }
00112 
00113   Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
00114   // Get joint limits penalty
00115   double penalty = getJointLimitsPenalty(state, joint_model_group);
00116   if (translation)
00117   {
00118     if (jacobian.cols() < 6)
00119     {
00120       Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3, jacobian.cols()));
00121       Eigen::MatrixXd singular_values = svdsolver.singularValues();
00122       manipulability_index = 1.0;
00123       for (unsigned int i = 0; i < singular_values.rows(); ++i)
00124       {
00125         logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
00126         manipulability_index *= singular_values(i, 0);
00127       }
00128       // Get manipulability index
00129       manipulability_index = penalty * manipulability_index;
00130     }
00131     else
00132     {
00133       Eigen::MatrixXd jacobian_2 = jacobian.topLeftCorner(3, jacobian.cols());
00134       Eigen::MatrixXd matrix = jacobian_2 * jacobian_2.transpose();
00135       // Get manipulability index
00136       manipulability_index = penalty * sqrt(matrix.determinant());
00137     }
00138   }
00139   else
00140   {
00141     if (jacobian.cols() < 6)
00142     {
00143       Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
00144       Eigen::MatrixXd singular_values = svdsolver.singularValues();
00145       manipulability_index = 1.0;
00146       for (unsigned int i = 0; i < singular_values.rows(); ++i)
00147       {
00148         logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
00149         manipulability_index *= singular_values(i, 0);
00150       }
00151       // Get manipulability index
00152       manipulability_index = penalty * manipulability_index;
00153     }
00154     else
00155     {
00156       Eigen::MatrixXd matrix = jacobian * jacobian.transpose();
00157       // Get manipulability index
00158       manipulability_index = penalty * sqrt(matrix.determinant());
00159     }
00160   }
00161   return true;
00162 }
00163 
00164 bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state, const std::string& group_name,
00165                                                    Eigen::MatrixXcd& eigen_values,
00166                                                    Eigen::MatrixXcd& eigen_vectors) const
00167 {
00168   const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
00169   if (joint_model_group)
00170     return getManipulabilityEllipsoid(state, joint_model_group, eigen_values, eigen_vectors);
00171   else
00172     return false;
00173 }
00174 
00175 bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state,
00176                                                    const robot_model::JointModelGroup* joint_model_group,
00177                                                    Eigen::MatrixXcd& eigen_values,
00178                                                    Eigen::MatrixXcd& eigen_vectors) const
00179 {
00180   // state.getJacobian() only works for chain groups.
00181   if (!joint_model_group->isChain())
00182   {
00183     return false;
00184   }
00185 
00186   Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
00187   Eigen::MatrixXd matrix = jacobian * jacobian.transpose();
00188   Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(matrix.block(0, 0, 3, 3));
00189   eigen_values = eigensolver.eigenvalues();
00190   eigen_vectors = eigensolver.eigenvectors();
00191   return true;
00192 }
00193 
00194 bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, const std::string& group_name,
00195                                           double& manipulability, bool translation) const
00196 {
00197   const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
00198   if (joint_model_group)
00199     return getManipulability(state, joint_model_group, manipulability, translation);
00200   else
00201     return false;
00202 }
00203 
00204 bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state,
00205                                           const robot_model::JointModelGroup* joint_model_group, double& manipulability,
00206                                           bool translation) const
00207 {
00208   // state.getJacobian() only works for chain groups.
00209   if (!joint_model_group->isChain())
00210   {
00211     return false;
00212   }
00213   // Get joint limits penalty
00214   double penalty = getJointLimitsPenalty(state, joint_model_group);
00215   if (translation)
00216   {
00217     Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
00218     Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3, jacobian.cols()));
00219     Eigen::MatrixXd singular_values = svdsolver.singularValues();
00220     for (int i = 0; i < singular_values.rows(); ++i)
00221       logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
00222     manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
00223   }
00224   else
00225   {
00226     Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
00227     Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
00228     Eigen::MatrixXd singular_values = svdsolver.singularValues();
00229     for (int i = 0; i < singular_values.rows(); ++i)
00230       logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
00231     manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
00232   }
00233   return true;
00234 }
00235 
00236 }  // namespace


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49