37 #ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ 38 #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_ 40 #include <geometry_msgs/Pose.h> 41 #include <moveit_msgs/MoveItErrorCodes.h> 45 #include <boost/function.hpp> 66 namespace DiscretizationMethods
86 namespace KinematicErrors
111 : lock_redundant_joints(false)
112 , return_approximate_solution(false)
152 typedef boost::function<void(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_solution,
153 moveit_msgs::MoveItErrorCodes& error_code)>
169 getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
170 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
194 virtual bool getPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
211 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
212 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
230 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
231 const std::vector<double>& consistency_limits, std::vector<double>& solution,
232 moveit_msgs::MoveItErrorCodes& error_code,
249 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
250 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
251 moveit_msgs::MoveItErrorCodes& error_code,
270 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
271 const std::vector<double>& consistency_limits, std::vector<double>& solution,
272 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
297 searchPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
298 double timeout,
const std::vector<double>& consistency_limits, std::vector<double>& solution,
299 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
304 if (ik_poses.size() == 1)
307 if (solution_callback)
309 return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, solution_callback,
314 return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, error_code,
options);
319 ROS_ERROR_NAMED(
"kinematics_base",
"This kinematic solver does not support searchPositionIK with multiple poses");
330 virtual bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
331 std::vector<geometry_msgs::Pose>& poses)
const = 0;
344 [[deprecated]]
virtual void setValues(
const std::string& robot_description,
const std::string& group_name,
345 const std::string& base_frame,
const std::string& tip_frame,
346 double search_discretization);
358 virtual void setValues(
const std::string& robot_description,
const std::string& group_name,
359 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
360 double search_discretization);
376 [[deprecated]]
virtual bool initialize(
const std::string& robot_description,
const std::string& group_name,
377 const std::string& base_frame,
const std::string& tip_frame,
378 double search_discretization);
394 virtual bool initialize(
const std::string& robot_description,
const std::string& group_name,
395 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
396 double search_discretization);
413 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
414 double search_discretization);
442 if (tip_frames_.size() > 1)
443 ROS_ERROR_NAMED(
"kinematics_base",
"This kinematic solver has more than one tip frame, " 444 "do not call getTipFrame()");
446 return tip_frames_[0];
467 virtual bool setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices);
475 bool setRedundantJoints(
const std::vector<std::string>& redundant_joint_names);
482 redundant_joint_indices = redundant_joint_indices_;
488 virtual const std::vector<std::string>& getJointNames()
const = 0;
493 virtual const std::vector<std::string>& getLinkNames()
const = 0;
518 redundant_joint_discretization_.clear();
519 for (
unsigned int index : redundant_joint_indices_)
520 redundant_joint_discretization_[
index] = sd;
532 redundant_joint_discretization_.clear();
533 redundant_joint_indices_.clear();
534 for (std::map<int, double>::const_iterator i = discretization.begin(); i != discretization.end(); i++)
536 redundant_joint_discretization_.insert(*i);
537 redundant_joint_indices_.push_back(i->first);
546 if (redundant_joint_discretization_.count(joint_index) > 0)
548 return redundant_joint_discretization_.at(joint_index);
562 return supported_methods_;
569 default_timeout_ = timeout;
576 return default_timeout_;
617 template <
typename T>
618 inline bool lookupParam(
const std::string& param, T& val,
const T& default_val)
const 621 if (pnh.
hasParam(group_name_ +
"/" + param))
623 val = pnh.
param(group_name_ +
"/" + param, default_val);
629 val = pnh.
param(param, default_val);
634 if (nh.
hasParam(
"robot_description_kinematics/" + group_name_ +
"/" + param))
636 val = nh.
param(
"robot_description_kinematics/" + group_name_ +
"/" + param, default_val);
640 if (nh.
hasParam(
"robot_description_kinematics/" + param))
642 val = nh.
param(
"robot_description_kinematics/" + param, default_val);
660 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
661 double search_discretization);
664 std::string removeSlash(
const std::string& str)
const;
std::vector< unsigned int > redundant_joint_indices_
KinematicError kinematic_error
A set of options for the kinematics solver.
Core components of MoveIt!
void setDefaultTimeout(double timeout)
For functions that require a timeout specified but one is not specified using arguments, a default timeout is used, as set by this function (and initialized to KinematicsBase::DEFAULT_TIMEOUT)
ROSCONSOLE_DECL void initialize()
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...
void setSearchDiscretization(double sd)
Set the search discretization value for all the redundant joints.
std::vector< std::string > tip_frames_
moveit::core::RobotModelConstPtr robot_model_
double getSearchDiscretization(int joint_index=0) const
Get the value of the search discretization.
double solution_percentage
static const double DEFAULT_TIMEOUT
virtual void getRedundantJoints(std::vector< unsigned int > &redundant_joint_indices) const
Get the set of redundant joints.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
bool lock_redundant_joints
virtual const std::string & getBaseFrame() const
Return the name of the frame in which the solver is operating. This is usually a link name...
DiscretizationMethods::DiscretizationMethod DiscretizationMethod
bool return_approximate_solution
Provides an interface for kinematics solvers.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
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
Given a set of desired poses for a planning group with multiple end-effectors, search for the joint a...
MOVEIT_CLASS_FORWARD(RobotModel)
bool lookupParam(const std::string ¶m, T &val, const T &default_val) const
Enables kinematics plugins access to parameters that are defined for the private namespace and inside...
double getDefaultTimeout() const
For functions that require a timeout specified but one is not specified using arguments, this default timeout is used.
DiscretizationMethod discretization_method
std::vector< DiscretizationMethod > supported_methods_
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...
std::vector< DiscretizationMethod > getSupportedDiscretizationMethods() const
Returns the set of supported kinematics discretization search types. This implementation only support...
bool hasParam(const std::string &key) const
std::map< int, double > redundant_joint_discretization_
static const double DEFAULT_SEARCH_DISCRETIZATION
void setSearchDiscretization(const std::map< int, double > &discretization)
Sets individual discretization values for each redundant joint.
API for forward and inverse kinematics.
double search_discretization_
virtual const std::string & getGroupName() const
Return the name of the group that the solver is operating on.
#define ROS_ERROR_NAMED(name,...)
Representation of a robot's state. This includes position, velocity, acceleration and effort...
Main namespace for MoveIt!
std::string robot_description_
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. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used.
KinematicErrors::KinematicError KinematicError