kinematics_base.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Sachin Chitta, Dave Coleman */
 
#pragma once
 
#include <geometry_msgs/msg/pose.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
#include <moveit/macros/class_forward.h>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
#include <string>
#include <functional>
 
#include <moveit_kinematics_base_export.h>
 
namespace moveit
{
namespace core
{
MOVEIT_CLASS_FORWARD(JointModelGroup);
MOVEIT_CLASS_FORWARD(RobotState);
MOVEIT_CLASS_FORWARD(RobotModel);
}  // namespace core
}  // namespace moveit
 
namespace kinematics
{
/*
 * @enum DiscretizationMethods
 *
 * @brief Flags for choosing the type discretization method applied on the redundant joints during an ik query
 */
namespace DiscretizationMethods
{
enum DiscretizationMethod
{
  NO_DISCRETIZATION = 1, 
  ALL_DISCRETIZED,       
  SOME_DISCRETIZED, 
  ALL_RANDOM_SAMPLED, 
  SOME_RANDOM_SAMPLED 
};
}  // namespace DiscretizationMethods
using DiscretizationMethod = DiscretizationMethods::DiscretizationMethod;
 
/*
 * @enum KinematicErrors
 * @brief Kinematic error codes that occur in a ik query
 */
namespace KinematicErrors
{
enum KinematicError
{
  OK = 1,                              
  UNSUPORTED_DISCRETIZATION_REQUESTED, 
  DISCRETIZATION_NOT_INITIALIZED,      
  MULTIPLE_TIPS_NOT_SUPPORTED,         
  EMPTY_TIP_POSES,                     
  IK_SEED_OUTSIDE_LIMITS,              
  SOLVER_NOT_ACTIVE,                   
  NO_SOLUTION                          
};
}  // namespace KinematicErrors
using KinematicError = KinematicErrors::KinematicError;
 
struct KinematicsQueryOptions
{
  KinematicsQueryOptions()
    : lock_redundant_joints(false)
    , return_approximate_solution(false)
    , discretization_method(DiscretizationMethods::NO_DISCRETIZATION)
  {
  }
 
  bool lock_redundant_joints;                 
  bool return_approximate_solution;           
  DiscretizationMethod discretization_method; 
};
 
/*
 * @struct KinematicsResult
 * @brief Reports result details of an ik query
 *
 * This struct is used as an output argument of the getPositionIK(...) method that returns multiple joint solutions.
 * It contains the type of error that led to a failure or KinematicErrors::OK when a set of joint solutions is found.
 * The solution percentage shall provide a ratio of solutions found over solutions searched.
 *
 */
struct KinematicsResult
{
  KinematicError kinematic_error; 
  double solution_percentage;     
};
 
MOVEIT_CLASS_FORWARD(KinematicsBase);  // Defines KinematicsBasePtr, ConstPtr, WeakPtr... etc
 
class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
{
public:
  static const rclcpp::Logger LOGGER;
  static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
  static const double DEFAULT_TIMEOUT;               /* = 1.0 */
 
  using IKCallbackFn = std::function<void(const geometry_msgs::msg::Pose&, const std::vector<double>&,
                                          moveit_msgs::msg::MoveItErrorCodes&)>;
 
  using IKCostFn = std::function<double(const geometry_msgs::msg::Pose&, const moveit::core::RobotState&,
                                        moveit::core::JointModelGroup const*, const std::vector<double>&)>;
 
  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;
 
  virtual bool getPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,
                             const std::vector<double>& ik_seed_state, std::vector<std::vector<double> >& solutions,
                             KinematicsResult& result, const kinematics::KinematicsQueryOptions& options) const;
 
  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;
 
  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;
 
  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;
 
  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;
 
  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
  {
    (void)context_state;
    // For IK solvers that do not support multiple poses, fall back to single pose call
    if (ik_poses.size() == 1)
    {
      // Check if a solution_callback function was provided and call the corresponding function
      if (solution_callback)
      {
        return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, solution_callback,
                                error_code, options);
      }
      else
      {
        return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, error_code, options);
      }
    }
 
    // Otherwise throw error because this function should have been implemented
    RCLCPP_ERROR(LOGGER, "This kinematic solver does not support searchPositionIK with multiple poses");
    return false;
  }
 
  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
  {
    // if cost function is empty, omit
    if (!cost_function)
    {
      return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback,
                              error_code, options, context_state);
    }
    RCLCPP_ERROR(LOGGER, "This kinematic solver does not support IK solution cost functions");
    return false;
  }
 
  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;
 
  virtual void setValues(const std::string& robot_description, const std::string& group_name,
                         const std::string& base_frame, const std::vector<std::string>& tip_frames,
                         double search_discretization);
 
  virtual 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);
 
  virtual const std::string& getGroupName() const
  {
    return group_name_;
  }
 
  virtual const std::string& getBaseFrame() const
  {
    return base_frame_;
  }
 
  virtual const std::string& getTipFrame() const
  {
    if (tip_frames_.size() > 1)
      RCLCPP_ERROR(LOGGER, "This kinematic solver has more than one tip frame, "
                           "do not call getTipFrame()");
 
    return tip_frames_[0];
  }
 
  virtual const std::vector<std::string>& getTipFrames() const
  {
    return tip_frames_;
  }
 
  virtual bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
 
  bool setRedundantJoints(const std::vector<std::string>& redundant_joint_names);
 
  virtual void getRedundantJoints(std::vector<unsigned int>& redundant_joint_indices) const
  {
    redundant_joint_indices = redundant_joint_indices_;
  }
 
  virtual const std::vector<std::string>& getJointNames() const = 0;
 
  virtual const std::vector<std::string>& getLinkNames() const = 0;
 
  virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = nullptr) const;
 
  void setSearchDiscretization(double sd)
  {
    redundant_joint_discretization_.clear();
    for (unsigned int index : redundant_joint_indices_)
      redundant_joint_discretization_[index] = sd;
  }
 
  void setSearchDiscretization(const std::map<int, double>& discretization)
  {
    redundant_joint_discretization_.clear();
    redundant_joint_indices_.clear();
    for (const auto& pair : discretization)
    {
      redundant_joint_discretization_.insert(pair);
      redundant_joint_indices_.push_back(pair.first);
    }
  }
 
  double getSearchDiscretization(int joint_index = 0) const
  {
    if (redundant_joint_discretization_.count(joint_index) > 0)
    {
      return redundant_joint_discretization_.at(joint_index);
    }
    else
    {
      return 0.0;  // returned when there aren't any redundant joints
    }
  }
 
  std::vector<DiscretizationMethod> getSupportedDiscretizationMethods() const
  {
    return supported_methods_;
  }
 
  void setDefaultTimeout(double timeout)
  {
    default_timeout_ = timeout;
  }
 
  double getDefaultTimeout() const
  {
    return default_timeout_;
  }
 
  virtual ~KinematicsBase();
 
  KinematicsBase();
 
protected:
  rclcpp::Node::SharedPtr node_;
  moveit::core::RobotModelConstPtr robot_model_;
  std::string robot_description_;
  std::string group_name_;
  std::string base_frame_;
  std::vector<std::string> tip_frames_;
 
  double default_timeout_;
  std::vector<unsigned int> redundant_joint_indices_;
  std::map<int, double> redundant_joint_discretization_;
  std::vector<DiscretizationMethod> supported_methods_;
 
  template <typename T>
  inline bool lookupParam(const rclcpp::Node::SharedPtr& node, const std::string& param, T& val,
                          const T& default_val) const
  {
    if (node->has_parameter({ group_name_ + "." + param }))
    {
      node->get_parameter(group_name_ + "." + param, val);
      return true;
    }
 
    if (node->has_parameter({ param }))
    {
      node->get_parameter(param, val);
      return true;
    }
 
    if (node->has_parameter({ "robot_description_kinematics." + group_name_ + "." + param }))
    {
      node->get_parameter("robot_description_kinematics." + group_name_ + "." + param, val);
      return true;
    }
 
    if (node->has_parameter("robot_description_kinematics." + param))
    {
      node->get_parameter("robot_description_kinematics." + param, val);
      return true;
    }
 
    val = default_val;
    return false;
  }
 
  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);
 
private:
  std::string removeSlash(const std::string& str) const;
};
}  // namespace kinematics