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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Apr 21 2018 02:54:51