cartesian_interpolator.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ioan A. Sucan
5  * Copyright (c) 2013, Willow Garage, Inc.
6  * Copyright (c) 2019, PickNik LLC.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the Willow Garage nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */
38 
39 #include <memory>
42 
43 namespace moveit
44 {
45 namespace core
46 {
51 static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10;
52 
53 const std::string LOGNAME = "cartesian_interpolator";
54 
55 double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
56  std::vector<RobotStatePtr>& traj, const LinkModel* link,
57  const Eigen::Vector3d& translation, bool global_reference_frame,
58  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
59  const GroupStateValidityCallbackFn& validCallback,
61 {
62  const double distance = translation.norm();
63  // The target pose is obtained by adding the translation vector to the link's current pose
64  Eigen::Isometry3d pose = start_state->getGlobalLinkTransform(link);
65 
66  // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame)
67  pose.translation() += global_reference_frame ? translation : pose.linear() * translation;
68 
69  // call computeCartesianPath for the computed target pose in the global reference frame
70  return distance * computeCartesianPath(start_state, group, traj, link, pose, true, max_step, jump_threshold,
71  validCallback, options);
72 }
73 
74 double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
75  std::vector<RobotStatePtr>& traj, const LinkModel* link,
76  const Eigen::Isometry3d& target, bool global_reference_frame,
77  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
78  const GroupStateValidityCallbackFn& validCallback,
80  const Eigen::Isometry3d& link_offset)
81 {
82  // check unsanitized inputs for non-isometry
83  ASSERT_ISOMETRY(target)
84  ASSERT_ISOMETRY(link_offset)
85 
86  const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
87  // make sure that continuous joints wrap
88  for (const JointModel* joint : cjnt)
89  start_state->enforceBounds(joint);
90 
91  // Cartesian pose we start from
92  Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link) * link_offset;
93  Eigen::Isometry3d offset = link_offset.inverse();
94 
95  // the target can be in the local reference frame (in which case we rotate it)
96  Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target;
97 
98  Eigen::Quaterniond start_quaternion(start_pose.linear());
99  Eigen::Quaterniond target_quaternion(rotated_target.linear());
100 
101  if (max_step.translation <= 0.0 && max_step.rotation <= 0.0)
102  {
104  "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and "
105  "MaxEEFStep.translation components must be non-negative and at least one component must be "
106  "greater than zero");
107  return 0.0;
108  }
109 
110  double rotation_distance = start_quaternion.angularDistance(target_quaternion);
111  double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();
112 
113  // decide how many steps we will need for this trajectory
114  std::size_t translation_steps = 0;
115  if (max_step.translation > 0.0)
116  translation_steps = floor(translation_distance / max_step.translation);
117 
118  std::size_t rotation_steps = 0;
119  if (max_step.rotation > 0.0)
120  rotation_steps = floor(rotation_distance / max_step.rotation);
121 
122  // If we are testing for relative jumps, we always want at least MIN_STEPS_FOR_JUMP_THRESH steps
123  std::size_t steps = std::max(translation_steps, rotation_steps) + 1;
124  if (jump_threshold.factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH)
126 
127  // To limit absolute joint-space jumps, we pass consistency limits to the IK solver
128  std::vector<double> consistency_limits;
129  if (jump_threshold.prismatic > 0 || jump_threshold.revolute > 0)
130  for (const JointModel* jm : group->getActiveJointModels())
131  {
132  double limit;
133  switch (jm->getType())
134  {
136  limit = jump_threshold.revolute;
137  break;
139  limit = jump_threshold.prismatic;
140  break;
141  default:
142  limit = 0.0;
143  }
144  if (limit == 0.0)
145  limit = jm->getMaximumExtent();
146  consistency_limits.push_back(limit);
147  }
148 
149  traj.clear();
150  traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
151 
152  double last_valid_percentage = 0.0;
153  for (std::size_t i = 1; i <= steps; ++i)
154  {
155  double percentage = (double)i / (double)steps;
156 
157  Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion));
158  pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
159 
160  // Explicitly use a single IK attempt only: We want a smooth trajectory.
161  // Random seeding (of additional attempts) would probably create IK jumps.
162  if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options))
163  traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
164  else
165  break;
166 
167  last_valid_percentage = percentage;
168  }
169 
170  last_valid_percentage *= checkJointSpaceJump(group, traj, jump_threshold);
171 
172  return last_valid_percentage;
173 }
174 
176  RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
177  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, const MaxEEFStep& max_step,
178  const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
179  const kinematics::KinematicsQueryOptions& options, const Eigen::Isometry3d& link_offset)
180 {
181  double percentage_solved = 0.0;
182  for (std::size_t i = 0; i < waypoints.size(); ++i)
183  {
184  // Don't test joint space jumps for every waypoint, test them later on the whole trajectory.
185  static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST;
186  std::vector<RobotStatePtr> waypoint_traj;
187  double wp_percentage_solved =
188  computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
189  NO_JOINT_SPACE_JUMP_TEST, validCallback, options, link_offset);
190  if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
191  {
192  percentage_solved = (double)(i + 1) / (double)waypoints.size();
193  std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
194  if (i > 0 && !waypoint_traj.empty())
195  std::advance(start, 1);
196  traj.insert(traj.end(), start, waypoint_traj.end());
197  }
198  else
199  {
200  percentage_solved += wp_percentage_solved / (double)waypoints.size();
201  std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
202  if (i > 0 && !waypoint_traj.empty())
203  std::advance(start, 1);
204  traj.insert(traj.end(), start, waypoint_traj.end());
205  break;
206  }
207  }
208 
209  percentage_solved *= checkJointSpaceJump(group, traj, jump_threshold);
210 
211  return percentage_solved;
212 }
213 
214 double CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
215  const JumpThreshold& jump_threshold)
216 {
217  double percentage_solved = 1.0;
218  if (traj.size() <= 1)
219  return percentage_solved;
220 
221  if (jump_threshold.factor > 0.0)
222  percentage_solved *= checkRelativeJointSpaceJump(group, traj, jump_threshold.factor);
223 
224  if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0)
225  percentage_solved *= checkAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic);
226 
227  return percentage_solved;
228 }
229 
230 double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* group,
231  std::vector<RobotStatePtr>& traj,
232  double jump_threshold_factor)
233 {
234  if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH)
235  {
237  "The computed trajectory is too short to detect jumps in joint-space "
238  "Need at least %zu steps, only got %zu. Try a lower max_step.",
239  MIN_STEPS_FOR_JUMP_THRESH, traj.size());
240  }
241 
242  std::vector<double> dist_vector;
243  dist_vector.reserve(traj.size() - 1);
244  double total_dist = 0.0;
245  for (std::size_t i = 1; i < traj.size(); ++i)
246  {
247  double dist_prev_point = traj[i]->distance(*traj[i - 1], group);
248  dist_vector.push_back(dist_prev_point);
249  total_dist += dist_prev_point;
250  }
251 
252  double percentage = 1.0;
253  // compute the average distance between the states we looked at
254  double thres = jump_threshold_factor * (total_dist / (double)dist_vector.size());
255  for (std::size_t i = 0; i < dist_vector.size(); ++i)
256  if (dist_vector[i] > thres)
257  {
258  ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump in joint-space distance");
259  percentage = (double)(i + 1) / (double)(traj.size());
260  traj.resize(i + 1);
261  break;
262  }
263 
264  return percentage;
265 }
266 
267 double CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* group,
268  std::vector<RobotStatePtr>& traj, double revolute_threshold,
269  double prismatic_threshold)
270 {
271  bool check_revolute = revolute_threshold > 0.0;
272  bool check_prismatic = prismatic_threshold > 0.0;
273 
274  double joint_threshold;
275  bool check_joint;
276  bool still_valid = true;
277  const std::vector<const JointModel*>& joints = group->getActiveJointModels();
278  for (std::size_t traj_ix = 0, ix_end = traj.size() - 1; traj_ix != ix_end; ++traj_ix)
279  {
280  for (auto& joint : joints)
281  {
282  switch (joint->getType())
283  {
285  check_joint = check_revolute;
286  joint_threshold = revolute_threshold;
287  break;
289  check_joint = check_prismatic;
290  joint_threshold = prismatic_threshold;
291  break;
292  default:
294  "Joint %s has not supported type %s. \n"
295  "checkAbsoluteJointSpaceJump only supports prismatic and revolute joints.",
296  joint->getName().c_str(), joint->getTypeName().c_str());
297  continue;
298  }
299  if (check_joint)
300  {
301  double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint);
302  if (distance > joint_threshold)
303  {
304  ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s",
305  distance, joint_threshold, joint->getName().c_str());
306  still_valid = false;
307  break;
308  }
309  }
310  }
311 
312  if (!still_valid)
313  {
314  double percent_valid = (double)(traj_ix + 1) / (double)(traj.size());
315  traj.resize(traj_ix + 1);
316  return percent_valid;
317  }
318  }
319  return 1.0;
320 }
321 
322 } // end of namespace core
323 } // end of namespace moveit
moveit::core::GroupStateValidityCallbackFn
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
Definition: robot_state.h:136
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
moveit::core::CartesianInterpolator::computeCartesianPath
static double computeCartesianPath(RobotState *start_state, const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Vector3d &translation, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular li...
moveit::core::CartesianInterpolator::checkRelativeJointSpaceJump
static double checkRelativeJointSpaceJump(const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, double jump_threshold_factor)
Tests for relative joint space jumps of the trajectory traj.
Definition: cartesian_interpolator.cpp:298
check_isometry.h
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::JointModel::PRISMATIC
@ PRISMATIC
Definition: joint_model.h:181
moveit::core::JointModel::REVOLUTE
@ REVOLUTE
Definition: joint_model.h:180
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
cartesian_interpolator.h
moveit::core::CartesianInterpolator::checkJointSpaceJump
static double checkJointSpaceJump(const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const JumpThreshold &jump_threshold)
Tests joint space jumps of a trajectory.
Definition: cartesian_interpolator.cpp:282
moveit::core::LOGNAME
const std::string LOGNAME
Definition: joint_model_group.cpp:165
moveit::core::CartesianInterpolator::checkAbsoluteJointSpaceJump
static double checkAbsoluteJointSpaceJump(const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, double revolute_jump_threshold, double prismatic_jump_threshold)
Tests for absolute joint space jumps of the trajectory traj.
Definition: cartesian_interpolator.cpp:335
start
ROSCPP_DECL void start()
moveit::core::MIN_STEPS_FOR_JUMP_THRESH
static const std::size_t MIN_STEPS_FOR_JUMP_THRESH
It is recommended that there are at least 10 steps per trajectory for testing jump thresholds with co...
Definition: cartesian_interpolator.cpp:119
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:109
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 18 2022 02:26:15