moveit2
The MoveIt Motion Planning Framework for ROS 2.
kinematics_base.hpp
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 
37 #pragma once
38 
39 #include <geometry_msgs/msg/pose.hpp>
40 #include <moveit_msgs/msg/move_it_error_codes.hpp>
42 #include <rclcpp/logging.hpp>
43 #include <rclcpp/node.hpp>
44 #include <string>
45 #include <functional>
46 #include <moveit/utils/logger.hpp>
47 
48 #include <moveit_kinematics_base_export.h>
49 
50 namespace moveit
51 {
52 namespace core
53 {
57 } // namespace core
58 } // namespace moveit
59 
61 namespace kinematics
62 {
63 /*
64  * @enum DiscretizationMethods
65  *
66  * @brief Flags for choosing the type discretization method applied on the redundant joints during an ik query
67  */
68 namespace DiscretizationMethods
69 {
71 {
80 };
81 } // namespace DiscretizationMethods
83 
84 /*
85  * @enum KinematicErrors
86  * @brief Kinematic error codes that occur in a ik query
87  */
88 namespace KinematicErrors
89 {
91 {
92  OK = 1,
100  NO_SOLUTION
102 };
103 } // namespace KinematicErrors
105 
111 {
113  : lock_redundant_joints(false)
115  , discretization_method(DiscretizationMethods::NO_DISCRETIZATION)
116  {
117  }
118 
123 };
124 
125 /*
126  * @struct KinematicsResult
127  * @brief Reports result details of an ik query
128  *
129  * This struct is used as an output argument of the getPositionIK(...) method that returns multiple joint solutions.
130  * It contains the type of error that led to a failure or KinematicErrors::OK when a set of joint solutions is found.
131  * The solution percentage shall provide a ratio of solutions found over solutions searched.
132  *
133  */
135 {
139 };
140 
141 MOVEIT_CLASS_FORWARD(KinematicsBase); // Defines KinematicsBasePtr, ConstPtr, WeakPtr... etc
142 
147 class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
148 {
149 public:
150  static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
151  static const double DEFAULT_TIMEOUT; /* = 1.0 */
152 
154  using IKCallbackFn = std::function<void(const geometry_msgs::msg::Pose&, const std::vector<double>&,
155  moveit_msgs::msg::MoveItErrorCodes&)>;
156 
158  using IKCostFn = std::function<double(const geometry_msgs::msg::Pose&, const moveit::core::RobotState&,
159  const moveit::core::JointModelGroup*, const std::vector<double>&)>;
160 
173  virtual bool
174  getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
175  std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
177 
199  virtual bool getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,
200  const std::vector<double>& ik_seed_state, std::vector<std::vector<double> >& solutions,
202 
215  virtual bool
216  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
217  std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
219 
234  virtual bool
235  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
236  const std::vector<double>& consistency_limits, std::vector<double>& solution,
237  moveit_msgs::msg::MoveItErrorCodes& error_code,
239 
253  virtual bool
254  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
255  std::vector<double>& solution, const IKCallbackFn& solution_callback,
256  moveit_msgs::msg::MoveItErrorCodes& error_code,
258 
274  virtual bool
275  searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
276  const std::vector<double>& consistency_limits, std::vector<double>& solution,
277  const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
279 
301  virtual bool
302  searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
303  double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
304  const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
306  const moveit::core::RobotState* context_state = nullptr) const
307  {
308  (void)context_state;
309  // For IK solvers that do not support multiple poses, fall back to single pose call
310  if (ik_poses.size() == 1)
311  {
312  // Check if a solution_callback function was provided and call the corresponding function
313  if (solution_callback)
314  {
315  return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, solution_callback,
316  error_code, options);
317  }
318  else
319  {
320  return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, error_code, options);
321  }
322  }
323 
324  // Otherwise throw error because this function should have been implemented
325  RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"),
326  "This kinematic solver does not support searchPositionIK with multiple poses");
327  return false;
328  }
329 
352  virtual bool
353  searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
354  double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
355  const IKCallbackFn& solution_callback, const IKCostFn& cost_function,
356  moveit_msgs::msg::MoveItErrorCodes& error_code,
358  const moveit::core::RobotState* context_state = nullptr) const
359  {
360  // if cost function is empty, omit
361  if (!cost_function)
362  {
363  return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback,
364  error_code, options, context_state);
365  }
366  RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"),
367  "This kinematic solver does not support IK solution cost functions");
368  return false;
369  }
370 
378  virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
379  std::vector<geometry_msgs::msg::Pose>& poses) const = 0;
380 
391  virtual void setValues(const std::string& robot_description, const std::string& group_name,
392  const std::string& base_frame, const std::vector<std::string>& tip_frames,
393  double search_discretization);
394 
408  virtual bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model,
409  const std::string& group_name, const std::string& base_frame,
410  const std::vector<std::string>& tip_frames, double search_discretization);
411 
416  virtual const std::string& getGroupName() const
417  {
418  return group_name_;
419  }
420 
426  virtual const std::string& getBaseFrame() const
427  {
428  return base_frame_;
429  }
430 
436  virtual const std::string& getTipFrame() const
437  {
438  if (tip_frames_.size() > 1)
439  {
440  RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"),
441  "This kinematic solver has more than one tip frame, "
442  "do not call getTipFrame()");
443  }
444 
445  return tip_frames_[0];
446  }
447 
453  virtual const std::vector<std::string>& getTipFrames() const
454  {
455  return tip_frames_;
456  }
457 
466  virtual bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
467 
474  bool setRedundantJoints(const std::vector<std::string>& redundant_joint_names);
475 
479  virtual void getRedundantJoints(std::vector<unsigned int>& redundant_joint_indices) const
480  {
481  redundant_joint_indices = redundant_joint_indices_;
482  }
483 
487  virtual const std::vector<std::string>& getJointNames() const = 0;
488 
492  virtual const std::vector<std::string>& getLinkNames() const = 0;
493 
510  virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = nullptr) const;
511 
515  void setSearchDiscretization(double sd)
516  {
517  redundant_joint_discretization_.clear();
518  for (unsigned int index : redundant_joint_indices_)
519  redundant_joint_discretization_[index] = sd;
520  }
521 
529  void setSearchDiscretization(const std::map<int, double>& discretization)
530  {
531  redundant_joint_discretization_.clear();
532  redundant_joint_indices_.clear();
533  for (const auto& pair : discretization)
534  {
535  redundant_joint_discretization_.insert(pair);
536  redundant_joint_indices_.push_back(pair.first);
537  }
538  }
539 
543  double getSearchDiscretization(int joint_index = 0) const
544  {
545  if (redundant_joint_discretization_.count(joint_index) > 0)
546  {
547  return redundant_joint_discretization_.at(joint_index);
548  }
549  else
550  {
551  return 0.0; // returned when there aren't any redundant joints
552  }
553  }
554 
559  std::vector<DiscretizationMethod> getSupportedDiscretizationMethods() const
560  {
561  return supported_methods_;
562  }
563 
566  void setDefaultTimeout(double timeout)
567  {
568  default_timeout_ = timeout;
569  }
570 
573  double getDefaultTimeout() const
574  {
575  return default_timeout_;
576  }
577 
581  virtual ~KinematicsBase();
582 
583  KinematicsBase();
584 
585 protected:
586  rclcpp::Node::SharedPtr node_;
587  moveit::core::RobotModelConstPtr robot_model_;
588  std::string robot_description_;
589  std::string group_name_;
590  std::string base_frame_;
591  std::vector<std::string> tip_frames_;
592 
594  std::vector<unsigned int> redundant_joint_indices_;
595  std::map<int, double> redundant_joint_discretization_;
596  std::vector<DiscretizationMethod> supported_methods_;
597 
606  void storeValues(const moveit::core::RobotModel& robot_model, const std::string& group_name,
607  const std::string& base_frame, const std::vector<std::string>& tip_frames,
608  double search_discretization);
609 
610 private:
611  std::string removeSlash(const std::string& str) const;
612 };
613 } // namespace kinematics
Provides an interface for kinematics solvers.
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const =0
Given a set of joint angles and a set of links, compute their pose.
virtual bool searchPositionIK(const std::vector< geometry_msgs::msg::Pose > &ik_poses, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, const IKCostFn &cost_function, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const
Given a set of desired poses for a planning group with multiple end-effectors, search for the joint a...
virtual const std::vector< std::string > & getJointNames() const =0
Return all the joint names in the order they are used internally.
virtual bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const =0
Given a desired pose of the end-effector, search for the joint angles required to reach it....
void setSearchDiscretization(double sd)
Set the search discretization value for all the redundant joints.
std::vector< DiscretizationMethod > getSupportedDiscretizationMethods() const
Returns the set of supported kinematics discretization search types. This implementation only support...
virtual void getRedundantJoints(std::vector< unsigned int > &redundant_joint_indices) const
Get the set of redundant joints.
virtual bool searchPositionIK(const std::vector< geometry_msgs::msg::Pose > &ik_poses, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const
Given a set of desired poses for a planning group with multiple end-effectors, search for the joint a...
virtual const std::vector< std::string > & getLinkNames() const =0
Return all the link names in the order they are represented internally.
std::map< int, double > redundant_joint_discretization_
std::vector< unsigned int > redundant_joint_indices_
virtual const std::string & getTipFrame() const
Return the name of the tip frame of the chain on which the solver is operating. This is usually a lin...
virtual const std::string & getBaseFrame() const
Return the name of the frame in which the solver is operating. This is usually a link name....
std::function< double(const geometry_msgs::msg::Pose &, const moveit::core::RobotState &, const moveit::core::JointModelGroup *, const std::vector< double > &)> IKCostFn
Signature for a cost function used to evaluate IK solutions.
std::vector< DiscretizationMethod > supported_methods_
void setDefaultTimeout(double timeout)
For functions that require a timeout specified but one is not specified using arguments,...
virtual 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 =0
Given a desired pose of the end-effector, search for the joint angles required to reach it....
virtual 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 =0
Given a desired pose of the end-effector, compute the joint angles to reach it.
double getSearchDiscretization(int joint_index=0) const
Get the value of the search discretization.
virtual bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const =0
Given a desired pose of the end-effector, search for the joint angles required to reach it....
rclcpp::Node::SharedPtr node_
virtual ~KinematicsBase()
Virtual destructor for the interface.
virtual const std::vector< std::string > & getTipFrames() const
Return the names of the tip frames of the kinematic tree on which the solver is operating....
std::vector< std::string > tip_frames_
virtual bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const =0
Given a desired pose of the end-effector, search for the joint angles required to reach it....
static const double DEFAULT_TIMEOUT
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.
moveit::core::RobotModelConstPtr robot_model_
double getDefaultTimeout() const
For functions that require a timeout specified but one is not specified using arguments,...
void setSearchDiscretization(const std::map< int, double > &discretization)
Sets individual discretization values for each redundant joint.
static const double DEFAULT_SEARCH_DISCRETIZATION
virtual const std::string & getGroupName() const
Return the name of the group that the solver is operating on.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.hpp:76
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.hpp:90
API for forward and inverse kinematics.
MOVEIT_CLASS_FORWARD(KinematicsBase)
MOVEIT_CLASS_FORWARD(JointModelGroup)
Main namespace for MoveIt.
Definition: exceptions.hpp:43
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition: logger.cpp:79
A set of options for the kinematics solver.
DiscretizationMethod discretization_method