pr2_arm_kinematics_plugin.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 */
36 
37 #include <geometry_msgs/PoseStamped.h>
41 #include <algorithm>
42 #include <numeric>
43 
45 
46 using namespace KDL;
47 using namespace std;
48 
49 namespace pr2_arm_kinematics
50 {
51 bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count)
52 {
53  if (count > 0)
54  {
55  if (-count >= min_count)
56  {
57  count = -count;
58  return true;
59  }
60  else if (count + 1 <= max_count)
61  {
62  count = count + 1;
63  return true;
64  }
65  else
66  {
67  return false;
68  }
69  }
70  else
71  {
72  if (1 - count <= max_count)
73  {
74  count = 1 - count;
75  return true;
76  }
77  else if (count - 1 >= min_count)
78  {
79  count = count - 1;
80  return true;
81  }
82  else
83  return false;
84  }
85 }
86 
87 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name,
88  const std::string& tip_frame_name, const double& search_discretization_angle,
89  const int& free_angle)
91 {
92  search_discretization_angle_ = search_discretization_angle;
93  free_angle_ = free_angle;
94  root_frame_name_ = root_frame_name;
95  if (!pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name))
96  active_ = false;
97  else
98  active_ = true;
99 }
100 
101 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out)
102 {
103  const bool verbose = false;
104  Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
105  std::vector<std::vector<double> > solution_ik;
106  if (free_angle_ == 0)
107  {
108  if (verbose)
109  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solving with %f", q_init(0));
110  pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
111  }
112  else
113  {
114  pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
115  }
116 
117  if (solution_ik.empty())
118  return -1;
119 
120  double min_distance = 1e6;
121  int min_index = -1;
122 
123  for (int i = 0; i < (int)solution_ik.size(); i++)
124  {
125  if (verbose)
126  {
127  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solution : %d", (int)solution_ik.size());
128 
129  for (int j = 0; j < (int)solution_ik[i].size(); j++)
130  {
131  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d: %f", j, solution_ik[i][j]);
132  }
133  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
134  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
135  }
136  double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
137  if (tmp_distance < min_distance)
138  {
139  min_distance = tmp_distance;
140  min_index = i;
141  }
142  }
143 
144  if (min_index > -1)
145  {
146  q_out.resize((int)solution_ik[min_index].size());
147  for (int i = 0; i < (int)solution_ik[min_index].size(); i++)
148  {
149  q_out(i) = solution_ik[min_index][i];
150  }
151  return 1;
152  }
153  else
154  return -1;
155 }
156 
158  const double& timeout)
159 {
160  const bool verbose = false;
161  KDL::JntArray q_init = q_in;
162  double initial_guess = q_init(free_angle_);
163 
164  ros::Time start_time = ros::Time::now();
165  double loop_time = 0;
166  int count = 0;
167 
168  int num_positive_increments =
169  (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position - initial_guess) / search_discretization_angle_);
170  int num_negative_increments =
171  (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_);
172  if (verbose)
173  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%f %f %f %d %d \n\n", initial_guess,
174  pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,
175  pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments,
176  num_negative_increments);
177  while (loop_time < timeout)
178  {
179  if (CartToJnt(q_init, p_in, q_out) > 0)
180  return 1;
181  if (!getCount(count, num_positive_increments, -num_negative_increments))
182  return -1;
183  q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
184  if (verbose)
185  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d, %f", count, q_init(free_angle_));
186  loop_time = (ros::Time::now() - start_time).toSec();
187  }
188  if (loop_time >= timeout)
189  {
190  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "IK Timed out in %f seconds", timeout);
191  return TIMED_OUT;
192  }
193  else
194  {
195  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "No IK solution was found");
196  return NO_IK_SOLUTION;
197  }
198  return NO_IK_SOLUTION;
199 }
200 
201 bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name, const std::string& tip_name,
202  KDL::Chain& kdl_chain)
203 {
204  // create robot chain from root to tip
205  KDL::Tree tree;
206  if (!kdl_parser::treeFromUrdfModel(model, tree))
207  {
208  ROS_ERROR("Could not initialize tree object");
209  return false;
210  }
211  if (!tree.getChain(root_name, tip_name, kdl_chain))
212  {
213  ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
214  return false;
215  }
216  return true;
217 }
218 
219 Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame& p)
220 {
221  Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
222  for (int i = 0; i < 3; i++)
223  {
224  for (int j = 0; j < 3; j++)
225  {
226  b(i, j) = p.M(i, j);
227  }
228  b(i, 3) = p.p(i);
229  }
230  return b;
231 }
232 
233 double computeEuclideanDistance(const std::vector<double>& array_1, const KDL::JntArray& array_2)
234 {
235  double distance = 0.0;
236  for (int i = 0; i < (int)array_1.size(); i++)
237  {
238  distance += (array_1[i] - array_2(i)) * (array_1[i] - array_2(i));
239  }
240  return sqrt(distance);
241 }
242 
243 void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::KinematicSolverInfo& chain_info)
244 {
245  int i = 0; // segment number
246  while (i < (int)chain.getNrOfSegments())
247  {
248  chain_info.link_names.push_back(chain.getSegment(i).getName());
249  i++;
250  }
251 }
252 
254 {
255 }
256 
258 {
259  return active_;
260 }
261 
262 void PR2ArmKinematicsPlugin::setRobotModel(urdf::ModelInterfaceSharedPtr& robot_model)
263 {
264  robot_model_ = robot_model;
265 }
266 
267 bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
268  const std::string& base_name, const std::string& tip_name,
269  double search_discretization)
270 {
271  setValues(robot_description, group_name, base_name, tip_name, search_discretization);
272  const bool verbose = false;
273  std::string xml_string;
274  dimension_ = 7;
275 
276  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Loading KDL Tree");
278  {
279  active_ = false;
280  ROS_ERROR("Could not load kdl tree");
281  }
283  free_angle_ = 2;
284 
287  if (!pr2_arm_ik_solver_->active_)
288  {
289  ROS_ERROR("Could not load ik");
290  active_ = false;
291  }
292  else
293  {
294  pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
296  fk_solver_info_.joint_names = ik_solver_info_.joint_names;
297 
298  if (verbose)
299  {
300  for (unsigned int i = 0; i < ik_solver_info_.joint_names.size(); i++)
301  {
302  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics:: joint name: %s",
303  ik_solver_info_.joint_names[i].c_str());
304  }
305  for (unsigned int i = 0; i < ik_solver_info_.link_names.size(); i++)
306  {
307  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve IK for %s",
308  ik_solver_info_.link_names[i].c_str());
309  }
310  for (unsigned int i = 0; i < fk_solver_info_.link_names.size(); i++)
311  {
312  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve FK for %s",
313  fk_solver_info_.link_names[i].c_str());
314  }
315  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2KinematicsPlugin::active for %s", group_name.c_str());
316  }
317  active_ = true;
318  }
319  return active_;
320 }
321 
322 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
323  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
324  const kinematics::KinematicsQueryOptions& options) const
325 {
326  return false;
327 }
328 
329 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
330  const std::vector<double>& ik_seed_state, double timeout,
331  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
332  const kinematics::KinematicsQueryOptions& options) const
333 {
334  if (!active_)
335  {
336  ROS_ERROR("kinematics not active");
337  error_code.val = error_code.PLANNING_FAILED;
338  return false;
339  }
340  KDL::Frame pose_desired;
341  Eigen::Affine3d tp;
342  tf::poseMsgToEigen(ik_pose, tp);
343  tf::transformEigenToKDL(tp, pose_desired);
344 
345  // Do the IK
346  KDL::JntArray jnt_pos_in;
347  KDL::JntArray jnt_pos_out;
348  jnt_pos_in.resize(dimension_);
349  for (int i = 0; i < dimension_; i++)
350  {
351  jnt_pos_in(i) = ik_seed_state[i];
352  }
353 
354  int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in, pose_desired, jnt_pos_out, timeout);
355  if (ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
356  {
357  error_code.val = error_code.NO_IK_SOLUTION;
358  return false;
359  }
360 
361  if (ik_valid >= 0)
362  {
363  solution.resize(dimension_);
364  for (int i = 0; i < dimension_; i++)
365  {
366  solution[i] = jnt_pos_out(i);
367  }
368  error_code.val = error_code.SUCCESS;
369  return true;
370  }
371  else
372  {
373  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "An IK solution could not be found");
374  error_code.val = error_code.NO_IK_SOLUTION;
375  return false;
376  }
377 }
378 
379 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
380  const std::vector<double>& ik_seed_state, double timeout,
381  const std::vector<double>& consistency_limit,
382  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
383  const kinematics::KinematicsQueryOptions& options) const
384 {
385  return false;
386 }
387 
388 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
389  const std::vector<double>& ik_seed_state, double timeout,
390  std::vector<double>& solution, const IKCallbackFn& solution_callback,
391  moveit_msgs::MoveItErrorCodes& error_code,
392  const kinematics::KinematicsQueryOptions& options) const
393 {
394  return false;
395 }
396 
397 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
398  const std::vector<double>& ik_seed_state, double timeout,
399  const std::vector<double>& consistency_limit,
400  std::vector<double>& solution, const IKCallbackFn& solution_callback,
401  moveit_msgs::MoveItErrorCodes& error_code,
402  const kinematics::KinematicsQueryOptions& options) const
403 {
404  return false;
405 }
406 
407 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
408  const std::vector<double>& joint_angles,
409  std::vector<geometry_msgs::Pose>& poses) const
410 {
411  return false;
412 }
413 
414 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
415 {
416  if (!active_)
417  {
418  ROS_ERROR("kinematics not active");
419  }
420  return ik_solver_info_.joint_names;
421 }
422 
423 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
424 {
425  if (!active_)
426  {
427  ROS_ERROR("kinematics not active");
428  }
429  return fk_solver_info_.link_names;
430 }
431 
432 } // namespace
void computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &shoulder_pan_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder pan angle.
Definition: pr2_arm_ik.cpp:197
A set of options for the kinematics solver.
Core components of MoveIt!
const Segment & getSegment(unsigned int nr) const
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
bool getKDLChain(const urdf::ModelInterface &model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
unsigned int getNrOfSegments() const
bool getCount(int &count, const int &max_count, const int &min_count)
virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
Given a desired pose of the end-effector, search for the joint angles required to reach it...
moveit_msgs::KinematicSolverInfo fk_solver_info_
int CartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout)
static const int NO_IK_SOLUTION
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
void computeIKShoulderRoll(const Eigen::Matrix4f &g_in, const double &shoulder_roll_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder roll angle. h
Definition: pr2_arm_ik.cpp:464
Rotation M
#define ROS_DEBUG_NAMED(name,...)
const std::string & getName() const
bool verbose
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
Given a set of joint angles and a set of links, compute their pose.
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
Given a desired pose of the end-effector, compute the joint angles to reach it.
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out)
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
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.
def xml_string(rootXml, addHeader=True)
bool active_
Indicates whether the solver has been successfully initialized.
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
The signature for a callback that can compute IK.
moveit_msgs::KinematicSolverInfo ik_solver_info_
void resize(unsigned int newSize)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
bool init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name)
Initialize the solver by providing a urdf::Model and a root and tip name.
Definition: pr2_arm_ik.cpp:56
static Time now()
moveit_msgs::KinematicSolverInfo solver_info_
get chain information about the arm.
Definition: pr2_arm_ik.h:166
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
Initialization function for the kinematics.
#define ROS_ERROR_STREAM(args)
void setRobotModel(urdf::ModelInterfaceSharedPtr &robot_model)
#define ROS_ERROR(...)
bool isActive()
Specifies if the node is active or not.
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 18 2018 02:48:31