Implementation of kinematics using Levenberg-Marquardt (LMA) solver from KDL. This version supports any kinematic chain without mimic joints. More...

#include <lma_kinematics_plugin.h>

Inheritance diagram for lma_kinematics_plugin::LMAKinematicsPlugin:

[legend]

Collaboration diagram for lma_kinematics_plugin::LMAKinematicsPlugin:

[legend]

Public Member Functions
	LMAKinematicsPlugin ()
	Default constructor. More...

bool	getPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	Given a desired pose of the end-effector, compute the joint angles to reach it. More...

bool	searchPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	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 solution by stepping through the redundancy (or other numerical routines). More...

bool	searchPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	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 solution by stepping through the redundancy (or other numerical routines). More...

bool	searchPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	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 solution by stepping through the redundancy (or other numerical routines). More...

bool	searchPositionIK (const geometry_msgs::msg::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::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	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 solution by stepping through the redundancy (or other numerical routines). More...

bool	getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const override
	Given a set of joint angles and a set of links, compute their pose. More...

bool	initialize (const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
	Initialization function for the kinematics, for use with kinematic chain IK solvers. More...

const std::vector< std::string > &	getJointNames () const override
	Return all the joint names in the order they are used internally. More...

const std::vector< std::string > &	getLinkNames () const override
	Return all the link names in the order they are represented internally. More...

Public Member Functions inherited from kinematics::KinematicsBase
virtual bool	getPositionIK (const std::vector< geometry_msgs::msg::Pose > &ik_poses, const std::vector< double > &ik_seed_state, std::vector< std::vector< double > > &solutions, KinematicsResult &result, const kinematics::KinematicsQueryOptions &options) const
	Given the desired poses of all end-effectors, compute joint angles that are able to reach it. More...

virtual bool	searchPositionIK (const std::vector< geometry_msgs::msg::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::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const
	Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. Not necessary for most robots that have kinematic chains. This particular method is intended for "searching" for a solution by stepping through the redundancy (or other numerical routines). More...

virtual bool	searchPositionIK (const std::vector< geometry_msgs::msg::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, const IKCostFn &cost_function, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const
	Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. Not necessary for most robots that have kinematic chains. This particular method is intended for "searching" for a solution by stepping through the redundancy (or other numerical routines). More...

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)
	Set the parameters for the solver, for use with non-chain IK solvers. More...

virtual const std::string &	getGroupName () const
	Return the name of the group that the solver is operating on. More...

virtual const std::string &	getBaseFrame () const
	Return the name of the frame in which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. More...

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 link name. No namespacing (e.g., no "/" prefix) should be used. More...

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

virtual bool	setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices)
	Set a set of redundant joints for the kinematics solver to use. This can fail, depending on the IK solver and choice of redundant joints!. Also, it sets the discretization values for each redundant joint to a default value. More...

bool	setRedundantJoints (const std::vector< std::string > &redundant_joint_names)
	Set a set of redundant joints for the kinematics solver to use. This function is just a convenience function that calls the previous definition of setRedundantJoints() More...

virtual void	getRedundantJoints (std::vector< unsigned int > &redundant_joint_indices) const
	Get the set of redundant joints. More...

virtual bool	supportsGroup (const moveit::core::JointModelGroup jmg, std::string error_text_out=nullptr) const
	Check if this solver supports a given JointModelGroup. More...

void	setSearchDiscretization (double sd)
	Set the search discretization value for all the redundant joints. More...

void	setSearchDiscretization (const std::map< int, double > &discretization)
	Sets individual discretization values for each redundant joint. More...

double	getSearchDiscretization (int joint_index=0) const
	Get the value of the search discretization. More...

std::vector< DiscretizationMethod >	getSupportedDiscretizationMethods () const
	Returns the set of supported kinematics discretization search types. This implementation only supports the DiscretizationMethods::ONE search. More...

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) More...

double	getDefaultTimeout () const
	For functions that require a timeout specified but one is not specified using arguments, this default timeout is used. More...

virtual	~KinematicsBase ()
	Virtual destructor for the interface. More...

	KinematicsBase ()

Additional Inherited Members
Public Types inherited from kinematics::KinematicsBase
using	IKCallbackFn = std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)>
	Signature for a callback to validate an IK solution. Typically used for collision checking. More...

using	IKCostFn = std::function< double(const geometry_msgs::msg::Pose &, const moveit::core::RobotState &, moveit::core::JointModelGroup const *, const std::vector< double > &)>
	Signature for a cost function used to evaluate IK solutions. More...

Static Public Attributes inherited from kinematics::KinematicsBase
static const rclcpp::Logger	LOGGER = rclcpp::get_logger("moveit_kinematics_base.kinematics_base")

static const double	DEFAULT_SEARCH_DISCRETIZATION = 0.1

static const double	DEFAULT_TIMEOUT = 1.0

Protected Member Functions inherited from kinematics::KinematicsBase
template<typename T >
bool	lookupParam (const rclcpp::Node::SharedPtr &node, const std::string &param, T &val, const T &default_val) const
	Enables kinematics plugins access to parameters that are defined for the private namespace and inside 'robot_description_kinematics'. Parameters are searched in the following locations and order. More...

void	storeValues (const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)

Protected Attributes inherited from kinematics::KinematicsBase
rclcpp::Node::SharedPtr	node_

moveit::core::RobotModelConstPtr	robot_model_

std::string	robot_description_

std::string	group_name_

std::string	base_frame_

std::vector< std::string >	tip_frames_

double	default_timeout_

std::vector< unsigned int >	redundant_joint_indices_

std::map< int, double >	redundant_joint_discretization_

std::vector< DiscretizationMethod >	supported_methods_

Detailed Description

Implementation of kinematics using Levenberg-Marquardt (LMA) solver from KDL. This version supports any kinematic chain without mimic joints.

Definition at line 66 of file lma_kinematics_plugin.h.

Constructor & Destructor Documentation

◆ LMAKinematicsPlugin()

lma_kinematics_plugin::LMAKinematicsPlugin::LMAKinematicsPlugin ( )

Default constructor.

Definition at line 54 of file lma_kinematics_plugin.cpp.

Member Function Documentation

◆ getJointNames()

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

overridevirtual

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

Implements kinematics::KinematicsBase.

Definition at line 348 of file lma_kinematics_plugin.cpp.

◆ getLinkNames()

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

overridevirtual

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

Implements kinematics::KinematicsBase.

Definition at line 353 of file lma_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ getPositionFK()

bool lma_kinematics_plugin::LMAKinematicsPlugin::getPositionFK	(	const std::vector< std::string > &	link_names,
		const std::vector< double > &	joint_angles,
		std::vector< geometry_msgs::msg::Pose > &	poses
	)		const

overridevirtual

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

Parameters

link_names	A set of links for which FK needs to be computed
joint_angles	The state for which FK is being computed
poses	The 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 312 of file lma_kinematics_plugin.cpp.

◆ getPositionIK()

bool lma_kinematics_plugin::LMAKinematicsPlugin::getPositionIK	(	const geometry_msgs::msg::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		std::vector< double > &	solution,
		moveit_msgs::msg::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

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

In contrast to the searchPositionIK methods, this one is expected to return the solution closest to the seed state. Randomly re-seeding is explicitly not allowed.

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
solution	the solution vector
error_code	an error code that encodes the reason for failure or success
options	container for other IK options. See definition of KinematicsQueryOptions for details.

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 153 of file lma_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ initialize()

bool lma_kinematics_plugin::LMAKinematicsPlugin::initialize	(	const rclcpp::Node::SharedPtr &	node,
		const moveit::core::RobotModel &	robot_model,
		const std::string &	group_name,
		const std::string &	base_frame,
		const std::vector< std::string > &	tip_frames,
		double	search_discretization
	)

overridevirtual

Initialization function for the kinematics, for use with kinematic chain IK solvers.

Parameters

robot_model	- allow the URDF to be loaded much quicker by passing in a pre-parsed model of the robot
group_name	The group for which this solver is being configured
base_frame	The base frame in which all input poses are expected. This may (or may not) be the root frame of the chain that the solver operates on
tip_frames	The tip of the chain
search_discretization	The discretization of the search when the solver steps through the redundancy

Returns: true if initialization was successful, false otherwise

Default implementation returns false and issues a warning to implement this new API. TODO: Make this method purely virtual after some soaking time, replacing the fallback.

Reimplemented from kinematics::KinematicsBase.

Definition at line 82 of file lma_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ searchPositionIK() [1/4]

bool lma_kinematics_plugin::LMAKinematicsPlugin::searchPositionIK	(	const geometry_msgs::msg::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::msg::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
timeout	The amount of time (in seconds) available to the solver
consistency_limits	the distance that any joint in the solution can be from the corresponding joints in the current seed state
solution	the solution vector
solution_callback	A callback to validate an IK solution
error_code	an error code that encodes the reason for failure or success
options	container for other IK options. See definition of KinematicsQueryOptions for details.

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 214 of file lma_kinematics_plugin.cpp.

◆ searchPositionIK() [2/4]

bool lma_kinematics_plugin::LMAKinematicsPlugin::searchPositionIK	(	const geometry_msgs::msg::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		double	timeout,
		const std::vector< double > &	consistency_limits,
		std::vector< double > &	solution,
		moveit_msgs::msg::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
timeout	The amount of time (in seconds) available to the solver
consistency_limits	the distance that any joint in the solution can be from the corresponding joints in the current seed state
solution	the solution vector
error_code	an error code that encodes the reason for failure or success
options	container for other IK options. See definition of KinematicsQueryOptions for details.

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 177 of file lma_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ searchPositionIK() [3/4]

bool lma_kinematics_plugin::LMAKinematicsPlugin::searchPositionIK	(	const geometry_msgs::msg::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		double	timeout,
		std::vector< double > &	solution,
		const IKCallbackFn &	solution_callback,
		moveit_msgs::msg::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
timeout	The amount of time (in seconds) available to the solver
solution	the solution vector
solution_callback	A callback to validate an IK solution
error_code	an error code that encodes the reason for failure or success
options	container for other IK options. See definition of KinematicsQueryOptions for details.

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 187 of file lma_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ searchPositionIK() [4/4]

bool lma_kinematics_plugin::LMAKinematicsPlugin::searchPositionIK	(	const geometry_msgs::msg::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		double	timeout,
		std::vector< double > &	solution,
		moveit_msgs::msg::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
timeout	The amount of time (in seconds) available to the solver
solution	the solution vector
error_code	an error code that encodes the reason for failure or success
options	container for other IK options. See definition of KinematicsQueryOptions for details.

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 165 of file lma_kinematics_plugin.cpp.

Here is the caller graph for this function:

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

moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h
moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp

Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ LMAKinematicsPlugin()

Member Function Documentation

◆ getJointNames()

◆ getLinkNames()

◆ getPositionFK()

◆ getPositionIK()

◆ initialize()

◆ searchPositionIK() [1/4]

◆ searchPositionIK() [2/4]

◆ searchPositionIK() [3/4]

◆ searchPositionIK() [4/4]