moveit2
The MoveIt Motion Planning Framework for ROS 2.
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/msg/pose_stamped.hpp>
38 #include <kdl_parser/kdl_parser.hpp>
39 #include <tf2_kdl/tf2_kdl.hpp>
40 #include <algorithm>
41 #include <cmath>
42 
45 
46 using namespace KDL;
47 using namespace std;
48 
49 namespace pr2_arm_kinematics
50 {
51 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin");
52 
53 bool PR2ArmIKSolver::getCount(int& count, int max_count, int min_count)
54 {
55  if (count > 0)
56  {
57  if (-count >= min_count)
58  {
59  count = -count;
60  return true;
61  }
62  else if (count + 1 <= max_count)
63  {
64  count = count + 1;
65  return true;
66  }
67  else
68  {
69  return false;
70  }
71  }
72  else
73  {
74  if (1 - count <= max_count)
75  {
76  count = 1 - count;
77  return true;
78  }
79  else if (count - 1 >= min_count)
80  {
81  count = count - 1;
82  return true;
83  }
84  else
85  return false;
86  }
87 }
88 
89 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name,
90  const std::string& tip_frame_name, double search_discretization_angle, int free_angle)
91  : ChainIkSolverPos()
92 {
93  search_discretization_angle_ = search_discretization_angle;
94  free_angle_ = free_angle;
95  root_frame_name_ = root_frame_name;
96  active_ = pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name);
97 }
98 
100 {
101  // TODO: move (re)allocation of any internal data structures here
102  // to react to changes in chain
103 }
104 
105 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out)
106 {
107  const bool verbose = false;
108  Eigen::Isometry3f b = kdlToEigenMatrix(p_in);
109  std::vector<std::vector<double> > solution_ik;
110  if (free_angle_ == 0)
111  {
112  if (verbose)
113  RCLCPP_WARN(LOGGER, "Solving with %f", q_init(0));
114  pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
115  }
116  else
117  {
118  pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
119  }
120 
121  if (solution_ik.empty())
122  return -1;
123 
124  double min_distance = 1e6;
125  int min_index = -1;
126 
127  for (int i = 0; i < static_cast<int>(solution_ik.size()); ++i)
128  {
129  if (verbose)
130  {
131  RCLCPP_WARN(LOGGER, "Solution : %d", static_cast<int>(solution_ik.size()));
132 
133  for (int j = 0; j < static_cast<int>(solution_ik[i].size()); ++j)
134  {
135  RCLCPP_WARN(LOGGER, "%d: %f", j, solution_ik[i][j]);
136  }
137  RCLCPP_WARN(LOGGER, " ");
138  RCLCPP_WARN(LOGGER, " ");
139  }
140  double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
141  if (tmp_distance < min_distance)
142  {
143  min_distance = tmp_distance;
144  min_index = i;
145  }
146  }
147 
148  if (min_index > -1)
149  {
150  q_out.resize(static_cast<int>(solution_ik[min_index].size()));
151  for (int i = 0; i < static_cast<int>(solution_ik[min_index].size()); ++i)
152  {
153  q_out(i) = solution_ik[min_index][i];
154  }
155  return 1;
156  }
157  else
158  return -1;
159 }
160 
161 int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out,
162  double timeout)
163 {
164  const bool verbose = false;
165  KDL::JntArray q_init = q_in;
166  double initial_guess = q_init(free_angle_);
167 
168  rclcpp::Time start_time = rclcpp::Clock(RCL_ROS_TIME).now();
169  double loop_time = 0;
170  int count = 0;
171 
172  int num_positive_increments = static_cast<int>(
173  (pr2_arm_ik_.solver_info_.limits[free_angle_].max_position - initial_guess) / search_discretization_angle_);
174  int num_negative_increments = static_cast<int>(
175  (initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_);
176  if (verbose)
177  {
178  RCLCPP_WARN(LOGGER, "%f %f %f %d %d \n\n", initial_guess, pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,
179  pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments,
180  num_negative_increments);
181  }
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  RCLCPP_WARN(LOGGER, "%d, %f", count, q_init(free_angle_));
191  loop_time = rclcpp::Clock(RCL_ROS_TIME).now().seconds() - start_time.seconds();
192  }
193  if (loop_time >= timeout)
194  {
195  RCLCPP_WARN(LOGGER, "IK Timed out in %f seconds", timeout);
196  return TIMED_OUT;
197  }
198  else
199  {
200  RCLCPP_WARN(LOGGER, "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  RCLCPP_ERROR(LOGGER, "Could not initialize tree object");
214  return false;
215  }
216  if (!tree.getChain(root_name, tip_name, kdl_chain))
217  {
218  RCLCPP_ERROR(LOGGER, "Could not initialize chain object for base %s tip %s", root_name.c_str(), tip_name.c_str());
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 < static_cast<int>(array_1.size()); ++i)
242  {
243  distance += (array_1[i] - array_2(i)) * (array_1[i] - array_2(i));
244  }
245  return std::sqrt(distance);
246 }
247 
248 void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::msg::KinematicSolverInfo& chain_info)
249 {
250  int i = 0; // segment number
251  while (i < static_cast<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 
267 bool PR2ArmKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node,
268  const moveit::core::RobotModel& robot_model, const std::string& group_name,
269  const std::string& base_frame, const std::vector<std::string>& tip_frames,
270  double search_discretization)
271 {
272  node_ = node;
273  storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
274  const bool verbose = false;
275  std::string xml_string;
276  dimension_ = 7;
277 
278  RCLCPP_WARN(LOGGER, "Loading KDL Tree");
279  if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_))
280  {
281  active_ = false;
282  RCLCPP_ERROR(LOGGER, "Could not load kdl tree");
283  }
284  jnt_to_pose_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(kdl_chain_);
285  free_angle_ = 2;
286 
287  pr2_arm_ik_solver_ = std::make_shared<pr2_arm_kinematics::PR2ArmIKSolver>(
288  *robot_model.getURDF(), base_frame_, tip_frames_[0], search_discretization, free_angle_);
289  if (!pr2_arm_ik_solver_->active_)
290  {
291  RCLCPP_ERROR(LOGGER, "Could not load ik");
292  active_ = false;
293  }
294  else
295  {
296  pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
298  fk_solver_info_.joint_names = ik_solver_info_.joint_names;
299 
300  if (verbose)
301  {
302  for (const std::string& joint_name : ik_solver_info_.joint_names)
303  {
304  RCLCPP_WARN(LOGGER, "PR2Kinematics:: joint name: %s", joint_name.c_str());
305  }
306  for (const std::string& link_name : ik_solver_info_.link_names)
307  {
308  RCLCPP_WARN(LOGGER, "PR2Kinematics can solve IK for %s", link_name.c_str());
309  }
310  for (const std::string& link_name : fk_solver_info_.link_names)
311  {
312  RCLCPP_WARN(LOGGER, "PR2Kinematics can solve FK for %s", link_name.c_str());
313  }
314  RCLCPP_WARN(LOGGER, "PR2KinematicsPlugin::active for %s", group_name.c_str());
315  }
316  active_ = true;
317  }
318  return active_;
319 }
320 
321 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& /*ik_pose*/,
322  const std::vector<double>& /*ik_seed_state*/,
323  std::vector<double>& /*solution*/,
324  moveit_msgs::msg::MoveItErrorCodes& /*error_code*/,
325  const kinematics::KinematicsQueryOptions& /*options*/) const
326 {
327  return false;
328 }
329 
330 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
331  const std::vector<double>& ik_seed_state, double timeout,
332  std::vector<double>& solution,
333  moveit_msgs::msg::MoveItErrorCodes& error_code,
334  const kinematics::KinematicsQueryOptions& /*options*/) const
335 {
336  if (!active_)
337  {
338  RCLCPP_ERROR(LOGGER, "kinematics not active");
339  error_code.val = error_code.PLANNING_FAILED;
340  return false;
341  }
342 
343  geometry_msgs::msg::PoseStamped ik_pose_stamped;
344  ik_pose_stamped.pose = ik_pose;
345 
346  tf2::Stamped<KDL::Frame> pose_desired;
347 
348  tf2::fromMsg(ik_pose_stamped, pose_desired);
349 
350  // Do the IK
351  KDL::JntArray jnt_pos_in;
352  KDL::JntArray jnt_pos_out;
353  jnt_pos_in.resize(dimension_);
354  for (int i = 0; i < dimension_; ++i)
355  {
356  jnt_pos_in(i) = ik_seed_state[i];
357  }
358 
359  int ik_valid = pr2_arm_ik_solver_->cartToJntSearch(jnt_pos_in, pose_desired, jnt_pos_out, timeout);
360  if (ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
361  {
362  error_code.val = error_code.NO_IK_SOLUTION;
363  return false;
364  }
365 
366  if (ik_valid >= 0)
367  {
368  solution.resize(dimension_);
369  for (int i = 0; i < dimension_; ++i)
370  {
371  solution[i] = jnt_pos_out(i);
372  }
373  error_code.val = error_code.SUCCESS;
374  return true;
375  }
376  else
377  {
378  RCLCPP_WARN(LOGGER, "An IK solution could not be found");
379  error_code.val = error_code.NO_IK_SOLUTION;
380  return false;
381  }
382 }
383 
384 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& /*ik_pose*/,
385  const std::vector<double>& /*ik_seed_state*/, double /*timeout*/,
386  const std::vector<double>& /*consistency_limit*/,
387  std::vector<double>& /*solution*/,
388  moveit_msgs::msg::MoveItErrorCodes& /*error_code*/,
389  const kinematics::KinematicsQueryOptions& /*options*/) const
390 {
391  return false;
392 }
393 
394 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& /*ik_pose*/,
395  const std::vector<double>& /*ik_seed_state*/, double /*timeout*/,
396  std::vector<double>& /*solution*/,
397  const IKCallbackFn& /*solution_callback*/,
398  moveit_msgs::msg::MoveItErrorCodes& /*error_code*/,
399  const kinematics::KinematicsQueryOptions& /*options*/) const
400 {
401  return false;
402 }
403 
404 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& /*ik_pose*/,
405  const std::vector<double>& /*ik_seed_state*/, double /*timeout*/,
406  const std::vector<double>& /*consistency_limit*/,
407  std::vector<double>& /*solution*/,
408  const IKCallbackFn& /*solution_callback*/,
409  moveit_msgs::msg::MoveItErrorCodes& /*error_code*/,
410  const kinematics::KinematicsQueryOptions& /*options*/) const
411 {
412  return false;
413 }
414 
415 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string>& /*link_names*/,
416  const std::vector<double>& /*joint_angles*/,
417  std::vector<geometry_msgs::msg::Pose>& /*poses*/) const
418 {
419  return false;
420 }
421 
422 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
423 {
424  if (!active_)
425  {
426  RCLCPP_ERROR(LOGGER, "kinematics not active");
427  }
428  return ik_solver_info_.joint_names;
429 }
430 
431 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
432 {
433  if (!active_)
434  {
435  RCLCPP_ERROR(LOGGER, "kinematics not active");
436  }
437  return fk_solver_info_.link_names;
438 }
439 
440 } // namespace pr2_arm_kinematics
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)
static const rclcpp::Logger LOGGER
rclcpp::Node::SharedPtr node_
std::vector< std::string > tip_frames_
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
Definition: robot_model.h:106
int cartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, double timeout)
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
bool active_
Indicates whether the solver has been successfully initialized.
void computeIKShoulderRoll(const Eigen::Isometry3f &g_in, 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:466
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:58
void computeIKShoulderPan(const Eigen::Isometry3f &g_in, 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:201
moveit_msgs::msg::KinematicSolverInfo solver_info_
get chain information about the arm.
Definition: pr2_arm_ik.h:166
bool isActive()
Specifies if the node is active or not.
moveit_msgs::msg::KinematicSolverInfo ik_solver_info_
const std::vector< std::string > & getJointNames() const override
Return all the joint names in the order they are used internally.
bool initialize(const rclcpp::Node::SharedPtr &node, 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.
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
moveit_msgs::msg::KinematicSolverInfo fk_solver_info_
bool getPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::msg::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.
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::msg::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....
bool getKDLChain(const urdf::ModelInterface &model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::msg::KinematicSolverInfo &chain_info)
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
Eigen::Isometry3f kdlToEigenMatrix(const KDL::Frame &p)
FilterFn chain(const std::vector< FilterFn > &filter_functions)
A set of options for the kinematics solver.