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 {
123  ROS_WARN_NAMED(LOGNAME, "IK plugin for group '%s' relies on deprecated API. "
124  "Please implement initialize(RobotModel, ...).",
125  group_name.c_str());
126  return false;
127 }
128 
129 bool KinematicsBase::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
130 {
131  for (std::size_t i = 0; i < redundant_joint_indices.size(); ++i)
132  {
133  if (redundant_joint_indices[i] >= getJointNames().size())
134  {
135  return false;
136  }
137  }
138  redundant_joint_indices_ = redundant_joint_indices;
140 
141  return true;
142 }
143 
144 bool KinematicsBase::setRedundantJoints(const std::vector<std::string>& redundant_joint_names)
145 {
146  const std::vector<std::string>& jnames = getJointNames();
147  std::vector<unsigned int> redundant_joint_indices;
148  for (std::size_t i = 0; i < redundant_joint_names.size(); ++i)
149  for (std::size_t j = 0; j < jnames.size(); ++j)
150  if (jnames[j] == redundant_joint_names[i])
151  {
152  redundant_joint_indices.push_back(j);
153  break;
154  }
155  return redundant_joint_indices.size() == redundant_joint_names.size() ? setRedundantJoints(redundant_joint_indices) :
156  false;
157 }
158 
159 std::string KinematicsBase::removeSlash(const std::string& str) const
160 {
161  return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str;
162 }
163 
164 bool KinematicsBase::supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out) const
165 {
166  // Default implementation for legacy solvers:
167  if (!jmg->isChain())
168  {
169  if (error_text_out)
170  {
171  *error_text_out = "This plugin only supports joint groups which are chains";
172  }
173  return false;
174  }
175 
176  return true;
177 }
178 
180  : tip_frame_("DEPRECATED")
181  // help users understand why this variable might not be set
182  // (if multiple tip frames provided, this variable will be unset)
185 {
187 }
188 
190 
191 bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
192  const std::vector<double>& ik_seed_state,
193  std::vector<std::vector<double> >& solutions, KinematicsResult& result,
194  const KinematicsQueryOptions& options) const
195 {
196  std::vector<double> solution;
197  result.solution_percentage = 0.0;
198 
199  if (std::find(supported_methods_.begin(), supported_methods_.end(), options.discretization_method) ==
200  supported_methods_.end())
201  {
203  return false;
204  }
205 
206  if (ik_poses.size() != 1)
207  {
208  ROS_ERROR_NAMED(LOGNAME, "This kinematic solver does not support getPositionIK for multiple tips");
210  return false;
211  }
212 
213  if (ik_poses.empty())
214  {
215  ROS_ERROR_NAMED(LOGNAME, "Input ik_poses array is empty");
217  return false;
218  }
219 
220  moveit_msgs::MoveItErrorCodes error_code;
221  if (getPositionIK(ik_poses[0], ik_seed_state, solution, error_code, options))
222  {
223  solutions.resize(1);
224  solutions[0] = solution;
226  result.solution_percentage = 1.0f;
227  }
228  else
229  {
231  return false;
232  }
233 
234  return true;
235 }
236 
237 } // end of namespace kinematics
std::vector< unsigned int > redundant_joint_indices_
A set of options for the kinematics solver.
std::string removeSlash(const std::string &str) const
Core components of MoveIt!
static void noDeleter(const moveit::core::RobotModel *)
virtual const std::vector< std::string > & getJointNames() const =0
Return all the joint names in the order they are used internally.
#define ROS_WARN_NAMED(name,...)
void setSearchDiscretization(double sd)
Set the search discretization value for all the redundant joints.
std::vector< std::string > tip_frames_
moveit::core::RobotModelConstPtr robot_model_
static const double DEFAULT_TIMEOUT
static const std::string LOGNAME
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.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
bool isChain() const
Check if this group is a linear chain.
virtual bool supportsGroup(const moveit::core::JointModelGroup *jmg, std::string *error_text_out=NULL) const
Check if this solver supports a given JointModelGroup.
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...
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)
DiscretizationMethod discretization_method
std::vector< DiscretizationMethod > supported_methods_
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.
static const double DEFAULT_SEARCH_DISCRETIZATION
API for forward and inverse kinematics.
#define ROS_ERROR_NAMED(name,...)
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.
virtual ~KinematicsBase()
Virtual destructor for the interface.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Feb 17 2020 03:50:49