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 namespace kinematics
41 {
43 const double KinematicsBase::DEFAULT_TIMEOUT = 1.0;
44 
45 void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name,
46  const std::string& base_frame, const std::string& tip_frame,
47  double search_discretization)
48 {
49  robot_description_ = robot_description;
50  group_name_ = group_name;
51  base_frame_ = removeSlash(base_frame);
52  tip_frame_ = removeSlash(tip_frame); // for backwards compatibility
53  tip_frames_.push_back(removeSlash(tip_frame));
54  search_discretization_ = search_discretization;
55  setSearchDiscretization(search_discretization);
56 }
57 
58 void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name,
59  const std::string& base_frame, const std::vector<std::string>& tip_frames,
60  double search_discretization)
61 {
62  robot_description_ = robot_description;
63  group_name_ = group_name;
64  base_frame_ = removeSlash(base_frame);
65  search_discretization_ = search_discretization;
66  setSearchDiscretization(search_discretization);
67 
68  // Copy tip frames to local vector after stripping slashes
69  tip_frames_.clear();
70  for (std::size_t i = 0; i < tip_frames.size(); ++i)
71  tip_frames_.push_back(removeSlash(tip_frames[i]));
72 
73  // Copy tip frames to our legacy variable if only one tip frame is passed in the input vector. Remove eventually.
74  if (tip_frames.size() == 1)
75  tip_frame_ = removeSlash(tip_frames[0]);
76 }
77 
78 bool KinematicsBase::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
79 {
80  for (std::size_t i = 0; i < redundant_joint_indices.size(); ++i)
81  {
82  if (redundant_joint_indices[i] >= getJointNames().size())
83  {
84  return false;
85  }
86  }
87  redundant_joint_indices_ = redundant_joint_indices;
89 
90  return true;
91 }
92 
93 bool KinematicsBase::setRedundantJoints(const std::vector<std::string>& redundant_joint_names)
94 {
95  const std::vector<std::string>& jnames = getJointNames();
96  std::vector<unsigned int> redundant_joint_indices;
97  for (std::size_t i = 0; i < redundant_joint_names.size(); ++i)
98  for (std::size_t j = 0; j < jnames.size(); ++j)
99  if (jnames[j] == redundant_joint_names[i])
100  {
101  redundant_joint_indices.push_back(j);
102  break;
103  }
104  return redundant_joint_indices.size() == redundant_joint_names.size() ? setRedundantJoints(redundant_joint_indices) :
105  false;
106 }
107 
108 std::string KinematicsBase::removeSlash(const std::string& str) const
109 {
110  return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str;
111 }
112 
113 bool KinematicsBase::supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out) const
114 {
115  // Default implementation for legacy solvers:
116  if (!jmg->isChain())
117  {
118  if (error_text_out)
119  {
120  *error_text_out = "This plugin only supports joint groups which are chains";
121  }
122  return false;
123  }
124 
125  return true;
126 }
127 
128 bool KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
129  const std::vector<double>& ik_seed_state,
130  std::vector<std::vector<double> >& solutions, KinematicsResult& result,
131  const KinematicsQueryOptions& options) const
132 {
133  std::vector<double> solution;
134  result.solution_percentage = 0.0;
135 
136  if (std::find(supported_methods_.begin(), supported_methods_.end(), options.discretization_method) ==
137  supported_methods_.end())
138  {
140  return false;
141  }
142 
143  if (ik_poses.size() != 1)
144  {
145  ROS_ERROR_NAMED("kinematics_base", "This kinematic solver does not support getPositionIK for multiple poses");
147  return false;
148  }
149 
150  if (ik_poses.empty())
151  {
152  ROS_ERROR_NAMED("kinematics_base", "Input ik_poses array is empty");
154  return false;
155  }
156 
157  moveit_msgs::MoveItErrorCodes error_code;
158  if (getPositionIK(ik_poses[0], ik_seed_state, solution, error_code, options))
159  {
160  solutions.resize(1);
161  solutions[0] = solution;
163  result.solution_percentage = 1.0f;
164  }
165  else
166  {
168  return false;
169  }
170 
171  return true;
172 }
173 
174 } // end of namespace kinematics
std::vector< unsigned int > redundant_joint_indices_
A set of options for the kinematics solver.
virtual const std::vector< std::string > & getJointNames() const =0
Return all the joint names in the order they are used internally.
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.
void setSearchDiscretization(double sd)
Set the search discretization value for all the redundant joints.
std::vector< std::string > tip_frames_
static const double DEFAULT_TIMEOUT
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.
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...
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,...)
std::string removeSlash(const std::string &str) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05