Specific implementation of kinematics using ROS service calls to communicate with external IK solvers. This version can be used with any robot. Supports non-chain kinematic groups. More...
#include <srv_kinematics_plugin.h>
Public Member Functions | |
const std::vector< std::string > & | getJointNames () const |
Return all the joint names in the order they are used internally. | |
const std::vector< std::string > & | getLinkNames () const |
Return all the link names in the order they are represented internally. | |
virtual bool | getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const |
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 |
const std::vector< std::string > & | getVariableNames () const |
Return all the variable names in the order they are represented internally. | |
virtual bool | initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_frame, double search_discretization) |
virtual bool | initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::vector< std::string > &tip_frames, double search_discretization) |
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 |
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 |
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 |
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 |
SrvKinematicsPlugin () | |
Default constructor. | |
Protected Member Functions | |
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 std::vector< double > &consistency_limits, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) 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 |
virtual bool | setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices) |
Private Member Functions | |
int | getJointIndex (const std::string &name) const |
bool | isRedundantJoint (unsigned int index) const |
bool | timedOut (const ros::WallTime &start_time, double duration) const |
Private Attributes | |
bool | active_ |
unsigned int | dimension_ |
moveit_msgs::KinematicSolverInfo | ik_group_info_ |
boost::shared_ptr < ros::ServiceClient > | ik_service_client_ |
robot_model::JointModelGroup * | joint_model_group_ |
int | num_possible_redundant_joints_ |
robot_model::RobotModelPtr | robot_model_ |
robot_state::RobotStatePtr | robot_state_ |
Specific implementation of kinematics using ROS service calls to communicate with external IK solvers. This version can be used with any robot. Supports non-chain kinematic groups.
Definition at line 69 of file srv_kinematics_plugin.h.
Default constructor.
Definition at line 57 of file srv_kinematics_plugin.cpp.
int srv_kinematics_plugin::SrvKinematicsPlugin::getJointIndex | ( | const std::string & | name | ) | const [private] |
Definition at line 175 of file srv_kinematics_plugin.cpp.
const std::vector< std::string > & srv_kinematics_plugin::SrvKinematicsPlugin::getJointNames | ( | ) | const [virtual] |
Return all the joint names in the order they are used internally.
Implements kinematics::KinematicsBase.
Definition at line 468 of file srv_kinematics_plugin.cpp.
const std::vector< std::string > & srv_kinematics_plugin::SrvKinematicsPlugin::getLinkNames | ( | ) | const [virtual] |
Return all the link names in the order they are represented internally.
Implements kinematics::KinematicsBase.
Definition at line 473 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::getPositionFK | ( | const std::vector< std::string > & | link_names, |
const std::vector< double > & | joint_angles, | ||
std::vector< geometry_msgs::Pose > & | poses | ||
) | const [virtual] |
Implements kinematics::KinematicsBase.
Definition at line 446 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::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 [virtual] |
Implements kinematics::KinematicsBase.
Definition at line 189 of file srv_kinematics_plugin.cpp.
const std::vector< std::string > & srv_kinematics_plugin::SrvKinematicsPlugin::getVariableNames | ( | ) | const |
Return all the variable names in the order they are represented internally.
Definition at line 478 of file srv_kinematics_plugin.cpp.
virtual bool srv_kinematics_plugin::SrvKinematicsPlugin::initialize | ( | const std::string & | robot_description, |
const std::string & | group_name, | ||
const std::string & | base_name, | ||
const std::string & | tip_frame, | ||
double | search_discretization | ||
) | [inline, virtual] |
Implements kinematics::KinematicsBase.
Definition at line 120 of file srv_kinematics_plugin.h.
bool srv_kinematics_plugin::SrvKinematicsPlugin::initialize | ( | const std::string & | robot_description, |
const std::string & | group_name, | ||
const std::string & | base_name, | ||
const std::vector< std::string > & | tip_frames, | ||
double | search_discretization | ||
) | [virtual] |
Reimplemented from kinematics::KinematicsBase.
Definition at line 61 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::isRedundantJoint | ( | unsigned int | index | ) | const [private] |
Definition at line 167 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::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 [virtual] |
Implements kinematics::KinematicsBase.
Definition at line 208 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::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 [virtual] |
Implements kinematics::KinematicsBase.
Definition at line 228 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::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 [virtual] |
Implements kinematics::KinematicsBase.
Definition at line 247 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::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 [virtual] |
Implements kinematics::KinematicsBase.
Definition at line 266 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::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 std::vector< double > & | consistency_limits, | ||
const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
) | const [protected, virtual] |
Definition at line 285 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::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 [protected, virtual] |
Definition at line 308 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::setRedundantJoints | ( | const std::vector< unsigned int > & | redundant_joint_indices | ) | [protected, virtual] |
Reimplemented from kinematics::KinematicsBase.
Definition at line 151 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::timedOut | ( | const ros::WallTime & | start_time, |
double | duration | ||
) | const [private] |
Definition at line 184 of file srv_kinematics_plugin.cpp.
bool srv_kinematics_plugin::SrvKinematicsPlugin::active_ [private] |
Definition at line 182 of file srv_kinematics_plugin.h.
unsigned int srv_kinematics_plugin::SrvKinematicsPlugin::dimension_ [private] |
Stores information for the inverse kinematics solver
Definition at line 186 of file srv_kinematics_plugin.h.
moveit_msgs::KinematicSolverInfo srv_kinematics_plugin::SrvKinematicsPlugin::ik_group_info_ [private] |
Internal variable that indicates whether solvers are configured and ready
Definition at line 184 of file srv_kinematics_plugin.h.
boost::shared_ptr<ros::ServiceClient> srv_kinematics_plugin::SrvKinematicsPlugin::ik_service_client_ [private] |
Definition at line 195 of file srv_kinematics_plugin.h.
robot_model::JointModelGroup* srv_kinematics_plugin::SrvKinematicsPlugin::joint_model_group_ [private] |
Definition at line 189 of file srv_kinematics_plugin.h.
Definition at line 193 of file srv_kinematics_plugin.h.
robot_model::RobotModelPtr srv_kinematics_plugin::SrvKinematicsPlugin::robot_model_ [private] |
Dimension of the group
Definition at line 188 of file srv_kinematics_plugin.h.
robot_state::RobotStatePtr srv_kinematics_plugin::SrvKinematicsPlugin::robot_state_ [private] |
Definition at line 191 of file srv_kinematics_plugin.h.