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>
39 #include <tf2_kdl/tf2_kdl.h>
40 #include <algorithm>
41 
44 
45 using namespace KDL;
46 using namespace std;
47 
48 namespace pr2_arm_kinematics
49 {
50 bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count)
51 {
52  if (count > 0)
53  {
54  if (-count >= min_count)
55  {
56  count = -count;
57  return true;
58  }
59  else if (count + 1 <= max_count)
60  {
61  count = count + 1;
62  return true;
63  }
64  else
65  {
66  return false;
67  }
68  }
69  else
70  {
71  if (1 - count <= max_count)
72  {
73  count = 1 - count;
74  return true;
75  }
76  else if (count - 1 >= min_count)
77  {
78  count = count - 1;
79  return true;
80  }
81  else
82  return false;
83  }
84 }
85 
86 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name,
87  const std::string& tip_frame_name, const double& search_discretization_angle,
88  const int& free_angle)
90 {
91  search_discretization_angle_ = search_discretization_angle;
92  free_angle_ = free_angle;
93  root_frame_name_ = root_frame_name;
94  if (!pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name))
95  active_ = false;
96  else
97  active_ = true;
98 }
99 
101 {
102  // TODO: move (re)allocation of any internal data structures here
103  // to react to changes in chain
104 }
105 
106 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out)
107 {
108  const bool verbose = false;
109  Eigen::Isometry3f b = KDLToEigenMatrix(p_in);
110  std::vector<std::vector<double> > solution_ik;
111  if (free_angle_ == 0)
112  {
113  if (verbose)
114  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solving with %f", q_init(0));
115  pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
116  }
117  else
118  {
119  pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
120  }
121 
122  if (solution_ik.empty())
123  return -1;
124 
125  double min_distance = 1e6;
126  int min_index = -1;
127 
128  for (int i = 0; i < (int)solution_ik.size(); i++)
129  {
130  if (verbose)
131  {
132  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solution : %d", (int)solution_ik.size());
133 
134  for (int j = 0; j < (int)solution_ik[i].size(); j++)
135  {
136  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d: %f", j, solution_ik[i][j]);
137  }
138  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
139  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
140  }
141  double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
142  if (tmp_distance < min_distance)
143  {
144  min_distance = tmp_distance;
145  min_index = i;
146  }
147  }
148 
149  if (min_index > -1)
150  {
151  q_out.resize((int)solution_ik[min_index].size());
152  for (int i = 0; i < (int)solution_ik[min_index].size(); i++)
153  {
154  q_out(i) = solution_ik[min_index][i];
155  }
156  return 1;
157  }
158  else
159  return -1;
160 }
161 
163  const double& timeout)
164 {
165  const bool verbose = false;
166  KDL::JntArray q_init = q_in;
167  double initial_guess = q_init(free_angle_);
168 
169  ros::Time start_time = ros::Time::now();
170  double loop_time = 0;
171  int count = 0;
172 
173  int num_positive_increments =
174  (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position - initial_guess) / search_discretization_angle_);
175  int num_negative_increments =
176  (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_);
177  if (verbose)
178  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%f %f %f %d %d \n\n", initial_guess,
179  pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,
180  pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments,
181  num_negative_increments);
182  while (loop_time < timeout)
183  {
184  if (CartToJnt(q_init, p_in, q_out) > 0)
185  return 1;
186  if (!getCount(count, num_positive_increments, -num_negative_increments))
187  return -1;
188  q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
189  if (verbose)
190  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d, %f", count, q_init(free_angle_));
191  loop_time = (ros::Time::now() - start_time).toSec();
192  }
193  if (loop_time >= timeout)
194  {
195  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "IK Timed out in %f seconds", timeout);
196  return TIMED_OUT;
197  }
198  else
199  {
200  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "No IK solution was found");
201  return NO_IK_SOLUTION;
202  }
203  return NO_IK_SOLUTION;
204 }
205 
206 bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name, const std::string& tip_name,
207  KDL::Chain& kdl_chain)
208 {
209  // create robot chain from root to tip
210  KDL::Tree tree;
211  if (!kdl_parser::treeFromUrdfModel(model, tree))
212  {
213  ROS_ERROR("Could not initialize tree object");
214  return false;
215  }
216  if (!tree.getChain(root_name, tip_name, kdl_chain))
217  {
218  ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
219  return false;
220  }
221  return true;
222 }
223 
224 Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame& p)
225 {
226  Eigen::Isometry3f b = Eigen::Isometry3f::Identity();
227  for (int i = 0; i < 3; i++)
228  {
229  for (int j = 0; j < 3; j++)
230  {
231  b(i, j) = p.M(i, j);
232  }
233  b(i, 3) = p.p(i);
234  }
235  return b;
236 }
237 
238 double computeEuclideanDistance(const std::vector<double>& array_1, const KDL::JntArray& array_2)
239 {
240  double distance = 0.0;
241  for (int i = 0; i < (int)array_1.size(); i++)
242  {
243  distance += (array_1[i] - array_2(i)) * (array_1[i] - array_2(i));
244  }
245  return sqrt(distance);
246 }
247 
248 void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::KinematicSolverInfo& chain_info)
249 {
250  int i = 0; // segment number
251  while (i < (int)chain.getNrOfSegments())
252  {
253  chain_info.link_names.push_back(chain.getSegment(i).getName());
254  i++;
255  }
256 }
257 
259 {
260 }
261 
263 {
264  return active_;
265 }
266 
268  const std::string& base_frame, const std::vector<std::string>& tip_frames,
269  double search_discretization)
270 {
271  storeValues(robot_model, group_name, base_frame, tip_frames, 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");
277  if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_))
278  {
279  active_ = false;
280  ROS_ERROR("Could not load kdl tree");
281  }
283  free_angle_ = 2;
284 
285  pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(*robot_model.getURDF(), base_frame_, tip_frames_[0],
286  search_discretization, free_angle_));
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  tf2::fromMsg(ik_pose, pose_desired);
342 
343  // Do the IK
344  KDL::JntArray jnt_pos_in;
345  KDL::JntArray jnt_pos_out;
346  jnt_pos_in.resize(dimension_);
347  for (int i = 0; i < dimension_; i++)
348  {
349  jnt_pos_in(i) = ik_seed_state[i];
350  }
351 
352  int ik_valid = pr2_arm_ik_solver_->cartToJntSearch(jnt_pos_in, pose_desired, jnt_pos_out, timeout);
353  if (ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
354  {
355  error_code.val = error_code.NO_IK_SOLUTION;
356  return false;
357  }
358 
359  if (ik_valid >= 0)
360  {
361  solution.resize(dimension_);
362  for (int i = 0; i < dimension_; i++)
363  {
364  solution[i] = jnt_pos_out(i);
365  }
366  error_code.val = error_code.SUCCESS;
367  return true;
368  }
369  else
370  {
371  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "An IK solution could not be found");
372  error_code.val = error_code.NO_IK_SOLUTION;
373  return false;
374  }
375 }
376 
377 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
378  const std::vector<double>& ik_seed_state, double timeout,
379  const std::vector<double>& consistency_limit,
380  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
381  const kinematics::KinematicsQueryOptions& options) const
382 {
383  return false;
384 }
385 
386 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
387  const std::vector<double>& ik_seed_state, double timeout,
388  std::vector<double>& solution, const IKCallbackFn& solution_callback,
389  moveit_msgs::MoveItErrorCodes& error_code,
390  const kinematics::KinematicsQueryOptions& options) const
391 {
392  return false;
393 }
394 
395 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
396  const std::vector<double>& ik_seed_state, double timeout,
397  const std::vector<double>& consistency_limit,
398  std::vector<double>& solution, const IKCallbackFn& solution_callback,
399  moveit_msgs::MoveItErrorCodes& error_code,
400  const kinematics::KinematicsQueryOptions& options) const
401 {
402  return false;
403 }
404 
405 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
406  const std::vector<double>& joint_angles,
407  std::vector<geometry_msgs::Pose>& poses) const
408 {
409  return false;
410 }
411 
412 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
413 {
414  if (!active_)
415  {
416  ROS_ERROR("kinematics not active");
417  }
418  return ik_solver_info_.joint_names;
419 }
420 
421 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
422 {
423  if (!active_)
424  {
425  ROS_ERROR("kinematics not active");
426  }
427  return fk_solver_info_.link_names;
428 }
429 
430 } // namespace pr2_arm_kinematics
A set of options for the kinematics solver.
Core components of MoveIt!
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
unsigned int getNrOfSegments() const
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_
std::vector< std::string > tip_frames_
const Segment & getSegment(unsigned int nr) const
bool getCount(int &count, const int &max_count, const int &min_count)
#define ROS_ERROR(...)
moveit_msgs::KinematicSolverInfo fk_solver_info_
static const int NO_IK_SOLUTION
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
Rotation M
#define ROS_DEBUG_NAMED(name,...)
bool verbose
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 override
Given a desired pose of the end-effector, compute the joint angles to reach it.
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)
void fromMsg(const A &, B &b)
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
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 override
Given a desired pose of the end-effector, search for the joint angles required to reach it...
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
Definition: robot_model.h:98
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
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
Signature for a callback to validate an IK solution. Typically used for collision checking...
moveit_msgs::KinematicSolverInfo ik_solver_info_
const std::vector< std::string > & getJointNames() const override
Return all the joint names in the order they are used internally.
void resize(unsigned int newSize)
bool initialize(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) override
Initialization function for the kinematics.
const std::vector< std::string > & getLinkNames() const override
Return all the link names in the order they are represented internally.
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
int cartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout)
const std::string & getName() const
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
void computeIKShoulderPan(const Eigen::Isometry3f &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
#define ROS_ERROR_STREAM(args)
bool isActive()
Specifies if the node is active or not.
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
void computeIKShoulderRoll(const Eigen::Isometry3f &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:462
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame &p)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Mar 2 2021 03:51:22