cartesian_interpolator.h
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 Willow Garage, Inc. 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, Mike Lautman */
38 
39 #pragma once
40 
42 
43 #include <boost/assert.hpp>
44 
45 namespace moveit
46 {
47 namespace core
48 {
54 struct JumpThreshold
55 {
56  double factor;
57  double revolute; // Radians
58  double prismatic; // Meters
59 
60  explicit JumpThreshold() : factor(0.0), revolute(0.0), prismatic(0.0)
61  {
62  }
63 
64  explicit JumpThreshold(double jt_factor) : JumpThreshold()
65  {
66  factor = jt_factor;
67  }
68 
69  explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold()
70  {
71  revolute = jt_revolute; // Radians
72  prismatic = jt_prismatic; // Meters
73  }
74 };
75 
81 struct MaxEEFStep
82 {
84  {
85  }
86 
87  MaxEEFStep(double step_size) : translation(step_size), rotation(3.5 * step_size) // 0.035 rad = 2 deg
88  {
89  }
90 
91  double translation; // Meters
92  double rotation; // Radians
93 };
94 
95 class CartesianInterpolator
96 {
97  // TODO(mlautman): Eventually, this planner should be moved out of robot_state
98 
99 public:
128  static double
129  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
130  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
131  const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
132  const JumpThreshold& jump_threshold,
135 
140  static double
141  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
142  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
143  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
144  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
147  {
148  return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step,
149  jump_threshold, validCallback, options);
150  }
151 
158  static double
159  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
160  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
161  const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
162  const JumpThreshold& jump_threshold,
165  const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
166 
173  static double
174  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
175  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
176  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
177  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
180  const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
181 
199  static double checkJointSpaceJump(const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
200  const JumpThreshold& jump_threshold);
201 
214  static double checkRelativeJointSpaceJump(const JointModelGroup* group,
215  std::vector<std::shared_ptr<RobotState>>& traj,
216  double jump_threshold_factor);
217 
232  static double checkAbsoluteJointSpaceJump(const JointModelGroup* group,
233  std::vector<std::shared_ptr<RobotState>>& traj,
234  double revolute_jump_threshold, double prismatic_jump_threshold);
235 };
236 
237 } // end of namespace core
238 } // end of namespace moveit
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
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::JumpThreshold
Struct for containing jump_threshold.
Definition: cartesian_interpolator.h:122
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
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::JumpThreshold::revolute
double revolute
Definition: cartesian_interpolator.h:159
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:300
moveit::core::JumpThreshold::prismatic
double prismatic
Definition: cartesian_interpolator.h:160
moveit::core::MaxEEFStep::MaxEEFStep
MaxEEFStep(double translation, double rotation)
Definition: cartesian_interpolator.h:151
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::MaxEEFStep
Struct for containing max_step for computeCartesianPath.
Definition: cartesian_interpolator.h:149
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::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:337
moveit::core::JumpThreshold::factor
double factor
Definition: cartesian_interpolator.h:158
moveit::core::MaxEEFStep::translation
double translation
Definition: cartesian_interpolator.h:159
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
moveit::core::MaxEEFStep::rotation
double rotation
Definition: cartesian_interpolator.h:160
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
robot_state.h
moveit::core::JumpThreshold::JumpThreshold
JumpThreshold()
Definition: cartesian_interpolator.h:162
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 Jul 21 2024 02:24:28