kinematics_base.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, 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, Dave Coleman */
36 
39 
40 static const std::string LOGNAME = "kinematics_base";
41 
42 namespace kinematics
43 {
45 const double KinematicsBase::DEFAULT_TIMEOUT = 1.0;
46 
47 static void noDeleter(const moveit::core::RobotModel* /*unused*/)
48 {
49 }
50 
51 void KinematicsBase::storeValues(const moveit::core::RobotModel& robot_model, const std::string& group_name,
52  const std::string& base_frame, const std::vector<std::string>& tip_frames,
53  double search_discretization)
54 {
55  // The RobotModel is passed in as a borrowed reference from the JointModelGroup belonging to this RobotModel.
56  // Hence, it is ensured that the RobotModel will not be destroyed before the JMG and its associated
57  // kinematics solvers. To keep RobotModelPtr API (instead of storing the reference here only), but break
58  // the circular reference (RM => JMG => KS -> RM), here we create a new shared_ptr that doesn't delete the RM.
59  robot_model_ = moveit::core::RobotModelConstPtr(&robot_model, &noDeleter);
60  robot_description_ = "";
61  group_name_ = group_name;
62  base_frame_ = removeSlash(base_frame);
63  tip_frames_.clear();
64  for (const std::string& name : tip_frames)
65  tip_frames_.push_back(removeSlash(name));
66  setSearchDiscretization(search_discretization);
67  search_discretization_ = search_discretization;
68 }
69 
70 void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name,
71  const std::string& base_frame, const std::vector<std::string>& tip_frames,
72  double search_discretization)
73 {
74  robot_model_.reset();
75  robot_description_ = robot_description;
76  group_name_ = group_name;
77  base_frame_ = removeSlash(base_frame);
78  tip_frames_.clear();
79  for (const std::string& name : tip_frames)
80  tip_frames_.push_back(removeSlash(name));
81  setSearchDiscretization(search_discretization);
82 
83  // store deprecated values for backwards compatibility
84  search_discretization_ = search_discretization;
85  if (tip_frames_.size() == 1)
87  else
88  tip_frame_.clear();
89 }
90 
91 void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name,
92  const std::string& base_frame, const std::string& tip_frame,
93  double search_discretization)
94 {
95  setValues(robot_description, group_name, base_frame, std::vector<std::string>({ tip_frame }), search_discretization);
96 }
97 
98 bool KinematicsBase::initialize(const std::string& robot_description, const std::string& group_name,
99  const std::string& base_frame, const std::string& tip_frame,
100  double search_discretization)
101 {
102  return false; // default implementation returns false
103 }
104 
105 bool KinematicsBase::initialize(const std::string& robot_description, const std::string& group_name,
106  const std::string& base_frame, const std::vector<std::string>& tip_frames,
107  double search_discretization)
108 {
109  // For IK solvers that do not support multiple tip frames, fall back to single pose call
110  if (tip_frames.size() == 1)
111  {
112  return initialize(robot_description, group_name, base_frame, tip_frames[0], search_discretization);
113  }
114 
115  ROS_ERROR_NAMED(LOGNAME, "This solver does not support multiple tip frames");
116  return false;
117 }
118 
119 bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name,
120  const std::string& base_frame, const std::vector<std::string>& tip_frames,
121  double search_discretization)
122 {
124  "IK plugin for group '%s' relies on deprecated API. "
125  "Please implement initialize(RobotModel, ...).",
126  group_name.c_str());
127  return false;
128 }
129 
130 bool KinematicsBase::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
131 {
132  for (const unsigned int& redundant_joint_index : redundant_joint_indices)
133  {
134  if (redundant_joint_index >= getJointNames().size())
135  {
136  return false;
137  }
138  }
139  redundant_joint_indices_ = redundant_joint_indices;
141 
142  return true;
143 }
144 
145 bool KinematicsBase::setRedundantJoints(const std::vector<std::string>& redundant_joint_names)
146 {
147  const std::vector<std::string>& jnames = getJointNames();
148  std::vector<unsigned int> redundant_joint_indices;
149  for (const std::string& redundant_joint_name : redundant_joint_names)
150  for (std::size_t j = 0; j < jnames.size(); ++j)
151  if (jnames[j] == redundant_joint_name)
152  {
153  redundant_joint_indices.push_back(j);
154  break;
155  }
156  return redundant_joint_indices.size() == redundant_joint_names.size() ? setRedundantJoints(redundant_joint_indices) :
157  false;
158 }
159 
160 std::string KinematicsBase::removeSlash(const std::string& str) const
161 {
162  return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str;
163 }
164 
165 bool KinematicsBase::supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out) const
166 {
167  // Default implementation for legacy solvers:
168  if (!jmg->isChain())
169  {
170  if (error_text_out)
171  {
172  *error_text_out = "This plugin only supports joint groups which are chains";
173  }
174  return false;
175  }
176 
177  return true;
178 }
179 
181  : tip_frame_("DEPRECATED")
182  // help users understand why this variable might not be set
183  // (if multiple tip frames provided, this variable will be unset)
184  , search_discretization_(DEFAULT_SEARCH_DISCRETIZATION)
185  , default_timeout_(DEFAULT_TIMEOUT)
186 {
188 }
189 
191 
192 bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
193  const std::vector<double>& ik_seed_state,
194  std::vector<std::vector<double> >& solutions, KinematicsResult& result,
195  const KinematicsQueryOptions& options) const
196 {
197  std::vector<double> solution;
198  result.solution_percentage = 0.0;
199 
200  if (std::find(supported_methods_.begin(), supported_methods_.end(), options.discretization_method) ==
201  supported_methods_.end())
202  {
204  return false;
205  }
206 
207  if (ik_poses.size() != 1)
208  {
209  ROS_ERROR_NAMED(LOGNAME, "This kinematic solver does not support getPositionIK for multiple tips");
211  return false;
212  }
213 
214  if (ik_poses.empty())
215  {
216  ROS_ERROR_NAMED(LOGNAME, "Input ik_poses array is empty");
218  return false;
219  }
220 
221  moveit_msgs::MoveItErrorCodes error_code;
222  if (getPositionIK(ik_poses[0], ik_seed_state, solution, error_code, options))
223  {
224  solutions.resize(1);
225  solutions[0] = solution;
227  result.solution_percentage = 1.0f;
228  }
229  else
230  {
232  return false;
233  }
234 
235  return true;
236 }
237 
238 } // end of namespace kinematics
kinematics::KinematicsBase::setRedundantJoints
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
Set a set of redundant joints for the kinematics solver to use. This can fail, depending on the IK so...
Definition: kinematics_base.cpp:130
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:81
LOGNAME
static const std::string LOGNAME
Definition: kinematics_base.cpp:40
kinematics::KinematicsBase::supported_methods_
std::vector< DiscretizationMethod > supported_methods_
Definition: kinematics_base.h:601
kinematics::KinematicsBase::storeValues
void storeValues(const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)
Definition: kinematics_base.cpp:51
kinematics::KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION
static const double DEFAULT_SEARCH_DISCRETIZATION
Definition: kinematics_base.h:147
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
kinematics::noDeleter
static void noDeleter(const moveit::core::RobotModel *)
Definition: kinematics_base.cpp:47
kinematics::KinematicsResult::solution_percentage
double solution_percentage
Definition: kinematics_base.h:134
kinematics::KinematicsBase::setValues
virtual void setValues(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
Set the parameters for the solver, for use with kinematic chain IK solvers.
Definition: kinematics_base.cpp:91
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:132
kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED
@ MULTIPLE_TIPS_NOT_SUPPORTED
Definition: kinematics_base.h:93
kinematics_base.h
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
kinematics::KinematicsBase::search_discretization_
double search_discretization_
Definition: kinematics_base.h:596
kinematics::KinematicsBase::group_name_
std::string group_name_
Definition: kinematics_base.h:588
kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED
@ UNSUPORTED_DISCRETIZATION_REQUESTED
Definition: kinematics_base.h:90
kinematics::KinematicsBase::robot_description_
std::string robot_description_
Definition: kinematics_base.h:587
kinematics::KinematicErrors::OK
@ OK
Definition: kinematics_base.h:89
kinematics::KinematicsBase::~KinematicsBase
virtual ~KinematicsBase()
Virtual destructor for the interface.
kinematics::KinematicsBase::redundant_joint_indices_
std::vector< unsigned int > redundant_joint_indices_
Definition: kinematics_base.h:599
kinematics::KinematicsBase::KinematicsBase
KinematicsBase()
Definition: kinematics_base.cpp:180
kinematics::KinematicsBase::initialize
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
Initialization function for the kinematics, for use with kinematic chain IK solvers.
Definition: kinematics_base.cpp:98
kinematics
API for forward and inverse kinematics.
Definition: kinematics_base.h:58
kinematics::KinematicsBase::removeSlash
std::string removeSlash(const std::string &str) const
Definition: kinematics_base.cpp:160
kinematics::KinematicsResult
Definition: kinematics_base.h:131
kinematics::KinematicsBase::getJointNames
virtual const std::vector< std::string > & getJointNames() const =0
Return all the joint names in the order they are used internally.
kinematics::KinematicsBase::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: kinematics_base.h:586
kinematics::KinematicsBase::supportsGroup
virtual bool supportsGroup(const moveit::core::JointModelGroup *jmg, std::string *error_text_out=nullptr) const
Check if this solver supports a given JointModelGroup.
Definition: kinematics_base.cpp:165
kinematics::KinematicsQueryOptions::discretization_method
DiscretizationMethod discretization_method
Definition: kinematics_base.h:118
kinematics::KinematicsBase::setSearchDiscretization
void setSearchDiscretization(double sd)
Set the search discretization value for all the redundant joints.
Definition: kinematics_base.h:515
kinematics::KinematicErrors::EMPTY_TIP_POSES
@ EMPTY_TIP_POSES
Definition: kinematics_base.h:94
kinematics::KinematicsBase::tip_frames_
std::vector< std::string > tip_frames_
Definition: kinematics_base.h:590
kinematics::DiscretizationMethods::NO_DISCRETIZATION
@ NO_DISCRETIZATION
Definition: kinematics_base.h:69
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:107
moveit::core::JointModelGroup::isChain
bool isChain() const
Check if this group is a linear chain.
Definition: joint_model_group.h:499
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
joint_model_group.h
kinematics::KinematicErrors::NO_SOLUTION
@ NO_SOLUTION
Definition: kinematics_base.h:97
kinematics::KinematicsBase::DEFAULT_TIMEOUT
static const double DEFAULT_TIMEOUT
Definition: kinematics_base.h:148
kinematics::KinematicsBase::getPositionIK
virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const =0
Given a desired pose of the end-effector, compute the joint angles to reach it.
kinematics::KinematicsResult::kinematic_error
KinematicError kinematic_error
Definition: kinematics_base.h:133
kinematics::KinematicsBase::base_frame_
std::string base_frame_
Definition: kinematics_base.h:589
kinematics::KinematicsBase::tip_frame_
std::string tip_frame_
Definition: kinematics_base.h:595


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Oct 21 2020 03:28:04