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/Eigenvalues>
39 #include <boost/math/constants/constants.hpp>
40 
41 namespace kinematics_metrics
42 {
44  const robot_model::JointModelGroup* joint_model_group) const
45 {
46  if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
47  return 1.0;
48  double joint_limits_multiplier(1.0);
49  const std::vector<const robot_model::JointModel*>& joint_model_vector = joint_model_group->getJointModels();
50  for (std::size_t i = 0; i < joint_model_vector.size(); ++i)
51  {
52  if (joint_model_vector[i]->getType() == robot_model::JointModel::REVOLUTE)
53  {
54  const robot_model::RevoluteJointModel* revolute_model =
55  static_cast<const robot_model::RevoluteJointModel*>(joint_model_vector[i]);
56  if (revolute_model->isContinuous())
57  continue;
58  }
59  if (joint_model_vector[i]->getType() == robot_model::JointModel::PLANAR)
60  {
61  const robot_model::JointModel::Bounds& bounds = joint_model_vector[i]->getVariableBounds();
62  if (bounds[0].min_position_ == -std::numeric_limits<double>::max() ||
63  bounds[0].max_position_ == std::numeric_limits<double>::max() ||
64  bounds[1].min_position_ == -std::numeric_limits<double>::max() ||
65  bounds[1].max_position_ == std::numeric_limits<double>::max() ||
66  bounds[2].min_position_ == -boost::math::constants::pi<double>() ||
67  bounds[2].max_position_ == boost::math::constants::pi<double>())
68  continue;
69  }
70  if (joint_model_vector[i]->getType() == robot_model::JointModel::FLOATING)
71  {
72  // Joint limits are not well-defined for floating joints
73  continue;
74  }
75  const double* joint_values = state.getJointPositions(joint_model_vector[i]);
76  const robot_model::JointModel::Bounds& bounds = joint_model_vector[i]->getVariableBounds();
77  std::vector<double> lower_bounds, upper_bounds;
78  for (std::size_t j = 0; j < bounds.size(); ++j)
79  {
80  lower_bounds.push_back(bounds[j].min_position_);
81  upper_bounds.push_back(bounds[j].max_position_);
82  }
83  double lower_bound_distance = joint_model_vector[i]->distance(joint_values, &lower_bounds[0]);
84  double upper_bound_distance = joint_model_vector[i]->distance(joint_values, &upper_bounds[0]);
85  double range = lower_bound_distance + upper_bound_distance;
86  if (range <= boost::math::tools::epsilon<double>())
87  continue;
88  joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance / (range * range));
89  }
90  return (1.0 - exp(-penalty_multiplier_ * joint_limits_multiplier));
91 }
92 
93 bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& state, const std::string& group_name,
94  double& manipulability_index, bool translation) const
95 {
96  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
97  if (joint_model_group)
98  return getManipulabilityIndex(state, joint_model_group, manipulability_index, translation);
99  else
100  return false;
101 }
102 
104  const robot_model::JointModelGroup* joint_model_group,
105  double& manipulability_index, bool translation) const
106 {
107  // state.getJacobian() only works for chain groups.
108  if (!joint_model_group->isChain())
109  {
110  return false;
111  }
112 
113  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
114  // Get joint limits penalty
115  double penalty = getJointLimitsPenalty(state, joint_model_group);
116  if (translation)
117  {
118  if (jacobian.cols() < 6)
119  {
120  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3, jacobian.cols()));
121  Eigen::MatrixXd singular_values = svdsolver.singularValues();
122  manipulability_index = 1.0;
123  for (unsigned int i = 0; i < singular_values.rows(); ++i)
124  {
125  logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
126  manipulability_index *= singular_values(i, 0);
127  }
128  // Get manipulability index
129  manipulability_index = penalty * manipulability_index;
130  }
131  else
132  {
133  Eigen::MatrixXd jacobian_2 = jacobian.topLeftCorner(3, jacobian.cols());
134  Eigen::MatrixXd matrix = jacobian_2 * jacobian_2.transpose();
135  // Get manipulability index
136  manipulability_index = penalty * sqrt(matrix.determinant());
137  }
138  }
139  else
140  {
141  if (jacobian.cols() < 6)
142  {
143  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
144  Eigen::MatrixXd singular_values = svdsolver.singularValues();
145  manipulability_index = 1.0;
146  for (unsigned int i = 0; i < singular_values.rows(); ++i)
147  {
148  logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
149  manipulability_index *= singular_values(i, 0);
150  }
151  // Get manipulability index
152  manipulability_index = penalty * manipulability_index;
153  }
154  else
155  {
156  Eigen::MatrixXd matrix = jacobian * jacobian.transpose();
157  // Get manipulability index
158  manipulability_index = penalty * sqrt(matrix.determinant());
159  }
160  }
161  return true;
162 }
163 
164 bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState& state, const std::string& group_name,
165  Eigen::MatrixXcd& eigen_values,
166  Eigen::MatrixXcd& eigen_vectors) const
167 {
168  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
169  if (joint_model_group)
170  return getManipulabilityEllipsoid(state, joint_model_group, eigen_values, eigen_vectors);
171  else
172  return false;
173 }
174 
176  const robot_model::JointModelGroup* joint_model_group,
177  Eigen::MatrixXcd& eigen_values,
178  Eigen::MatrixXcd& eigen_vectors) const
179 {
180  // state.getJacobian() only works for chain groups.
181  if (!joint_model_group->isChain())
182  {
183  return false;
184  }
185 
186  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
187  Eigen::MatrixXd matrix = jacobian * jacobian.transpose();
188  Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(matrix.block(0, 0, 3, 3));
189  eigen_values = eigensolver.eigenvalues();
190  eigen_vectors = eigensolver.eigenvectors();
191  return true;
192 }
193 
194 bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, const std::string& group_name,
195  double& manipulability, bool translation) const
196 {
197  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
198  if (joint_model_group)
199  return getManipulability(state, joint_model_group, manipulability, translation);
200  else
201  return false;
202 }
203 
205  const robot_model::JointModelGroup* joint_model_group, double& manipulability,
206  bool translation) const
207 {
208  // state.getJacobian() only works for chain groups.
209  if (!joint_model_group->isChain())
210  {
211  return false;
212  }
213  // Get joint limits penalty
214  double penalty = getJointLimitsPenalty(state, joint_model_group);
215  if (translation)
216  {
217  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
218  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3, jacobian.cols()));
219  Eigen::MatrixXd singular_values = svdsolver.singularValues();
220  for (int i = 0; i < singular_values.rows(); ++i)
221  logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
222  manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
223  }
224  else
225  {
226  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
227  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
228  Eigen::MatrixXd singular_values = svdsolver.singularValues();
229  for (int i = 0; i < singular_values.rows(); ++i)
230  logDebug("moveit.kin_metrics: Singular value: %d %f", i, singular_values(i, 0));
231  manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
232  }
233  return true;
234 }
235 
236 } // namespace
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...
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:82
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:535
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 Jan 21 2018 03:54:29