Public Member Functions | Protected Member Functions | Private Member Functions | Private Attributes | List of all members
lma_kinematics_plugin::LMAKinematicsPlugin Class Reference

Specific implementation of kinematics using Levenberg-Marquardt method available at KDL. This version can be used with any robot. More...

#include <lma_kinematics_plugin.h>

Inheritance diagram for lma_kinematics_plugin::LMAKinematicsPlugin:
Inheritance graph
[legend]

Public Member Functions

const std::vector< std::string > & getJointNames () const
 Return all the joint names in the order they are used internally. More...
 
const std::vector< std::string > & getLinkNames () const
 Return all the link names in the order they are represented internally. More...
 
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
 
virtual 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)
 
 LMAKinematicsPlugin ()
 Default constructor. More...
 
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
 
- 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 ()
 

Protected Member Functions

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
 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...
 
virtual bool setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices)
 
- Protected Member Functions inherited from kinematics::KinematicsBase
bool lookupParam (const std::string &param, T &val, const T &default_val) const
 

Private Member Functions

bool checkConsistency (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const
 Check whether the solution lies within the consistency limit of the seed state. More...
 
int getJointIndex (const std::string &name) const
 
int getKDLSegmentIndex (const std::string &name) const
 
void getRandomConfiguration (KDL::JntArray &jnt_array, bool lock_redundancy) const
 
void getRandomConfiguration (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limits, KDL::JntArray &jnt_array, bool lock_redundancy) const
 Get a random configuration within joint limits close to the seed state. More...
 
bool isRedundantJoint (unsigned int index) const
 
bool timedOut (const ros::WallTime &start_time, double duration) const
 

Private Attributes

bool active_
 
unsigned int dimension_
 
double epsilon_
 
moveit_msgs::KinematicSolverInfo fk_chain_info_
 
moveit_msgs::KinematicSolverInfo ik_chain_info_
 
KDL::JntArray joint_max_
 
KDL::JntArray joint_min_
 
robot_model::JointModelGroup * joint_model_group_
 
KDL::Chain kdl_chain_
 
double max_solver_iterations_
 
std::vector< JointMimicmimic_joints_
 
int num_possible_redundant_joints_
 
bool position_ik_
 
random_numbers::RandomNumberGenerator random_number_generator_
 
std::vector< unsigned int > redundant_joints_map_index_
 
robot_model::RobotModelPtr robot_model_
 
robot_state::RobotStatePtr state_
 
robot_state::RobotStatePtr state_2_
 

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

Specific implementation of kinematics using Levenberg-Marquardt method available at KDL. This version can be used with any robot.

Definition at line 71 of file lma_kinematics_plugin.h.

Constructor & Destructor Documentation

lma_kinematics_plugin::LMAKinematicsPlugin::LMAKinematicsPlugin ( )

Default constructor.

Definition at line 54 of file lma_kinematics_plugin.cpp.

Member Function Documentation

bool lma_kinematics_plugin::LMAKinematicsPlugin::checkConsistency ( const KDL::JntArray seed_state,
const std::vector< double > &  consistency_limit,
const KDL::JntArray solution 
) const
private

Check whether the solution lies within the consistency limit of the seed state.

Parameters
seed_stateSeed state
redundancyIndex of the redundant joint within the chain
consistency_limitThe returned state for redundant joint should be in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
solutionsolution configuration
Returns
true if check succeeds

Definition at line 117 of file lma_kinematics_plugin.cpp.

int lma_kinematics_plugin::LMAKinematicsPlugin::getJointIndex ( const std::string &  name) const
private

Definition at line 347 of file lma_kinematics_plugin.cpp.

const std::vector< std::string > & lma_kinematics_plugin::LMAKinematicsPlugin::getJointNames ( ) const
virtual

Return all the joint names in the order they are used internally.

Implements kinematics::KinematicsBase.

Definition at line 612 of file lma_kinematics_plugin.cpp.

int lma_kinematics_plugin::LMAKinematicsPlugin::getKDLSegmentIndex ( const std::string &  name) const
private

Definition at line 357 of file lma_kinematics_plugin.cpp.

const std::vector< std::string > & lma_kinematics_plugin::LMAKinematicsPlugin::getLinkNames ( ) const
virtual

Return all the link names in the order they are represented internally.

Implements kinematics::KinematicsBase.

Definition at line 617 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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 567 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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 376 of file lma_kinematics_plugin.cpp.

void lma_kinematics_plugin::LMAKinematicsPlugin::getRandomConfiguration ( KDL::JntArray jnt_array,
bool  lock_redundancy 
) const
private

Definition at line 58 of file lma_kinematics_plugin.cpp.

void lma_kinematics_plugin::LMAKinematicsPlugin::getRandomConfiguration ( const KDL::JntArray seed_state,
const std::vector< double > &  consistency_limits,
KDL::JntArray jnt_array,
bool  lock_redundancy 
) const
private

Get a random configuration within joint limits close to the seed state.

Parameters
seed_stateSeed state
redundancyIndex of the redundant joint within the chain
consistency_limitThe returned state will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
jnt_arrayReturned random configuration

Definition at line 80 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::initialize ( const std::string &  robot_description,
const std::string &  group_name,
const std::string &  base_name,
const std::string &  tip_name,
double  search_discretization 
)
virtual

Implements kinematics::KinematicsBase.

Definition at line 127 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::isRedundantJoint ( unsigned int  index) const
private

Definition at line 72 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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 387 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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 399 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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 409 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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 420 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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

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
timeoutThe amount of time (in seconds) available to the solver
solutionthe solution vector
solution_callbackA callback solution for the IK solution
error_codean error code that encodes the reason for failure or success
check_consistencySet to true if consistency check needs to be performed
redundancyThe index of the redundant joint
consistency_limitThe returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
Returns
True if a valid solution was found, false otherwise

Definition at line 430 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::setRedundantJoints ( const std::vector< unsigned int > &  redundant_joint_indices)
protectedvirtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 288 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::timedOut ( const ros::WallTime start_time,
double  duration 
) const
private

Definition at line 371 of file lma_kinematics_plugin.cpp.

Member Data Documentation

bool lma_kinematics_plugin::LMAKinematicsPlugin::active_
private

Definition at line 179 of file lma_kinematics_plugin.h.

unsigned int lma_kinematics_plugin::LMAKinematicsPlugin::dimension_
private

Definition at line 187 of file lma_kinematics_plugin.h.

double lma_kinematics_plugin::LMAKinematicsPlugin::epsilon_
private

Definition at line 204 of file lma_kinematics_plugin.h.

moveit_msgs::KinematicSolverInfo lma_kinematics_plugin::LMAKinematicsPlugin::fk_chain_info_
private

Stores information for the inverse kinematics solver

Definition at line 183 of file lma_kinematics_plugin.h.

moveit_msgs::KinematicSolverInfo lma_kinematics_plugin::LMAKinematicsPlugin::ik_chain_info_
private

Internal variable that indicates whether solvers are configured and ready

Definition at line 181 of file lma_kinematics_plugin.h.

KDL::JntArray lma_kinematics_plugin::LMAKinematicsPlugin::joint_max_
private

Definition at line 189 of file lma_kinematics_plugin.h.

KDL::JntArray lma_kinematics_plugin::LMAKinematicsPlugin::joint_min_
private

Dimension of the group

Definition at line 189 of file lma_kinematics_plugin.h.

robot_model::JointModelGroup* lma_kinematics_plugin::LMAKinematicsPlugin::joint_model_group_
private

Definition at line 202 of file lma_kinematics_plugin.h.

KDL::Chain lma_kinematics_plugin::LMAKinematicsPlugin::kdl_chain_
private

Store information for the forward kinematics solver

Definition at line 185 of file lma_kinematics_plugin.h.

double lma_kinematics_plugin::LMAKinematicsPlugin::max_solver_iterations_
private

Definition at line 203 of file lma_kinematics_plugin.h.

std::vector<JointMimic> lma_kinematics_plugin::LMAKinematicsPlugin::mimic_joints_
private

Definition at line 205 of file lma_kinematics_plugin.h.

int lma_kinematics_plugin::LMAKinematicsPlugin::num_possible_redundant_joints_
private

Definition at line 197 of file lma_kinematics_plugin.h.

bool lma_kinematics_plugin::LMAKinematicsPlugin::position_ik_
private

Definition at line 201 of file lma_kinematics_plugin.h.

random_numbers::RandomNumberGenerator lma_kinematics_plugin::LMAKinematicsPlugin::random_number_generator_
mutableprivate

Joint limits

Definition at line 191 of file lma_kinematics_plugin.h.

std::vector<unsigned int> lma_kinematics_plugin::LMAKinematicsPlugin::redundant_joints_map_index_
private

Definition at line 198 of file lma_kinematics_plugin.h.

robot_model::RobotModelPtr lma_kinematics_plugin::LMAKinematicsPlugin::robot_model_
private

Definition at line 193 of file lma_kinematics_plugin.h.

robot_state::RobotStatePtr lma_kinematics_plugin::LMAKinematicsPlugin::state_
private

Definition at line 195 of file lma_kinematics_plugin.h.

robot_state::RobotStatePtr lma_kinematics_plugin::LMAKinematicsPlugin::state_2_
private

Definition at line 195 of file lma_kinematics_plugin.h.


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


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:53