#include <kdl_state_solver.h>

Inheritance diagram for tesseract_scene_graph::KDLStateSolver:

Collaboration diagram for tesseract_scene_graph::KDLStateSolver:

[legend]

Public Types
using	Ptr = std::shared_ptr< KDLStateSolver >

using	ConstPtr = std::shared_ptr< const KDLStateSolver >

using	UPtr = std::unique_ptr< KDLStateSolver >

using	ConstUPtr = std::unique_ptr< const KDLStateSolver >

Public Types inherited from tesseract_scene_graph::StateSolver
using	Ptr = std::shared_ptr< StateSolver >

using	ConstPtr = std::shared_ptr< const StateSolver >

using	UPtr = std::unique_ptr< StateSolver >

using	ConstUPtr = std::unique_ptr< const StateSolver >

Public Member Functions
	KDLStateSolver (const tesseract_scene_graph::SceneGraph &scene_graph)

	KDLStateSolver (const tesseract_scene_graph::SceneGraph &scene_graph, KDLTreeData data)

	~KDLStateSolver () override=default

	KDLStateSolver (const KDLStateSolver &other)

KDLStateSolver &	operator= (const KDLStateSolver &other)

	KDLStateSolver (KDLStateSolver &&)=delete

KDLStateSolver &	operator= (KDLStateSolver &&)=delete

StateSolver::UPtr	clone () const override
	This should clone the object so it may be used in a multi threaded application where each thread would clone the solver. More...

void	setState (const Eigen::Ref< const Eigen::VectorXd > &joint_values) override final
	Set the current state of the solver. More...

void	setState (const std::unordered_map< std::string, double > &joint_values) override final
	Set the current state of the solver. More...

void	setState (const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values) override final

SceneState	getState (const Eigen::Ref< const Eigen::VectorXd > &joint_values) const override final
	Get the state of the solver given the joint values. More...

SceneState	getState (const std::unordered_map< std::string, double > &joint_values) const override final
	Get the state of the scene for a given set or subset of joint values. More...

SceneState	getState (const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values) const override final

SceneState	getState () const override final
	Get the current state of the scene. More...

SceneState	getRandomState () const override final
	Get the random state of the environment. More...

Eigen::MatrixXd	getJacobian (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name) const override final
	Get the jacobian of the solver given the joint values. More...

Eigen::MatrixXd	getJacobian (const std::unordered_map< std::string, double > &joint_values, const std::string &link_name) const override final
	Get the jacobian of the scene for a given set or subset of joint values. More...

Eigen::MatrixXd	getJacobian (const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name) const override final

std::vector< std::string >	getJointNames () const override final
	Get the vector of joint names. More...

std::vector< std::string >	getActiveJointNames () const override final
	Get the vector of joint names which align with the limits. More...

std::string	getBaseLinkName () const override final
	Get the base link name. More...

std::vector< std::string >	getLinkNames () const override final
	Get the vector of link names. More...

std::vector< std::string >	getActiveLinkNames () const override final
	Get the vector of active link names. More...

std::vector< std::string >	getStaticLinkNames () const override final
	Get a vector of static link names in the environment. More...

bool	isActiveLinkName (const std::string &link_name) const override final
	Check if link is an active link. More...

bool	hasLinkName (const std::string &link_name) const override final
	Check if link name exists. More...

tesseract_common::VectorIsometry3d	getLinkTransforms () const override final
	Get all of the links transforms. More...

Eigen::Isometry3d	getLinkTransform (const std::string &link_name) const override final
	Get the transform corresponding to the link. More...

Eigen::Isometry3d	getRelativeLinkTransform (const std::string &from_link_name, const std::string &to_link_name) const override final
	Get transform between two links using the current state. More...

tesseract_common::KinematicLimits	getLimits () const override final
	Getter for kinematic limits. More...

Public Member Functions inherited from tesseract_scene_graph::StateSolver
	StateSolver ()=default

virtual	~StateSolver ()=default

	StateSolver (const StateSolver &)=default

StateSolver &	operator= (const StateSolver &)=default

	StateSolver (StateSolver &&)=default

StateSolver &	operator= (StateSolver &&)=default

virtual StateSolver::UPtr	clone () const =0
	This should clone the object so it may be used in a multi threaded application where each thread would clone the solver. More...

virtual void	setState (const Eigen::Ref< const Eigen::VectorXd > &joint_values)=0
	Set the current state of the solver. More...

virtual void	setState (const std::unordered_map< std::string, double > &joint_values)=0
	Set the current state of the solver. More...

virtual void	setState (const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values)=0

virtual SceneState	getState (const Eigen::Ref< const Eigen::VectorXd > &joint_values) const =0
	Get the state of the solver given the joint values. More...

virtual SceneState	getState (const std::unordered_map< std::string, double > &joint_values) const =0
	Get the state of the scene for a given set or subset of joint values. More...

virtual SceneState	getState (const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values) const =0

virtual SceneState	getState () const =0
	Get the current state of the scene. More...

virtual Eigen::MatrixXd	getJacobian (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name) const =0
	Get the jacobian of the solver given the joint values. More...

virtual Eigen::MatrixXd	getJacobian (const std::unordered_map< std::string, double > &joint_values, const std::string &link_name) const =0
	Get the jacobian of the scene for a given set or subset of joint values. More...

virtual Eigen::MatrixXd	getJacobian (const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name) const =0

virtual SceneState	getRandomState () const =0
	Get the random state of the environment. More...

virtual std::vector< std::string >	getJointNames () const =0
	Get the vector of joint names. More...

virtual std::vector< std::string >	getActiveJointNames () const =0
	Get the vector of joint names which align with the limits. More...

virtual std::string	getBaseLinkName () const =0
	Get the base link name. More...

virtual std::vector< std::string >	getLinkNames () const =0
	Get the vector of link names. More...

virtual std::vector< std::string >	getActiveLinkNames () const =0
	Get the vector of active link names. More...

virtual std::vector< std::string >	getStaticLinkNames () const =0
	Get a vector of static link names in the environment. More...

virtual bool	isActiveLinkName (const std::string &link_name) const =0
	Check if link is an active link. More...

virtual bool	hasLinkName (const std::string &link_name) const =0
	Check if link name exists. More...

virtual tesseract_common::VectorIsometry3d	getLinkTransforms () const =0
	Get all of the links transforms. More...

virtual Eigen::Isometry3d	getLinkTransform (const std::string &link_name) const =0
	Get the transform corresponding to the link. More...

virtual Eigen::Isometry3d	getRelativeLinkTransform (const std::string &from_link_name, const std::string &to_link_name) const =0
	Get transform between two links using the current state. More...

virtual tesseract_common::KinematicLimits	getLimits () const =0
	Getter for kinematic limits. More...

Private Member Functions
void	calculateTransforms (SceneState &state, const KDL::JntArray &q_in, const KDL::SegmentMap::const_iterator &it, const Eigen::Isometry3d &parent_frame) const

void	calculateTransformsHelper (SceneState &state, const KDL::JntArray &q_in, const KDL::SegmentMap::const_iterator &it, const Eigen::Isometry3d &parent_frame) const

bool	setJointValuesHelper (KDL::JntArray &q, const std::string &joint_name, const double &joint_value) const

bool	calcJacobianHelper (KDL::Jacobian &jacobian, const KDL::JntArray &kdl_joints, const std::string &link_name) const

KDL::JntArray	getKDLJntArray (const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values) const
	Get an updated kdl joint array. More...

KDL::JntArray	getKDLJntArray (const std::unordered_map< std::string, double > &joint_values) const

bool	processKDLData (const tesseract_scene_graph::SceneGraph &scene_graph)

Private Attributes
SceneState	current_state_

KDLTreeData	data_

std::unique_ptr< KDL::TreeJntToJacSolver >	jac_solver_

std::unordered_map< std::string, unsigned int >	joint_to_qnr_

std::vector< int >	joint_qnr_

KDL::JntArray	kdl_jnt_array_

tesseract_common::KinematicLimits	limits_

std::mutex	mutex_
	KDL is not thread safe due to mutable variables in Joint Class. More...

Member Typedef Documentation

◆ ConstPtr

using tesseract_scene_graph::KDLStateSolver::ConstPtr = std::shared_ptr<const KDLStateSolver>

◆ ConstUPtr

using tesseract_scene_graph::KDLStateSolver::ConstUPtr = std::unique_ptr<const KDLStateSolver>

◆ Ptr

using tesseract_scene_graph::KDLStateSolver::Ptr = std::shared_ptr<KDLStateSolver>

◆ UPtr

using tesseract_scene_graph::KDLStateSolver::UPtr = std::unique_ptr<KDLStateSolver>

Constructor & Destructor Documentation

◆ KDLStateSolver() [1/4]

tesseract_scene_graph::KDLStateSolver::KDLStateSolver ( const tesseract_scene_graph::SceneGraph & scene_graph )

◆ KDLStateSolver() [2/4]

tesseract_scene_graph::KDLStateSolver::KDLStateSolver	(	const tesseract_scene_graph::SceneGraph &	scene_graph,
		KDLTreeData	data
	)

◆ ~KDLStateSolver()

tesseract_scene_graph::KDLStateSolver::~KDLStateSolver ( )

overridedefault

◆ KDLStateSolver() [3/4]

tesseract_scene_graph::KDLStateSolver::KDLStateSolver ( const KDLStateSolver & other )

◆ KDLStateSolver() [4/4]

tesseract_scene_graph::KDLStateSolver::KDLStateSolver ( KDLStateSolver && )

delete

Member Function Documentation

◆ calcJacobianHelper()

bool tesseract_scene_graph::KDLStateSolver::calcJacobianHelper	(	KDL::Jacobian &	jacobian,
		const KDL::JntArray &	kdl_joints,
		const std::string &	link_name
	)		const

private

◆ calculateTransforms()

void tesseract_scene_graph::KDLStateSolver::calculateTransforms	(	SceneState &	state,
		const KDL::JntArray &	q_in,
		const KDL::SegmentMap::const_iterator &	it,
		const Eigen::Isometry3d &	parent_frame
	)		const

private

◆ calculateTransformsHelper()

void tesseract_scene_graph::KDLStateSolver::calculateTransformsHelper	(	SceneState &	state,
		const KDL::JntArray &	q_in,
		const KDL::SegmentMap::const_iterator &	it,
		const Eigen::Isometry3d &	parent_frame
	)		const

private

◆ clone()

StateSolver::UPtr tesseract_scene_graph::KDLStateSolver::clone ( ) const

overridevirtual

This should clone the object so it may be used in a multi threaded application where each thread would clone the solver.

Returns: A clone of the object.

Implements tesseract_scene_graph::StateSolver.

◆ getActiveJointNames()

std::vector< std::string > tesseract_scene_graph::KDLStateSolver::getActiveJointNames ( ) const

finaloverridevirtual

Get the vector of joint names which align with the limits.

Returns: A vector of joint names

Implements tesseract_scene_graph::StateSolver.

◆ getActiveLinkNames()

std::vector< std::string > tesseract_scene_graph::KDLStateSolver::getActiveLinkNames ( ) const

finaloverridevirtual

Get the vector of active link names.

Returns: A vector of active link names

Implements tesseract_scene_graph::StateSolver.

◆ getBaseLinkName()

std::string tesseract_scene_graph::KDLStateSolver::getBaseLinkName ( ) const

finaloverridevirtual

Get the base link name.

Returns: The base link name

Implements tesseract_scene_graph::StateSolver.

◆ getJacobian() [1/3]

Eigen::MatrixXd tesseract_scene_graph::KDLStateSolver::getJacobian	(	const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const std::string &	link_name
	)		const

finaloverridevirtual

Get the jacobian of the solver given the joint values.

This must be the same size and order as what is returned by getJointNames

Parameters

joint_values	The joint values
link_name	The link name to calculate the jacobian

Implements tesseract_scene_graph::StateSolver.

◆ getJacobian() [2/3]

Eigen::MatrixXd tesseract_scene_graph::KDLStateSolver::getJacobian	(	const std::unordered_map< std::string, double > &	joint_values,
		const std::string &	link_name
	)		const

finaloverridevirtual

Get the jacobian of the scene for a given set or subset of joint values.

This does not return the jacobian based on the provided joint names. It is order based on the order returned from getJointNames

This does not change the internal state of the solver.

Parameters

joints	A map of joint names to joint values to change.
link_name	The link name to calculate the jacobian

Returns: A the state of the environment

Implements tesseract_scene_graph::StateSolver.

◆ getJacobian() [3/3]

Eigen::MatrixXd tesseract_scene_graph::KDLStateSolver::getJacobian	(	const std::vector< std::string > &	joint_names,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const std::string &	link_name
	)		const

finaloverridevirtual

Implements tesseract_scene_graph::StateSolver.

◆ getJointNames()

std::vector< std::string > tesseract_scene_graph::KDLStateSolver::getJointNames ( ) const

finaloverridevirtual

Get the vector of joint names.

Returns: A vector of joint names

Implements tesseract_scene_graph::StateSolver.

◆ getKDLJntArray() [1/2]

KDL::JntArray tesseract_scene_graph::KDLStateSolver::getKDLJntArray ( const std::unordered_map< std::string, double > & joint_values ) const

private

◆ getKDLJntArray() [2/2]

KDL::JntArray tesseract_scene_graph::KDLStateSolver::getKDLJntArray	(	const std::vector< std::string > &	joint_names,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values
	)		const

private

Get an updated kdl joint array.

◆ getLimits()

tesseract_common::KinematicLimits tesseract_scene_graph::KDLStateSolver::getLimits ( ) const

finaloverridevirtual

Getter for kinematic limits.

Returns: The kinematic limits

Implements tesseract_scene_graph::StateSolver.

◆ getLinkNames()

std::vector< std::string > tesseract_scene_graph::KDLStateSolver::getLinkNames ( ) const

finaloverridevirtual

Get the vector of link names.

Returns: A vector of link names

Implements tesseract_scene_graph::StateSolver.

◆ getLinkTransform()

Eigen::Isometry3d tesseract_scene_graph::KDLStateSolver::getLinkTransform ( const std::string & link_name ) const

finaloverridevirtual

Get the transform corresponding to the link.

Returns: Transform and is identity when no transform is available.

Implements tesseract_scene_graph::StateSolver.

◆ getLinkTransforms()

tesseract_common::VectorIsometry3d tesseract_scene_graph::KDLStateSolver::getLinkTransforms ( ) const

finaloverridevirtual

Get all of the links transforms.

Order should be the same as getLinkNames()

Returns: Get a vector of transforms for all links.

Implements tesseract_scene_graph::StateSolver.

◆ getRandomState()

SceneState tesseract_scene_graph::KDLStateSolver::getRandomState ( ) const

finaloverridevirtual

Get the random state of the environment.

Returns: Environment state

Implements tesseract_scene_graph::StateSolver.

◆ getRelativeLinkTransform()

Eigen::Isometry3d tesseract_scene_graph::KDLStateSolver::getRelativeLinkTransform	(	const std::string &	from_link_name,
		const std::string &	to_link_name
	)		const

finaloverridevirtual

Get transform between two links using the current state.

Parameters

from_link_name	The link name the transform should be relative to
to_link_name	The link name to get transform

Returns: The relative transform = inv(Transform(from_link_name)) * Transform(to_link_name)

Implements tesseract_scene_graph::StateSolver.

◆ getState() [1/4]

SceneState tesseract_scene_graph::KDLStateSolver::getState ( ) const

finaloverridevirtual

Get the current state of the scene.

Returns: The current state

Implements tesseract_scene_graph::StateSolver.

◆ getState() [2/4]

SceneState tesseract_scene_graph::KDLStateSolver::getState ( const Eigen::Ref< const Eigen::VectorXd > & joint_values ) const

finaloverridevirtual

Get the state of the solver given the joint values.

This must be the same size and order as what is returned by getJointNames

Parameters

joint_values The joint values

Implements tesseract_scene_graph::StateSolver.

◆ getState() [3/4]

SceneState tesseract_scene_graph::KDLStateSolver::getState ( const std::unordered_map< std::string, double > & joint_values ) const

finaloverridevirtual

Get the state of the scene for a given set or subset of joint values.

This does not change the internal state of the solver.

Parameters

joints A map of joint names to joint values to change.

Returns: A the state of the environment

Implements tesseract_scene_graph::StateSolver.

◆ getState() [4/4]

SceneState tesseract_scene_graph::KDLStateSolver::getState	(	const std::vector< std::string > &	joint_names,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values
	)		const

finaloverridevirtual

Implements tesseract_scene_graph::StateSolver.

◆ getStaticLinkNames()

std::vector< std::string > tesseract_scene_graph::KDLStateSolver::getStaticLinkNames ( ) const

finaloverridevirtual

Get a vector of static link names in the environment.

Returns: A vector of static link names

Implements tesseract_scene_graph::StateSolver.

◆ hasLinkName()

bool tesseract_scene_graph::KDLStateSolver::hasLinkName ( const std::string & link_name ) const

finaloverridevirtual

Check if link name exists.

Parameters

link_name The link name to check for

Returns: True if it exists, otherwise false

Implements tesseract_scene_graph::StateSolver.

◆ isActiveLinkName()

bool tesseract_scene_graph::KDLStateSolver::isActiveLinkName ( const std::string & link_name ) const

finaloverridevirtual

Check if link is an active link.

Parameters

link_name The link name to check

Returns: True if active, otherwise false

Implements tesseract_scene_graph::StateSolver.

◆ operator=() [1/2]

KDLStateSolver & tesseract_scene_graph::KDLStateSolver::operator= ( const KDLStateSolver & other )

◆ operator=() [2/2]

KDLStateSolver & tesseract_scene_graph::KDLStateSolver::operator= ( KDLStateSolver && )

delete

◆ processKDLData()

bool tesseract_scene_graph::KDLStateSolver::processKDLData ( const tesseract_scene_graph::SceneGraph & scene_graph )

private

◆ setJointValuesHelper()

bool tesseract_scene_graph::KDLStateSolver::setJointValuesHelper	(	KDL::JntArray &	q,
		const std::string &	joint_name,
		const double &	joint_value
	)		const

private

◆ setState() [1/3]

void tesseract_scene_graph::KDLStateSolver::setState ( const Eigen::Ref< const Eigen::VectorXd > & joint_values )

finaloverridevirtual

Set the current state of the solver.

This must be the same size and order as what is returned by getJointNames

Parameters

joint_values The joint values

Implements tesseract_scene_graph::StateSolver.

◆ setState() [2/3]

void tesseract_scene_graph::KDLStateSolver::setState ( const std::unordered_map< std::string, double > & joint_values )

finaloverridevirtual

Set the current state of the solver.

After updating the current state these function must call currentStateChanged() which will update the contact managers transforms

Implements tesseract_scene_graph::StateSolver.

◆ setState() [3/3]

void tesseract_scene_graph::KDLStateSolver::setState	(	const std::vector< std::string > &	joint_names,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values
	)

finaloverridevirtual

Implements tesseract_scene_graph::StateSolver.

Member Data Documentation

◆ current_state_

SceneState tesseract_scene_graph::KDLStateSolver::current_state_

private

Current state of the environment

◆ data_

KDLTreeData tesseract_scene_graph::KDLStateSolver::data_

private

KDL tree data

◆ jac_solver_

std::unique_ptr<KDL::TreeJntToJacSolver> tesseract_scene_graph::KDLStateSolver::jac_solver_

private

KDL Jacobian Solver

◆ joint_qnr_

std::vector<int> tesseract_scene_graph::KDLStateSolver::joint_qnr_

private

The kdl segment number corresponding to joint in joint names

◆ joint_to_qnr_

std::unordered_map<std::string, unsigned int> tesseract_scene_graph::KDLStateSolver::joint_to_qnr_

private

Map between joint name and kdl q index

◆ kdl_jnt_array_

KDL::JntArray tesseract_scene_graph::KDLStateSolver::kdl_jnt_array_

private

The kdl joint array

◆ limits_

tesseract_common::KinematicLimits tesseract_scene_graph::KDLStateSolver::limits_

private

The kinematic limits

◆ mutex_

std::mutex tesseract_scene_graph::KDLStateSolver::mutex_

mutableprivate

KDL is not thread safe due to mutable variables in Joint Class.

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

tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h
tesseract_state_solver/src/kdl_state_solver.cpp

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Member Typedef Documentation

◆ ConstPtr

◆ ConstUPtr

◆ Ptr

◆ UPtr

Constructor & Destructor Documentation

◆ KDLStateSolver() [1/4]

◆ KDLStateSolver() [2/4]

◆ ~KDLStateSolver()

◆ KDLStateSolver() [3/4]

◆ KDLStateSolver() [4/4]

Member Function Documentation

◆ calcJacobianHelper()

◆ calculateTransforms()

◆ calculateTransformsHelper()

◆ clone()

◆ getActiveJointNames()

◆ getActiveLinkNames()

◆ getBaseLinkName()

◆ getJacobian() [1/3]

◆ getJacobian() [2/3]

◆ getJacobian() [3/3]

◆ getJointNames()

◆ getKDLJntArray() [1/2]

◆ getKDLJntArray() [2/2]

◆ getLimits()

◆ getLinkNames()

◆ getLinkTransform()

◆ getLinkTransforms()

◆ getRandomState()

◆ getRelativeLinkTransform()

◆ getState() [1/4]

◆ getState() [2/4]

◆ getState() [3/4]

◆ getState() [4/4]

◆ getStaticLinkNames()

◆ hasLinkName()

◆ isActiveLinkName()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ processKDLData()

◆ setJointValuesHelper()

◆ setState() [1/3]

◆ setState() [2/3]

◆ setState() [3/3]

Member Data Documentation

◆ current_state_

◆ data_

◆ jac_solver_

◆ joint_qnr_

◆ joint_to_qnr_

◆ kdl_jnt_array_

◆ limits_

◆ mutex_