Public Member Functions | Private Member Functions | Private Attributes | List of all members
ikfast_kinematics_plugin::IKFastKinematicsPlugin Class Reference
Inheritance diagram for ikfast_kinematics_plugin::IKFastKinematicsPlugin:
Inheritance graph
[legend]

Public Member Functions

bool getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
 Given a set of joint angles and a set of links, compute their pose. More...
 
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
 Given a desired pose of the end-effector, compute the joint angles to reach it. More...
 
bool getPositionIK (const std::vector< geometry_msgs::Pose > &ik_poses, const std::vector< double > &ik_seed_state, std::vector< std::vector< double >> &solutions, kinematics::KinematicsResult &result, const kinematics::KinematicsQueryOptions &options) const
 Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. More...
 
 IKFastKinematicsPlugin ()
 
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
 Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More...
 
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
 Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More...
 
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
 Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More...
 
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
 Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). The consistency_limit specifies that only certain redundancy positions around those specified in the seed state are admissible and need to be searched. More...
 
bool setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices)
 Overrides the default method to prevent changing the redundant joints. More...
 
void setSearchDiscretization (const std::map< int, double > &discretization)
 Sets the discretization value for the redundant joint. More...
 
- Public Member Functions inherited from kinematics::KinematicsBase
virtual const std::string & getBaseFrame () const
 
double getDefaultTimeout () const
 
virtual const std::string & getGroupName () const
 
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 void getRedundantJoints (std::vector< unsigned int > &redundant_joint_indices) const
 
double getSearchDiscretization (int joint_index=0) const
 
std::vector< DiscretizationMethodgetSupportedDiscretizationMethods () const
 
virtual const std::string & getTipFrame () const
 
virtual const std::vector< std::string > & getTipFrames () const
 
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)
 
 KinematicsBase ()
 
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
 
void setDefaultTimeout (double timeout)
 
bool setRedundantJoints (const std::vector< std::string > &redundant_joint_names)
 
void setSearchDiscretization (const std::map< int, double > &discretization)
 
void setSearchDiscretization (double sd)
 
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 supportsGroup (const moveit::core::JointModelGroup *jmg, std::string *error_text_out=NULL) const
 
virtual ~KinematicsBase ()
 

Private Member Functions

void fillFreeParams (int count, int *array)
 
void getClosestSolution (const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
 
bool getCount (int &count, const int &max_count, const int &min_count) const
 
const std::vector< std::string > & getJointNames () const
 
const std::vector< std::string > & getLinkNames () const
 
void getSolution (const IkSolutionList< IkReal > &solutions, int i, std::vector< double > &solution) const
 Gets a specific solution from the set. More...
 
void getSolution (const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, int i, std::vector< double > &solution) const
 Gets a specific solution from the set with joints rotated 360° to be near seed state where possible. More...
 
double harmonize (const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
 
bool initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
 
bool sampleRedundantJoint (kinematics::DiscretizationMethod method, std::vector< double > &sampled_joint_vals) const
 samples the designated redundant joint using the chosen discretization method More...
 
int solve (KDL::Frame &pose_frame, const std::vector< double > &vfree, IkSolutionList< IkReal > &solutions) const
 Calls the IK solver from IKFast. More...
 

Private Attributes

bool active_
 
std::vector< int > free_params_
 
std::vector< bool > joint_has_limits_vector_
 
std::vector< double > joint_max_vector_
 
std::vector< double > joint_min_vector_
 
std::vector< std::string > joint_names_
 
std::vector< std::string > link_names_
 
const std::string name_ { "ikfast" }
 
const size_t num_joints_
 

Additional Inherited Members

- Public Types inherited from kinematics::KinematicsBase
typedef boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
 
- Static Public Attributes inherited from kinematics::KinematicsBase
static const double DEFAULT_SEARCH_DISCRETIZATION
 
static const double DEFAULT_TIMEOUT
 
- Protected Member Functions inherited from kinematics::KinematicsBase
bool lookupParam (const std::string &param, T &val, const T &default_val) const
 
- Protected Attributes inherited from kinematics::KinematicsBase
std::string base_frame_
 
double default_timeout_
 
std::string group_name_
 
std::map< int, double > redundant_joint_discretization_
 
std::vector< unsigned int > redundant_joint_indices_
 
std::string robot_description_
 
double search_discretization_
 
std::vector< DiscretizationMethodsupported_methods_
 
std::string tip_frame_
 
std::vector< std::string > tip_frames_
 

Detailed Description

Definition at line 154 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

Constructor & Destructor Documentation

ikfast_kinematics_plugin::IKFastKinematicsPlugin::IKFastKinematicsPlugin ( )
inline

Definition at line 179 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

Member Function Documentation

void ikfast_kinematics_plugin::IKFastKinematicsPlugin::fillFreeParams ( int  count,
int *  array 
)
private

Definition at line 706 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

void ikfast_kinematics_plugin::IKFastKinematicsPlugin::getClosestSolution ( const IkSolutionList< IkReal > &  solutions,
const std::vector< double > &  ik_seed_state,
std::vector< double > &  solution 
) const
private

Definition at line 678 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::getCount ( int &  count,
const int &  max_count,
const int &  min_count 
) const
private

Definition at line 713 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

const std::vector<std::string>& ikfast_kinematics_plugin::IKFastKinematicsPlugin::getJointNames ( ) const
inlineprivatevirtual
const std::vector<std::string>& ikfast_kinematics_plugin::IKFastKinematicsPlugin::getLinkNames ( ) const
inlineprivatevirtual
bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::getPositionFK ( const std::vector< std::string > &  link_names,
const std::vector< double > &  joint_angles,
std::vector< geometry_msgs::Pose > &  poses 
) const
virtual

Given a set of joint angles and a set of links, compute their pose.

Parameters
link_namesA set of links for which FK needs to be computed
joint_anglesThe state for which FK is being computed
posesThe resultant set of poses (in the frame returned by getBaseFrame())
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 749 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::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

Given a desired pose of the end-effector, compute the joint angles to reach it.

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
solutionthe solution vector
error_codean error code that encodes the reason for failure or success
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 1074 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::getPositionIK ( const std::vector< geometry_msgs::Pose > &  ik_poses,
const std::vector< double > &  ik_seed_state,
std::vector< std::vector< double >> &  solutions,
kinematics::KinematicsResult result,
const kinematics::KinematicsQueryOptions options 
) const

Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it.

This is a default implementation that returns only one solution and so its result is equivalent to calling 'getPositionIK(...)' with a zero initialized seed.

Parameters
ik_posesThe desired pose of each tip link
ik_seed_statean initial guess solution for the inverse kinematics
solutionsA vector of vectors where each entry is a valid joint solution
resultA struct that reports the results of the query
optionsAn option struct which contains the type of redundancy discretization used. This default implementation only supports the KinmaticSearches::NO_DISCRETIZATION method; requesting any other will result in failure.
Returns
True if a valid set of solutions was found, false otherwise.

Definition at line 1181 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

void ikfast_kinematics_plugin::IKFastKinematicsPlugin::getSolution ( const IkSolutionList< IkReal > &  solutions,
int  i,
std::vector< double > &  solution 
) const
private

Gets a specific solution from the set.

Definition at line 577 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

void ikfast_kinematics_plugin::IKFastKinematicsPlugin::getSolution ( const IkSolutionList< IkReal > &  solutions,
const std::vector< double > &  ik_seed_state,
int  i,
std::vector< double > &  solution 
) const
private

Gets a specific solution from the set with joints rotated 360° to be near seed state where possible.

Definition at line 596 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

double ikfast_kinematics_plugin::IKFastKinematicsPlugin::harmonize ( const std::vector< double > &  ik_seed_state,
std::vector< double > &  solution 
) const
private

Definition at line 628 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::initialize ( const std::string &  robot_description,
const std::string &  group_name,
const std::string &  base_name,
const std::string &  tip_name,
double  search_discretization 
)
privatevirtual
bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::sampleRedundantJoint ( kinematics::DiscretizationMethod  method,
std::vector< double > &  sampled_joint_vals 
) const
private

samples the designated redundant joint using the chosen discretization method

Parameters
methodAn enumeration flag indicating the discretization method to be used
sampled_joint_valsSampled joint values for the redundant joint
Returns
True if sampling succeeded.

Definition at line 1324 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::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

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines).

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 805 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::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

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines).

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
thedistance that the redundancy can be from the current position
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 817 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::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

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines).

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 828 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::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

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). The consistency_limit specifies that only certain redundancy positions around those specified in the seed state are admissible and need to be searched.

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
consistency_limitthe distance that the redundancy can be from the current position
Returns
True if a valid solution was found, false otherwise

search_mode is currently fixed during code generation

Implements kinematics::KinematicsBase.

Definition at line 839 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::setRedundantJoints ( const std::vector< unsigned int > &  redundant_joint_indices)
virtual

Overrides the default method to prevent changing the redundant joints.

Reimplemented from kinematics::KinematicsBase.

Definition at line 493 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

void ikfast_kinematics_plugin::IKFastKinematicsPlugin::setSearchDiscretization ( const std::map< int, double > &  discretization)

Sets the discretization value for the redundant joint.

Since this ikfast implementation allows for one redundant joint then only the first entry will be in the discretization map will be used. Calling this method replaces previous discretization settings.

Parameters
discretizationa map of joint indices and discretization value pairs.

Definition at line 460 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

int ikfast_kinematics_plugin::IKFastKinematicsPlugin::solve ( KDL::Frame pose_frame,
const std::vector< double > &  vfree,
IkSolutionList< IkReal > &  solutions 
) const
private

Calls the IK solver from IKFast.

Returns
The number of solutions found

Definition at line 499 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

Member Data Documentation

bool ikfast_kinematics_plugin::IKFastKinematicsPlugin::active_
private

Definition at line 163 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

std::vector<int> ikfast_kinematics_plugin::IKFastKinematicsPlugin::free_params_
private

Definition at line 162 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

std::vector<bool> ikfast_kinematics_plugin::IKFastKinematicsPlugin::joint_has_limits_vector_
private

Definition at line 159 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

std::vector<double> ikfast_kinematics_plugin::IKFastKinematicsPlugin::joint_max_vector_
private

Definition at line 158 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

std::vector<double> ikfast_kinematics_plugin::IKFastKinematicsPlugin::joint_min_vector_
private

Definition at line 157 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

std::vector<std::string> ikfast_kinematics_plugin::IKFastKinematicsPlugin::joint_names_
private

Definition at line 156 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

std::vector<std::string> ikfast_kinematics_plugin::IKFastKinematicsPlugin::link_names_
private

Definition at line 160 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

const std::string ikfast_kinematics_plugin::IKFastKinematicsPlugin::name_ { "ikfast" }
private

Definition at line 164 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.

const size_t ikfast_kinematics_plugin::IKFastKinematicsPlugin::num_joints_
private

Definition at line 161 of file turtlebot_arm_arm_ikfast_moveit_plugin.cpp.


The documentation for this class was generated from the following file:


turtlebot_arm_ikfast_plugin
Author(s):
autogenerated on Fri Feb 7 2020 03:56:27