Public Member Functions | Private Member Functions | Private Attributes | List of all members
nextage_left_arm_ikfast_kinematics_plugin::IKFastKinematicsPlugin Class Reference
Inheritance diagram for nextage_left_arm_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...
 
 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...
 
- 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)
 
virtual bool setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices)
 
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...
 
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)
 
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_
 
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 116 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

Constructor & Destructor Documentation

nextage_left_arm_ikfast_kinematics_plugin::IKFastKinematicsPlugin::IKFastKinematicsPlugin ( )
inline

Definition at line 135 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

Member Function Documentation

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

Definition at line 545 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

void nextage_left_arm_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 521 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 551 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

const std::vector<std::string>& nextage_left_arm_ikfast_kinematics_plugin::IKFastKinematicsPlugin::getJointNames ( ) const
inlineprivatevirtual
const std::vector<std::string>& nextage_left_arm_ikfast_kinematics_plugin::IKFastKinematicsPlugin::getLinkNames ( ) const
inlineprivatevirtual
bool nextage_left_arm_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.

This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node, otherwise ROS TF is used to calculate the forward kinematics

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 587 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

bool nextage_left_arm_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 899 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

void nextage_left_arm_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 457 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 475 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

bool nextage_left_arm_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 nextage_left_arm_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 629 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

bool nextage_left_arm_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 649 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

bool nextage_left_arm_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 668 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

bool nextage_left_arm_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 687 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

int nextage_left_arm_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 384 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

Member Data Documentation

bool nextage_left_arm_ikfast_kinematics_plugin::IKFastKinematicsPlugin::active_
private

Definition at line 125 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 124 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 121 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 120 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 119 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 118 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

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

Definition at line 122 of file nextage_left_arm_ikfast_moveit_plugin.cpp.

size_t nextage_left_arm_ikfast_kinematics_plugin::IKFastKinematicsPlugin::num_joints_
private

Definition at line 123 of file nextage_left_arm_ikfast_moveit_plugin.cpp.


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


nextage_ik_plugin
Author(s): TORK Developer 534o <534o@opensouce-robotics.tokyo.jp>
autogenerated on Wed Jun 17 2020 04:14:37