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 
79 struct MaxEEFStep
80 {
81  MaxEEFStep(double translation = 0.0, double rotation = 0.0) : translation(translation), rotation(rotation)
82  {
83  }
84 
85  double translation; // Meters
86  double rotation; // Radians
87 };
88 
89 class CartesianInterpolator
90 {
91  // TODO(mlautman): Eventually, this planner should be moved out of robot_state
92 
93 public:
124  static double
125  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
126  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
127  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
128  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
131 
138  static double
139  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
140  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
141  const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
142  const JumpThreshold& jump_threshold,
145 
152  static double
153  computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
154  std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
155  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
156  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
159 
177  static double checkJointSpaceJump(const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
178  const JumpThreshold& jump_threshold);
179 
192  static double checkRelativeJointSpaceJump(const JointModelGroup* group,
193  std::vector<std::shared_ptr<RobotState>>& traj,
194  double jump_threshold_factor);
195 
210  static double checkAbsoluteJointSpaceJump(const JointModelGroup* group,
211  std::vector<std::shared_ptr<RobotState>>& traj,
212  double revolute_jump_threshold, double prismatic_jump_threshold);
213 };
214 
215 } // end of namespace core
216 } // 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:301
moveit::core::JumpThreshold::prismatic
double prismatic
Definition: cartesian_interpolator.h:160
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::MaxEEFStep
MaxEEFStep(double translation=0.0, double rotation=0.0)
Definition: cartesian_interpolator.h:149
moveit::core::MaxEEFStep
Struct for containing max_step for computeCartesianPath.
Definition: cartesian_interpolator.h:147
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:285
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:338
moveit::core::JumpThreshold::factor
double factor
Definition: cartesian_interpolator.h:158
moveit::core::MaxEEFStep::translation
double translation
Definition: cartesian_interpolator.h:153
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:107
moveit::core::MaxEEFStep::rotation
double rotation
Definition: cartesian_interpolator.h:154
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 Sat Dec 12 2020 03:25:44