kinematics_metrics.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
38 #include <Eigen/Dense>
39 #include <Eigen/Eigenvalues>
40 #include <boost/math/constants/constants.hpp>
41 
42 namespace kinematics_metrics
43 {
45  const robot_model::JointModelGroup* joint_model_group) const
46 {
47  if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
48  return 1.0;
49  double joint_limits_multiplier(1.0);
50  const std::vector<const robot_model::JointModel*>& joint_model_vector = joint_model_group->getJointModels();
51  for (std::size_t i = 0; i < joint_model_vector.size(); ++i)
52  {
53  if (joint_model_vector[i]->getType() == robot_model::JointModel::REVOLUTE)
54  {
55  const robot_model::RevoluteJointModel* revolute_model =
56  static_cast<const robot_model::RevoluteJointModel*>(joint_model_vector[i]);
57  if (revolute_model->isContinuous())
58  continue;
59  }
60  if (joint_model_vector[i]->getType() == robot_model::JointModel::PLANAR)
61  {
62  const robot_model::JointModel::Bounds& bounds = joint_model_vector[i]->getVariableBounds();
63  if (bounds[0].min_position_ == -std::numeric_limits<double>::max() ||
64  bounds[0].max_position_ == std::numeric_limits<double>::max() ||
65  bounds[1].min_position_ == -std::numeric_limits<double>::max() ||
66  bounds[1].max_position_ == std::numeric_limits<double>::max() ||
67  bounds[2].min_position_ == -boost::math::constants::pi<double>() ||
68  bounds[2].max_position_ == boost::math::constants::pi<double>())
69  continue;
70  }
71  if (joint_model_vector[i]->getType() == robot_model::JointModel::FLOATING)
72  {
73  // Joint limits are not well-defined for floating joints
74  continue;
75  }
76  const double* joint_values = state.getJointPositions(joint_model_vector[i]);
77  const robot_model::JointModel::Bounds& bounds = joint_model_vector[i]->getVariableBounds();
78  std::vector<double> lower_bounds, upper_bounds;
79  for (std::size_t j = 0; j < bounds.size(); ++j)
80  {
81  lower_bounds.push_back(bounds[j].min_position_);
82  upper_bounds.push_back(bounds[j].max_position_);
83  }
84  double lower_bound_distance = joint_model_vector[i]->distance(joint_values, &lower_bounds[0]);
85  double upper_bound_distance = joint_model_vector[i]->distance(joint_values, &upper_bounds[0]);
86  double range = lower_bound_distance + upper_bound_distance;
87  if (range <= boost::math::tools::epsilon<double>())
88  continue;
89  joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance / (range * range));
90  }
91  return (1.0 - exp(-penalty_multiplier_ * joint_limits_multiplier));
92 }
93 
94 bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state, const std::string& group_name,
95  double& manipulability_index, bool translation) const
96 {
97  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
98  if (joint_model_group)
99  return getManipulabilityIndex(state, joint_model_group, manipulability_index, translation);
100  else
101  return false;
102 }
103 
105  const robot_model::JointModelGroup* joint_model_group,
106  double& manipulability_index, bool translation) const
107 {
108  // state.getJacobian() only works for chain groups.
109  if (!joint_model_group->isChain())
110  {
111  return false;
112  }
113 
114  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
115  // Get joint limits penalty
116  double penalty = getJointLimitsPenalty(state, joint_model_group);
117  if (translation)
118  {
119  if (jacobian.cols() < 6)
120  {
121  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3, jacobian.cols()));
122  Eigen::MatrixXd singular_values = svdsolver.singularValues();
123  manipulability_index = 1.0;
124  for (unsigned int i = 0; i < singular_values.rows(); ++i)
125  {
126  ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
127  manipulability_index *= singular_values(i, 0);
128  }
129  // Get manipulability index
130  manipulability_index = penalty * manipulability_index;
131  }
132  else
133  {
134  Eigen::MatrixXd jacobian_2 = jacobian.topLeftCorner(3, jacobian.cols());
135  Eigen::MatrixXd matrix = jacobian_2 * jacobian_2.transpose();
136  // Get manipulability index
137  manipulability_index = penalty * sqrt(matrix.determinant());
138  }
139  }
140  else
141  {
142  if (jacobian.cols() < 6)
143  {
144  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
145  Eigen::MatrixXd singular_values = svdsolver.singularValues();
146  manipulability_index = 1.0;
147  for (unsigned int i = 0; i < singular_values.rows(); ++i)
148  {
149  ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
150  manipulability_index *= singular_values(i, 0);
151  }
152  // Get manipulability index
153  manipulability_index = penalty * manipulability_index;
154  }
155  else
156  {
157  Eigen::MatrixXd matrix = jacobian * jacobian.transpose();
158  // Get manipulability index
159  manipulability_index = penalty * sqrt(matrix.determinant());
160  }
161  }
162  return true;
163 }
164 
165 bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state, const std::string& group_name,
166  Eigen::MatrixXcd& eigen_values,
167  Eigen::MatrixXcd& eigen_vectors) const
168 {
169  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
170  if (joint_model_group)
171  return getManipulabilityEllipsoid(state, joint_model_group, eigen_values, eigen_vectors);
172  else
173  return false;
174 }
175 
177  const robot_model::JointModelGroup* joint_model_group,
178  Eigen::MatrixXcd& eigen_values,
179  Eigen::MatrixXcd& eigen_vectors) const
180 {
181  // state.getJacobian() only works for chain groups.
182  if (!joint_model_group->isChain())
183  {
184  return false;
185  }
186 
187  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
188  Eigen::MatrixXd matrix = jacobian * jacobian.transpose();
189  Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(matrix.block(0, 0, 3, 3));
190  eigen_values = eigensolver.eigenvalues();
191  eigen_vectors = eigensolver.eigenvectors();
192  return true;
193 }
194 
195 bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, const std::string& group_name,
196  double& manipulability, bool translation) const
197 {
198  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
199  if (joint_model_group)
200  return getManipulability(state, joint_model_group, manipulability, translation);
201  else
202  return false;
203 }
204 
206  const robot_model::JointModelGroup* joint_model_group, double& manipulability,
207  bool translation) const
208 {
209  // state.getJacobian() only works for chain groups.
210  if (!joint_model_group->isChain())
211  {
212  return false;
213  }
214  // Get joint limits penalty
215  double penalty = getJointLimitsPenalty(state, joint_model_group);
216  if (translation)
217  {
218  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
219  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3, jacobian.cols()));
220  Eigen::MatrixXd singular_values = svdsolver.singularValues();
221  for (int i = 0; i < singular_values.rows(); ++i)
222  ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
223  manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
224  }
225  else
226  {
227  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
228  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
229  Eigen::MatrixXd singular_values = svdsolver.singularValues();
230  for (int i = 0; i < singular_values.rows(); ++i)
231  ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
232  manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
233  }
234  return true;
235 }
236 
237 } // end of namespace kinematics_metrics
bool isChain() const
Check if this group is a linear chain.
double getJointLimitsPenalty(const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group) const
Defines a multiplier for the manipulabilty = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distan...
bool isContinuous() const
Check if this joint wraps around.
bool getManipulabilityEllipsoid(const robot_state::RobotState &state, const std::string &group_name, Eigen::MatrixXcd &eigen_values, Eigen::MatrixXcd &eigen_vectors) const
Get the (translation) manipulability ellipsoid for a given group at a given joint configuration...
#define ROS_DEBUG_NAMED(name,...)
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group...
bool getManipulability(const robot_state::RobotState &state, const std::string &group_name, double &condition_number, bool translation=false) const
Get the manipulability = sigma_min/sigma_max where sigma_min and sigma_max are the smallest and large...
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
robot_model::RobotModelConstPtr robot_model_
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
bool getManipulabilityIndex(const robot_state::RobotState &state, const std::string &group_name, double &manipulability_index, bool translation=false) const
Get the manipulability for a given group at a given joint configuration.
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:569
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:123
Namespace for kinematics metrics.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33