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:
130  static double
131  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
132  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
133  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
134  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
137 
144  static double
145  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
146  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
147  const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
148  const JumpThreshold& jump_threshold,
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 EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
162  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
165 
183  static double checkJointSpaceJump(const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
184  const JumpThreshold& jump_threshold);
185 
198  static double checkRelativeJointSpaceJump(const JointModelGroup* group,
199  std::vector<std::shared_ptr<RobotState>>& traj,
200  double jump_threshold_factor);
201 
216  static double checkAbsoluteJointSpaceJump(const JointModelGroup* group,
217  std::vector<std::shared_ptr<RobotState>>& traj,
218  double revolute_jump_threshold, double prismatic_jump_threshold);
219 };
220 
221 } // end of namespace core
222 } // 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
moveit::core::JumpThreshold
Struct for containing jump_threshold.
Definition: cartesian_interpolator.h:122
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
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:302
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::CartesianInterpolator::computeCartesianPath
static double computeCartesianPath(RobotState *start_state, const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, 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 gr...
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:286
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:339
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 Tue Oct 12 2021 02:24:44