An implementation of the Optimized Forward Kinematic Tree as a stat solver. More...

#include <ofkt_state_solver.h>

Inheritance diagram for tesseract_scene_graph::OFKTStateSolver:

Collaboration diagram for tesseract_scene_graph::OFKTStateSolver:

[legend]

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

using	ConstPtr = std::shared_ptr< const OFKTStateSolver >

using	UPtr = std::unique_ptr< OFKTStateSolver >

using	ConstUPtr = std::unique_ptr< const OFKTStateSolver >

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

using	ConstPtr = std::shared_ptr< const MutableStateSolver >

using	UPtr = std::unique_ptr< MutableStateSolver >

using	ConstUPtr = std::unique_ptr< const MutableStateSolver >

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
	OFKTStateSolver (const tesseract_scene_graph::SceneGraph &scene_graph, const std::string &prefix="")

	OFKTStateSolver (const std::string &root_name)

	~OFKTStateSolver () override=default

	OFKTStateSolver (const OFKTStateSolver &other)

OFKTStateSolver &	operator= (const OFKTStateSolver &other)

	OFKTStateSolver (OFKTStateSolver &&)=delete

OFKTStateSolver &	operator= (OFKTStateSolver &&)=delete

void	setRevision (int revision) override final
	Set the state solver revision number. More...

int	getRevision () const override final
	Get the state solver revision number. 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 > &joints_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...

bool	addLink (const Link &link, const Joint &joint) override final
	Adds a link/joint to the solver. More...

bool	moveLink (const Joint &joint) override final
	Move a link. More...

bool	removeLink (const std::string &name) override final
	Removes a link from the graph. More...

bool	replaceJoint (const Joint &joint) override final
	Replace and existing joint with the provided one. More...

bool	removeJoint (const std::string &name) override final
	Removes a joint from the graph. More...

bool	moveJoint (const std::string &name, const std::string &parent_link) override final
	Move joint to new parent link. More...

bool	changeJointOrigin (const std::string &name, const Eigen::Isometry3d &new_origin) override final
	Changes the "origin" transform of the joint and recomputes the associated edge. More...

bool	changeJointPositionLimits (const std::string &name, double lower, double upper) override final
	Changes the position limits associated with a joint. More...

bool	changeJointVelocityLimits (const std::string &name, double limit) override final
	Changes the velocity limits associated with a joint. More...

bool	changeJointAccelerationLimits (const std::string &name, double limit) override final
	Changes the acceleration limits associated with a joint. More...

bool	insertSceneGraph (const SceneGraph &scene_graph, const Joint &joint, const std::string &prefix="") override final
	Merge a scene into the current solver. More...

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

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

	~MutableStateSolver () override=default

	MutableStateSolver (const MutableStateSolver &)=default

MutableStateSolver &	operator= (const MutableStateSolver &)=default

	MutableStateSolver (MutableStateSolver &&)=default

MutableStateSolver &	operator= (MutableStateSolver &&)=default

virtual void	setRevision (int revision)=0
	Set the state solver revision number. More...

virtual int	getRevision () const =0
	Get the state solver revision number. More...

virtual bool	addLink (const Link &link, const Joint &joint)=0
	Adds a link/joint to the solver. More...

virtual bool	moveLink (const Joint &joint)=0
	Move a link. More...

virtual bool	removeLink (const std::string &name)=0
	Removes a link from the graph. More...

virtual bool	replaceJoint (const Joint &joint)=0
	Replace and existing joint with the provided one. More...

virtual bool	removeJoint (const std::string &name)=0
	Removes a joint from the graph. More...

virtual bool	moveJoint (const std::string &name, const std::string &parent_link)=0
	Move joint to new parent link. More...

virtual bool	changeJointOrigin (const std::string &name, const Eigen::Isometry3d &new_origin)=0
	Changes the "origin" transform of the joint and recomputes the associated edge. More...

virtual bool	changeJointPositionLimits (const std::string &name, double lower, double upper)=0
	Changes the position limits associated with a joint. More...

virtual bool	changeJointVelocityLimits (const std::string &name, double limit)=0
	Changes the velocity limits associated with a joint. More...

virtual bool	changeJointAccelerationLimits (const std::string &name, double limit)=0
	Changes the acceleration limits associated with a joint. More...

virtual bool	insertSceneGraph (const SceneGraph &scene_graph, const Joint &joint, const std::string &prefix="")=0
	Merge a scene into the current solver. 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
bool	initHelper (const tesseract_scene_graph::SceneGraph &scene_graph, const std::string &prefix)

void	clear ()

void	loadActiveLinkNamesRecursive (std::vector< std::string > &active_link_names, const OFKTNode *node, bool active) const
	load the active link names More...

void	loadStaticLinkNamesRecursive (std::vector< std::string > &static_link_names, const OFKTNode *node) const
	load the static link names More...

void	update (OFKTNode *node, bool update_required)
	This update the local and world transforms. More...

void	update (SceneState &state, const OFKTNode *node, Eigen::Isometry3d parent_world_tf, bool update_required) const
	This is a const version of the function above. More...

Eigen::MatrixXd	calcJacobianHelper (const std::unordered_map< std::string, double > &joints, const std::string &link_name) const
	Given a set of joint values calculate the jacobian for the provided link_name. More...

void	cloneHelper (OFKTStateSolver &cloned, const OFKTNode *node) const
	A helper function used for cloning the OFKTStateSolver. More...

void	addNode (const Joint &joint, const std::string &joint_name, const std::string &parent_link_name, const std::string &child_link_name, std::vector< JointLimits::ConstPtr > &new_joint_limits)
	Add a node to the tree. More...

void	removeNode (OFKTNode *node, std::vector< std::string > &removed_links, std::vector< std::string > &removed_joints, std::vector< std::string > &removed_active_joints, std::vector< long > &removed_active_joints_indices)
	Remove a node and all of its children. More...

void	moveLinkHelper (std::vector< JointLimits::ConstPtr > &new_joint_limits, const Joint &joint)
	This a helper function for moving a link. More...

void	replaceJointHelper (std::vector< JointLimits::ConstPtr > &new_joint_limits, const Joint &joint)
	This is a helper function for replacing a joint. More...

void	removeJointHelper (const std::vector< std::string > &removed_links, const std::vector< std::string > &removed_joints, const std::vector< std::string > &removed_active_joints, const std::vector< long > &removed_active_joints_indices)
	This will clean up member variables joint_names_ and limits_. More...

void	addNewJointLimits (const std::vector< JointLimits::ConstPtr > &new_joint_limits)
	appends the new joint limits More...

Private Attributes
SceneState	current_state_

std::vector< std::string >	joint_names_

std::vector< std::string >	active_joint_names_

std::vector< std::string >	link_names_

std::unordered_map< std::string, OFKTNode::UPtr >	nodes_

std::unordered_map< std::string, OFKTNode * >	link_map_

tesseract_common::KinematicLimits	limits_

OFKTNode::UPtr	root_

int	revision_ { 0 }

std::shared_mutex	mutex_
	The state solver can be accessed from multiple threads, need use mutex throughout. More...

Friends
struct	ofkt_builder

Detailed Description

An implementation of the Optimized Forward Kinematic Tree as a stat solver.

Starke, S., Hendrich, N., & Zhang, J. (2018). A Forward Kinematics Data Structure for Efficient Evolutionary Inverse Kinematics. In Computational Kinematics (pp. 560-568). Springer, Cham.

Member Typedef Documentation

◆ ConstPtr

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

◆ ConstUPtr

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

◆ Ptr

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

◆ UPtr

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

Constructor & Destructor Documentation

◆ OFKTStateSolver() [1/4]

tesseract_scene_graph::OFKTStateSolver::OFKTStateSolver	(	const tesseract_scene_graph::SceneGraph &	scene_graph,
		const std::string &	prefix = `""`
	)

◆ OFKTStateSolver() [2/4]

tesseract_scene_graph::OFKTStateSolver::OFKTStateSolver ( const std::string & root_name )

◆ ~OFKTStateSolver()

tesseract_scene_graph::OFKTStateSolver::~OFKTStateSolver ( )

overridedefault

◆ OFKTStateSolver() [3/4]

tesseract_scene_graph::OFKTStateSolver::OFKTStateSolver ( const OFKTStateSolver & other )

◆ OFKTStateSolver() [4/4]

tesseract_scene_graph::OFKTStateSolver::OFKTStateSolver ( OFKTStateSolver && )

delete

Member Function Documentation

◆ addLink()

bool tesseract_scene_graph::OFKTStateSolver::addLink	(	const Link &	link,
		const Joint &	joint
	)

finaloverridevirtual

Adds a link/joint to the solver.

Parameters

link	The link to be added to the graph
joint	The associated joint to be added to the graph

Returns: Return False if a link with the same name allready exists, otherwise true

Implements tesseract_scene_graph::MutableStateSolver.

◆ addNewJointLimits()

void tesseract_scene_graph::OFKTStateSolver::addNewJointLimits ( const std::vector< JointLimits::ConstPtr > & new_joint_limits )

private

appends the new joint limits

Parameters

new_joint_limits

◆ addNode()

void tesseract_scene_graph::OFKTStateSolver::addNode	(	const Joint &	joint,
		const std::string &	joint_name,
		const std::string &	parent_link_name,
		const std::string &	child_link_name,
		std::vector< JointLimits::ConstPtr > &	new_joint_limits
	)

private

Add a node to the tree.

The reason that joint_name, parent_link_name, child_link_name are required when joint is provided is to handle add a scene graph with a prefix.

Parameters

joint	The joint being added to the tree
joint_name	The joints name
parent_link_name	The joints parent link name
child_link_name	The joints child link name
kinematic_joints	The vector to store new kinematic joints added to the solver

◆ calcJacobianHelper()

Eigen::MatrixXd tesseract_scene_graph::OFKTStateSolver::calcJacobianHelper	(	const std::unordered_map< std::string, double > &	joints,
		const std::string &	link_name
	)		const

private

Given a set of joint values calculate the jacobian for the provided link_name.

Parameters

joints	The joint values to calculate the jacobian for
link_name	The link name to calculate the jacobian for

Returns: The calculated geometric jacobian

◆ changeJointAccelerationLimits()

bool tesseract_scene_graph::OFKTStateSolver::changeJointAccelerationLimits	(	const std::string &	name,
		double	limit
	)

finaloverridevirtual

Changes the acceleration limits associated with a joint.

Parameters

joint_name	Name of the joint to be updated
limits	New acceleration limits to be set as the joint limits

Returns

Implements tesseract_scene_graph::MutableStateSolver.

◆ changeJointOrigin()

bool tesseract_scene_graph::OFKTStateSolver::changeJointOrigin	(	const std::string &	name,
		const Eigen::Isometry3d &	new_origin
	)

finaloverridevirtual

Changes the "origin" transform of the joint and recomputes the associated edge.

Parameters

name	Name of the joint to be changed
new_origin	The new transform associated with the joint

Returns: True if successful.

Implements tesseract_scene_graph::MutableStateSolver.

◆ changeJointPositionLimits()

bool tesseract_scene_graph::OFKTStateSolver::changeJointPositionLimits	(	const std::string &	name,
		double	lower,
		double	upper
	)

finaloverridevirtual

Changes the position limits associated with a joint.

Parameters

joint_name	Name of the joint to be updated
limits	New position limits to be set as the joint limits @returnTrue if successful.

Implements tesseract_scene_graph::MutableStateSolver.

◆ changeJointVelocityLimits()

bool tesseract_scene_graph::OFKTStateSolver::changeJointVelocityLimits	(	const std::string &	name,
		double	limit
	)

finaloverridevirtual

Changes the velocity limits associated with a joint.

Parameters

joint_name	Name of the joint to be updated
limits	New velocity limits to be set as the joint limits

Returns

Implements tesseract_scene_graph::MutableStateSolver.

◆ clear()

void tesseract_scene_graph::OFKTStateSolver::clear ( )

private

◆ clone()

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

finaloverridevirtual

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.

◆ cloneHelper()

void tesseract_scene_graph::OFKTStateSolver::cloneHelper	(	OFKTStateSolver &	cloned,
		const OFKTNode *	node
	)		const

private

A helper function used for cloning the OFKTStateSolver.

Parameters

cloned	The cloned object
node	The node cloning

◆ getActiveJointNames()

std::vector< std::string > tesseract_scene_graph::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::getJointNames ( ) const

finaloverridevirtual

Get the vector of joint names.

Returns: A vector of joint names

Implements tesseract_scene_graph::StateSolver.

◆ getLimits()

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

finaloverridevirtual

Getter for kinematic limits.

Returns: The kinematic limits

Implements tesseract_scene_graph::StateSolver.

◆ getLinkNames()

std::vector< std::string > tesseract_scene_graph::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::getRandomState ( ) const

finaloverridevirtual

Get the random state of the environment.

Returns: Environment state

Implements tesseract_scene_graph::StateSolver.

◆ getRelativeLinkTransform()

Eigen::Isometry3d tesseract_scene_graph::OFKTStateSolver::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.

◆ getRevision()

int tesseract_scene_graph::OFKTStateSolver::getRevision ( ) const

finaloverridevirtual

Get the state solver revision number.

Returns: revision number

Implements tesseract_scene_graph::MutableStateSolver.

◆ getState() [1/4]

SceneState tesseract_scene_graph::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::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.

◆ initHelper()

bool tesseract_scene_graph::OFKTStateSolver::initHelper	(	const tesseract_scene_graph::SceneGraph &	scene_graph,
		const std::string &	prefix
	)

private

◆ insertSceneGraph()

bool tesseract_scene_graph::OFKTStateSolver::insertSceneGraph	(	const SceneGraph &	scene_graph,
		const Joint &	joint,
		const std::string &	prefix = `""`
	)

finaloverridevirtual

Merge a scene into the current solver.

Parameters

scene_graph	Const ref to the graph to be merged
joint	The joint that connects current scene with the inserted scene
prefix	string Will be prepended to every link and joint of the merged scene

Returns: Return False if any link or joint name collides with current solver, otherwise True The prefix argument is meant to allow adding multiple copies of the same subgraph with different names

Implements tesseract_scene_graph::MutableStateSolver.

◆ isActiveLinkName()

bool tesseract_scene_graph::OFKTStateSolver::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.

◆ loadActiveLinkNamesRecursive()

void tesseract_scene_graph::OFKTStateSolver::loadActiveLinkNamesRecursive	(	std::vector< std::string > &	active_link_names,
		const OFKTNode *	node,
		bool	active
	)		const

private

load the active link names

◆ loadStaticLinkNamesRecursive()

void tesseract_scene_graph::OFKTStateSolver::loadStaticLinkNamesRecursive	(	std::vector< std::string > &	static_link_names,
		const OFKTNode *	node
	)		const

private

load the static link names

◆ moveJoint()

bool tesseract_scene_graph::OFKTStateSolver::moveJoint	(	const std::string &	name,
		const std::string &	parent_link
	)

finaloverridevirtual

Move joint to new parent link.

Parameters

name	Name of the joint to move
parent_link	Name of parent link to move to

Returns: Returns true if successful, otherwise false.

Implements tesseract_scene_graph::MutableStateSolver.

◆ moveLink()

bool tesseract_scene_graph::OFKTStateSolver::moveLink ( const Joint & joint )

finaloverridevirtual

Move a link.

Parameters

joint The associated joint that defines the move

Returns: Return False if link does not exist or if joint name already exists, otherwise true

Implements tesseract_scene_graph::MutableStateSolver.

◆ moveLinkHelper()

void tesseract_scene_graph::OFKTStateSolver::moveLinkHelper	(	std::vector< JointLimits::ConstPtr > &	new_joint_limits,
		const Joint &	joint
	)

private

This a helper function for moving a link.

Parameters

new_kinematic_joints	The vector to store new kinematic joints added to the solver
joint	The joint performing the move

◆ operator=() [1/2]

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

◆ operator=() [2/2]

OFKTStateSolver & tesseract_scene_graph::OFKTStateSolver::operator= ( OFKTStateSolver && )

delete

◆ removeJoint()

bool tesseract_scene_graph::OFKTStateSolver::removeJoint ( const std::string & name )

finaloverridevirtual

Removes a joint from the graph.

Parameters

name	Name of the joint to be removed

Returns: Return False if a joint does not exists, otherwise true

Implements tesseract_scene_graph::MutableStateSolver.

◆ removeJointHelper()

void tesseract_scene_graph::OFKTStateSolver::removeJointHelper	(	const std::vector< std::string > &	removed_links,
		const std::vector< std::string > &	removed_joints,
		const std::vector< std::string > &	removed_active_joints,
		const std::vector< long > &	removed_active_joints_indices
	)

private

This will clean up member variables joint_names_ and limits_.

Parameters

removed_links	The removed link names container
removed_joints	The removed joint names container
removed_active_joints	The removed active joint names container
removed_active_joints_indices	The removed active joint names indices container

◆ removeLink()

bool tesseract_scene_graph::OFKTStateSolver::removeLink ( const std::string & name )

finaloverridevirtual

Removes a link from the graph.

Note: this will remove all inbound and outbound edges

Parameters

name	Name of the link to be removed

Returns: Return False if a link does not exists, otherwise true

Implements tesseract_scene_graph::MutableStateSolver.

◆ removeNode()

void tesseract_scene_graph::OFKTStateSolver::removeNode	(	OFKTNode *	node,
		std::vector< std::string > &	removed_links,
		std::vector< std::string > &	removed_joints,
		std::vector< std::string > &	removed_active_joints,
		std::vector< long > &	removed_active_joints_indices
	)

private

Remove a node and all of its children.

Parameters

node	The node to remove
removed_links	The removed link names container
removed_joints	The removed joint names container
removed_active_joints	The removed active joint names container
removed_active_joints_indices	The removed active joint names indices container

◆ replaceJoint()

bool tesseract_scene_graph::OFKTStateSolver::replaceJoint ( const Joint & joint )

finaloverridevirtual

Replace and existing joint with the provided one.

Parameters

joint The replacement joint

Returns: Return False if a joint does not exists, otherwise true

Implements tesseract_scene_graph::MutableStateSolver.

◆ replaceJointHelper()

void tesseract_scene_graph::OFKTStateSolver::replaceJointHelper	(	std::vector< JointLimits::ConstPtr > &	new_joint_limits,
		const Joint &	joint
	)

private

This is a helper function for replacing a joint.

Parameters

new_kinematic_joints	The vector to store new kinematic joints added to the solver
joint	The joint performing the replacement

◆ setRevision()

void tesseract_scene_graph::OFKTStateSolver::setRevision ( int revision )

finaloverridevirtual

Set the state solver revision number.

Parameters

revision The revision number to assign

Implements tesseract_scene_graph::MutableStateSolver.

◆ setState() [1/3]

void tesseract_scene_graph::OFKTStateSolver::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::OFKTStateSolver::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::OFKTStateSolver::setState	(	const std::vector< std::string > &	joint_names,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values
	)

finaloverridevirtual

Implements tesseract_scene_graph::StateSolver.

◆ update() [1/2]

void tesseract_scene_graph::OFKTStateSolver::update	(	OFKTNode *	node,
		bool	update_required
	)

private

This update the local and world transforms.

Parameters

node	The node to start from
update_required	Indicates if work transform update is required

◆ update() [2/2]

void tesseract_scene_graph::OFKTStateSolver::update	(	SceneState &	state,
		const OFKTNode *	node,
		Eigen::Isometry3d	parent_world_tf,
		bool	update_required
	)		const

private

This is a const version of the function above.

Parameters

node	The node to start from
parent_world_tf	The nodes parent's world transformaiton
update_required	Indicates if work transform update is required

Friends And Related Function Documentation

◆ ofkt_builder

friend struct ofkt_builder

friend

Member Data Documentation

◆ active_joint_names_

std::vector<std::string> tesseract_scene_graph::OFKTStateSolver::active_joint_names_

private

The active joint names

◆ current_state_

SceneState tesseract_scene_graph::OFKTStateSolver::current_state_

private

Current state of the scene

◆ joint_names_

std::vector<std::string> tesseract_scene_graph::OFKTStateSolver::joint_names_

private

The link names

◆ limits_

tesseract_common::KinematicLimits tesseract_scene_graph::OFKTStateSolver::limits_

private

The kinematic limits

◆ link_map_

std::unordered_map<std::string, OFKTNode*> tesseract_scene_graph::OFKTStateSolver::link_map_

private

The link name map to node

◆ link_names_

std::vector<std::string> tesseract_scene_graph::OFKTStateSolver::link_names_

private

The link names

◆ mutex_

std::shared_mutex tesseract_scene_graph::OFKTStateSolver::mutex_

mutableprivate

The state solver can be accessed from multiple threads, need use mutex throughout.

◆ nodes_

std::unordered_map<std::string, OFKTNode::UPtr> tesseract_scene_graph::OFKTStateSolver::nodes_

private

The joint name map to node

◆ revision_

int tesseract_scene_graph::OFKTStateSolver::revision_ { 0 }

private

The revision number

◆ root_

OFKTNode::UPtr tesseract_scene_graph::OFKTStateSolver::root_

private

The root node of the tree

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

tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h
tesseract_state_solver/src/ofkt_state_solver.cpp

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Friends

Detailed Description

Member Typedef Documentation

◆ ConstPtr

◆ ConstUPtr

◆ Ptr

◆ UPtr

Constructor & Destructor Documentation

◆ OFKTStateSolver() [1/4]

◆ OFKTStateSolver() [2/4]

◆ ~OFKTStateSolver()

◆ OFKTStateSolver() [3/4]

◆ OFKTStateSolver() [4/4]

Member Function Documentation

◆ addLink()

◆ addNewJointLimits()

◆ addNode()

◆ calcJacobianHelper()

◆ changeJointAccelerationLimits()

◆ changeJointOrigin()

◆ changeJointPositionLimits()

◆ changeJointVelocityLimits()

◆ clear()

◆ clone()

◆ cloneHelper()

◆ getActiveJointNames()

◆ getActiveLinkNames()

◆ getBaseLinkName()

◆ getJacobian() [1/3]

◆ getJacobian() [2/3]

◆ getJacobian() [3/3]

◆ getJointNames()

◆ getLimits()

◆ getLinkNames()

◆ getLinkTransform()

◆ getLinkTransforms()

◆ getRandomState()

◆ getRelativeLinkTransform()

◆ getRevision()

◆ getState() [1/4]

◆ getState() [2/4]

◆ getState() [3/4]

◆ getState() [4/4]

◆ getStaticLinkNames()

◆ hasLinkName()

◆ initHelper()

◆ insertSceneGraph()

◆ isActiveLinkName()

◆ loadActiveLinkNamesRecursive()

◆ loadStaticLinkNamesRecursive()

◆ moveJoint()

◆ moveLink()

◆ moveLinkHelper()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ removeJoint()

◆ removeJointHelper()

◆ removeLink()

◆ removeNode()

◆ replaceJoint()

◆ replaceJointHelper()

◆ setRevision()

◆ setState() [1/3]

◆ setState() [2/3]

◆ setState() [3/3]

◆ update() [1/2]

◆ update() [2/2]

Friends And Related Function Documentation

◆ ofkt_builder

Member Data Documentation

◆ active_joint_names_

◆ current_state_

◆ joint_names_

◆ limits_

◆ link_map_

◆ link_names_