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 */

#ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
#define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_

#include <geometry_msgs/PoseStamped.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit/macros/class_forward.h>
#include <ros/node_handle.h>

#include <boost/function.hpp>
#include <string>

namespace moveit
{
namespace core
{
class JointModelGroup;
class RobotState;
}
}

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 
};
}
typedef DiscretizationMethods::DiscretizationMethod 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                          
};
}
typedef KinematicErrors::KinematicError 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 ration of solutions found over solutions searched.
 *
 */
struct KinematicsResult
{
  KinematicError kinematic_error; 
  double solution_percentage;     
};

MOVEIT_CLASS_FORWARD(KinematicsBase);

class KinematicsBase
{
public:
  static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
  static const double DEFAULT_TIMEOUT;               /* = 1.0 */

  typedef boost::function<void(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_solution,
                               moveit_msgs::MoveItErrorCodes& error_code)>
      IKCallbackFn;

  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 = 0;

  virtual bool getPositionIK(const std::vector<geometry_msgs::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::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 = 0;

  virtual bool
  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
                   const std::vector<double>& consistency_limits, std::vector<double>& solution,
                   moveit_msgs::MoveItErrorCodes& error_code,
                   const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

  virtual bool
  searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
                   std::vector<double>& solution, const IKCallbackFn& solution_callback,
                   moveit_msgs::MoveItErrorCodes& error_code,
                   const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

  virtual bool
  searchPositionIK(const geometry_msgs::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::MoveItErrorCodes& error_code,
                   const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;

  virtual bool
  searchPositionIK(const std::vector<geometry_msgs::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::MoveItErrorCodes& error_code,
                   const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                   const moveit::core::RobotState* context_state = NULL) const
  {
    // 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
    ROS_ERROR_NAMED("kinematics_base", "This kinematic solver does not support searchPositionIK with multiple poses");
    return false;
  }

  virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
                             std::vector<geometry_msgs::Pose>& poses) const = 0;

  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);

  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 std::string& robot_description, const std::string& group_name,
                          const std::string& base_frame, const std::string& tip_frame,
                          double search_discretization) = 0;

  virtual bool initialize(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)
  {
    // For IK solvers that do not support multiple tip frames, fall back to single pose call
    if (tip_frames.size() == 1)
    {
      return initialize(robot_description, group_name, base_frame, tip_frames[0], search_discretization);
    }

    ROS_ERROR_NAMED("kinematics_base", "This kinematic solver does not support initialization "
                                       "with more than one tip frames");
    return false;
  }

  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)
      ROS_ERROR_NAMED("kinematics_base", "This kinematic solver has more than one tip frame, "
                                         "do not call getTipFrame()");

    return tip_frame_;  // for backwards-compatibility. should actually use 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 = NULL) const;

  void setSearchDiscretization(double sd)
  {
    redundant_joint_discretization_.clear();
    for (std::vector<unsigned int>::iterator i = redundant_joint_indices_.begin(); i != redundant_joint_indices_.end();
         i++)
    {
      redundant_joint_discretization_[*i] = sd;
    }
  }

  void setSearchDiscretization(const std::map<int, double>& discretization)
  {
    redundant_joint_discretization_.clear();
    redundant_joint_indices_.clear();
    for (std::map<int, double>::const_iterator i = discretization.begin(); i != discretization.end(); i++)
    {
      redundant_joint_discretization_.insert(*i);
      redundant_joint_indices_.push_back(i->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()
    : tip_frame_("DEPRECATED")
    ,  // help users understand why this variable might not be set
       // (if multiple tip frames provided, this variable will be unset)
    search_discretization_(DEFAULT_SEARCH_DISCRETIZATION)
    , default_timeout_(DEFAULT_TIMEOUT)
  {
    supported_methods_.push_back(DiscretizationMethods::NO_DISCRETIZATION);
  }

protected:
  std::string robot_description_;
  std::string group_name_;
  std::string base_frame_;
  std::vector<std::string> tip_frames_;
  std::string tip_frame_;  // DEPRECATED - this variable only still exists for backwards compatibility with
                           // previously generated custom ik solvers like IKFast

  double search_discretization_;  // DEPRECATED - this variable only still exists for backwards compatibility
                                  // with previous implementations.  Discretization values for each joint are
                                  // now stored in the redundant_joint_discretization_ member

  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 std::string& param, T& val, const T& default_val) const
  {
    ros::NodeHandle pnh("~");
    if (pnh.hasParam(group_name_ + "/" + param))
    {
      val = pnh.param(group_name_ + "/" + param, default_val);
      return true;
    }

    if (pnh.hasParam(param))
    {
      val = pnh.param(param, default_val);
      return true;
    }

    ros::NodeHandle nh;
    if (nh.hasParam("robot_description_kinematics/" + group_name_ + "/" + param))
    {
      val = nh.param("robot_description_kinematics/" + group_name_ + "/" + param, default_val);
      return true;
    }

    if (nh.hasParam("robot_description_kinematics/" + param))
    {
      val = nh.param("robot_description_kinematics/" + param, default_val);
      return true;
    }

    val = default_val;

    return false;
  }

private:
  std::string removeSlash(const std::string& str) const;
};
};

#endif