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:

Public Types
using	ConstPtr = std::shared_ptr< const OFKTStateSolver >

using	ConstUPtr = std::unique_ptr< const OFKTStateSolver >

using	Ptr = std::shared_ptr< OFKTStateSolver >

using	UPtr = std::unique_ptr< OFKTStateSolver >

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

using	ConstUPtr = std::unique_ptr< const MutableStateSolver >

using	Ptr = std::shared_ptr< MutableStateSolver >

using	UPtr = std::unique_ptr< MutableStateSolver >

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

using	ConstUPtr = std::unique_ptr< const StateSolver >

using	Ptr = std::shared_ptr< StateSolver >

using	UPtr = std::unique_ptr< StateSolver >

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

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

bool	changeJointJerkLimits (const std::string &name, double limit) override final
	Changes the jerk limits associated with a joint. 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...

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

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

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

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

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

Eigen::MatrixXd	getJacobian (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name, const tesseract_common::TransformMap &floating_joint_values={}) 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 tesseract_common::TransformMap &floating_joint_values={}) 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 tesseract_common::TransformMap &floating_joint_values={}) const override final

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

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

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

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

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

SceneState	getRandomState () const override final
	Get the random state of the environment. 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...

int	getRevision () const override final
	Get the state solver revision number. More...

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

SceneState	getState (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract_common::TransformMap &floating_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 tesseract_common::TransformMap &floating_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 tesseract_common::TransformMap &floating_joint_values={}) const override final

SceneState	getState (const tesseract_common::TransformMap &floating_joint_values) const override final
	Get the state given floating joint values. More...

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

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

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

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

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

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

	OFKTStateSolver (const OFKTStateSolver &other)

	OFKTStateSolver (const std::string &root_name)

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

	OFKTStateSolver (OFKTStateSolver &&)=delete

OFKTStateSolver &	operator= (const OFKTStateSolver &other)

OFKTStateSolver &	operator= (OFKTStateSolver &&)=delete

bool	removeJoint (const std::string &name) override final
	Removes a joint from the graph. 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...

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

void	setState (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract_common::TransformMap &floating_joint_values={}) override final
	Set the current state of the solver. More...

void	setState (const std::unordered_map< std::string, double > &joint_values, const tesseract_common::TransformMap &floating_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, const tesseract_common::TransformMap &floating_joint_values={}) override final

void	setState (const tesseract_common::TransformMap &floating_joint_values) override final
	Set the current state of the floating joint values. More...

	~OFKTStateSolver () override=default

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

	MutableStateSolver (const MutableStateSolver &)=default

	MutableStateSolver (MutableStateSolver &&)=default

MutableStateSolver &	operator= (const MutableStateSolver &)=default

MutableStateSolver &	operator= (MutableStateSolver &&)=default

	~MutableStateSolver () override=default

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

StateSolver &	operator= (StateSolver &&)=default

	StateSolver ()=default

	StateSolver (const StateSolver &)=default

	StateSolver (StateSolver &&)=default

virtual	~StateSolver ()=default

Private Member Functions
void	addNewJointLimits (const std::vector< std::shared_ptr< const JointLimits >> &new_joint_limits)
	appends the new joint limits 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< std::shared_ptr< const JointLimits >> &new_joint_limits)
	Add a node to the tree. More...

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

void	clear ()

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

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

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	moveLinkHelper (std::vector< std::shared_ptr< const JointLimits >> &new_joint_limits, const Joint &joint)
	This a helper function for moving a link. 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	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	replaceJointHelper (std::vector< std::shared_ptr< const JointLimits >> &new_joint_limits, const Joint &joint)
	This is a helper function for replacing a joint. More...

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

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

Private Attributes
std::vector< std::string >	active_joint_names_

SceneState	current_state_

std::vector< std::string >	floating_joint_names_

std::vector< std::string >	joint_names_

tesseract_common::KinematicLimits	limits_

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

std::vector< std::string >	link_names_

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

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

int	revision_ { 0 }

std::unique_ptr< OFKTNode >	root_

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.

Definition at line 56 of file ofkt_state_solver.h.

Member Typedef Documentation

◆ ConstPtr

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

Definition at line 64 of file ofkt_state_solver.h.

◆ ConstUPtr

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

Definition at line 66 of file ofkt_state_solver.h.

◆ Ptr

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

Definition at line 63 of file ofkt_state_solver.h.

◆ UPtr

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

Definition at line 65 of file ofkt_state_solver.h.

Constructor & Destructor Documentation

◆ OFKTStateSolver() [1/4]

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

Definition at line 155 of file ofkt_state_solver.cpp.

◆ OFKTStateSolver() [2/4]

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

Definition at line 160 of file ofkt_state_solver.cpp.

◆ ~OFKTStateSolver()

tesseract_scene_graph::OFKTStateSolver::~OFKTStateSolver ( )

overridedefault

◆ OFKTStateSolver() [3/4]

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

Definition at line 168 of file ofkt_state_solver.cpp.

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

Definition at line 538 of file ofkt_state_solver.cpp.

◆ addNewJointLimits()

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

private

appends the new joint limits

Parameters

new_joint_limits

Definition at line 1281 of file ofkt_state_solver.cpp.

◆ 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< std::shared_ptr< const JointLimits >> &	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

Definition at line 1146 of file ofkt_state_solver.cpp.

◆ calcJacobianHelper()

Eigen::MatrixXd tesseract_scene_graph::OFKTStateSolver::calcJacobianHelper	(	const std::unordered_map< std::string, double > &	joints,
		const std::string &	link_name,
		const tesseract_common::TransformMap &	floating_joint_values = `{}`
	)		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
floating_joint_values	The floating joint values

Returns: The calculated geometric jacobian

Definition at line 411 of file ofkt_state_solver.cpp.

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

Definition at line 773 of file ofkt_state_solver.cpp.

◆ changeJointJerkLimits()

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

finaloverridevirtual

Changes the jerk limits associated with a joint.

Parameters

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

Returns

Implements tesseract_scene_graph::MutableStateSolver.

Definition at line 791 of file ofkt_state_solver.cpp.

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

Definition at line 718 of file ofkt_state_solver.cpp.

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

Definition at line 737 of file ofkt_state_solver.cpp.

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

Definition at line 755 of file ofkt_state_solver.cpp.

◆ clear()

void tesseract_scene_graph::OFKTStateSolver::clear ( )

private

Definition at line 203 of file ofkt_state_solver.cpp.

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

Definition at line 185 of file ofkt_state_solver.cpp.

◆ 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

Definition at line 83 of file ofkt_state_solver.cpp.

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

Definition at line 462 of file ofkt_state_solver.cpp.

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

Definition at line 480 of file ofkt_state_solver.cpp.

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

Definition at line 468 of file ofkt_state_solver.cpp.

◆ getFloatingJointNames()

std::vector< std::string > tesseract_scene_graph::OFKTStateSolver::getFloatingJointNames ( ) const

finaloverridevirtual

Get the vector of floating joint names.

Returns: A vector of joint names

Implements tesseract_scene_graph::StateSolver.

Definition at line 456 of file ofkt_state_solver.cpp.

◆ getJacobian() [1/3]

Eigen::MatrixXd tesseract_scene_graph::OFKTStateSolver::getJacobian	(	const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const std::string &	link_name,
		const tesseract_common::TransformMap &	floating_joint_values = `{}`
	)		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.

Definition at line 362 of file ofkt_state_solver.cpp.

◆ 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 tesseract_common::TransformMap &	floating_joint_values = `{}`
	)		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.

Definition at line 378 of file ofkt_state_solver.cpp.

◆ 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 tesseract_common::TransformMap &	floating_joint_values = `{}`
	)		const

finaloverridevirtual

Implements tesseract_scene_graph::StateSolver.

Definition at line 394 of file ofkt_state_solver.cpp.

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

Definition at line 450 of file ofkt_state_solver.cpp.

◆ getLimits()

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

finaloverridevirtual

Getter for kinematic limits.

Returns: The kinematic limits

Implements tesseract_scene_graph::StateSolver.

Definition at line 532 of file ofkt_state_solver.cpp.

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

Definition at line 474 of file ofkt_state_solver.cpp.

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

Definition at line 521 of file ofkt_state_solver.cpp.

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

Definition at line 511 of file ofkt_state_solver.cpp.

◆ getRandomState()

SceneState tesseract_scene_graph::OFKTStateSolver::getRandomState ( ) const

finaloverridevirtual

Get the random state of the environment.

Returns: Environment state

Implements tesseract_scene_graph::StateSolver.

Definition at line 356 of file ofkt_state_solver.cpp.

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

Definition at line 526 of file ofkt_state_solver.cpp.

◆ getRevision()

int tesseract_scene_graph::OFKTStateSolver::getRevision ( ) const

finaloverridevirtual

Get the state solver revision number.

Returns: revision number

Implements tesseract_scene_graph::MutableStateSolver.

Definition at line 197 of file ofkt_state_solver.cpp.

◆ getState() [1/5]

SceneState tesseract_scene_graph::OFKTStateSolver::getState ( ) const

finaloverridevirtual

Get the current state of the scene.

Returns: The current state

Implements tesseract_scene_graph::StateSolver.

Definition at line 350 of file ofkt_state_solver.cpp.

◆ getState() [2/5]

SceneState tesseract_scene_graph::OFKTStateSolver::getState	(	const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const tesseract_common::TransformMap &	floating_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
floating_joint_values	The floating joint origin transform

Implements tesseract_scene_graph::StateSolver.

Definition at line 292 of file ofkt_state_solver.cpp.

◆ getState() [3/5]

SceneState tesseract_scene_graph::OFKTStateSolver::getState	(	const std::unordered_map< std::string, double > &	joint_values,
		const tesseract_common::TransformMap &	floating_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.
floating_joint_values	The floating joint origin transform

Returns: A the state of the environment

Implements tesseract_scene_graph::StateSolver.

Definition at line 308 of file ofkt_state_solver.cpp.

◆ getState() [4/5]

SceneState tesseract_scene_graph::OFKTStateSolver::getState	(	const std::vector< std::string > &	joint_names,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const tesseract_common::TransformMap &	floating_joint_values = `{}`
	)		const

finaloverridevirtual

Implements tesseract_scene_graph::StateSolver.

Definition at line 322 of file ofkt_state_solver.cpp.

◆ getState() [5/5]

SceneState tesseract_scene_graph::OFKTStateSolver::getState ( const tesseract_common::TransformMap & floating_joint_values ) const

finaloverridevirtual

Get the state given floating joint values.

Parameters

floating_joint_values The floating joint values to leverage

Returns: A the state of the environment

Implements tesseract_scene_graph::StateSolver.

Definition at line 339 of file ofkt_state_solver.cpp.

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

Definition at line 489 of file ofkt_state_solver.cpp.

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

Definition at line 505 of file ofkt_state_solver.cpp.

◆ initHelper()

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

private

Definition at line 966 of file ofkt_state_solver.cpp.

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

Definition at line 809 of file ofkt_state_solver.cpp.

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

Definition at line 498 of file ofkt_state_solver.cpp.

◆ 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

Definition at line 864 of file ofkt_state_solver.cpp.

◆ loadStaticLinkNamesRecursive()

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

private

load the static link names

Definition at line 891 of file ofkt_state_solver.cpp.

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

Definition at line 689 of file ofkt_state_solver.cpp.

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

Definition at line 599 of file ofkt_state_solver.cpp.

◆ moveLinkHelper()

void tesseract_scene_graph::OFKTStateSolver::moveLinkHelper	(	std::vector< std::shared_ptr< const JointLimits >> &	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

Definition at line 1008 of file ofkt_state_solver.cpp.

◆ operator=() [1/2]

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

Definition at line 170 of file ofkt_state_solver.cpp.

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

Definition at line 657 of file ofkt_state_solver.cpp.

◆ 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

Definition at line 1075 of file ofkt_state_solver.cpp.

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

Definition at line 625 of file ofkt_state_solver.cpp.

◆ 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

Definition at line 1249 of file ofkt_state_solver.cpp.

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

Definition at line 565 of file ofkt_state_solver.cpp.

◆ replaceJointHelper()

void tesseract_scene_graph::OFKTStateSolver::replaceJointHelper	(	std::vector< std::shared_ptr< const JointLimits >> &	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

Definition at line 1053 of file ofkt_state_solver.cpp.

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

Definition at line 191 of file ofkt_state_solver.cpp.

◆ setState() [1/4]

void tesseract_scene_graph::OFKTStateSolver::setState	(	const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const tesseract_common::TransformMap &	floating_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.

Definition at line 218 of file ofkt_state_solver.cpp.

◆ setState() [2/4]

void tesseract_scene_graph::OFKTStateSolver::setState	(	const std::unordered_map< std::string, double > &	joint_values,
		const tesseract_common::TransformMap &	floating_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.

Definition at line 238 of file ofkt_state_solver.cpp.

◆ setState() [3/4]

void tesseract_scene_graph::OFKTStateSolver::setState	(	const std::vector< std::string > &	joint_names,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const tesseract_common::TransformMap &	floating_joint_values = `{}`
	)

finaloverridevirtual

Implements tesseract_scene_graph::StateSolver.

Definition at line 258 of file ofkt_state_solver.cpp.

◆ setState() [4/4]

void tesseract_scene_graph::OFKTStateSolver::setState ( const tesseract_common::TransformMap & floating_joint_values )

finaloverridevirtual

Set the current state of the floating joint values.

Parameters

floating_joint_values The floating joint values to set

Implements tesseract_scene_graph::StateSolver.

Definition at line 280 of file ofkt_state_solver.cpp.

◆ 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

Definition at line 903 of file ofkt_state_solver.cpp.

◆ update() [2/2]

void tesseract_scene_graph::OFKTStateSolver::update	(	SceneState &	state,
		const OFKTNode *	node,
		const 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

Definition at line 925 of file ofkt_state_solver.cpp.

Friends And Related Function Documentation

◆ ofkt_builder

friend struct ofkt_builder

friend

Definition at line 293 of file ofkt_state_solver.h.

Member Data Documentation

◆ active_joint_names_

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

private

The active joint names

Definition at line 172 of file ofkt_state_solver.h.

◆ current_state_

SceneState tesseract_scene_graph::OFKTStateSolver::current_state_

private

Current state of the scene

Definition at line 170 of file ofkt_state_solver.h.

◆ floating_joint_names_

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

private

The floating joint names

Definition at line 173 of file ofkt_state_solver.h.

◆ joint_names_

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

private

The link names

Definition at line 171 of file ofkt_state_solver.h.

◆ limits_

tesseract_common::KinematicLimits tesseract_scene_graph::OFKTStateSolver::limits_

private

The kinematic limits

Definition at line 177 of file ofkt_state_solver.h.

◆ link_map_

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

private

The link name map to node

Definition at line 176 of file ofkt_state_solver.h.

◆ link_names_

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

private

The link names

Definition at line 174 of file ofkt_state_solver.h.

◆ mutex_

std::shared_mutex tesseract_scene_graph::OFKTStateSolver::mutex_

mutableprivate

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

Definition at line 182 of file ofkt_state_solver.h.

◆ nodes_

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

private

The joint name map to node

Definition at line 175 of file ofkt_state_solver.h.

◆ revision_

int tesseract_scene_graph::OFKTStateSolver::revision_ { 0 }

private

The revision number

Definition at line 179 of file ofkt_state_solver.h.

◆ root_

std::unique_ptr<OFKTNode> tesseract_scene_graph::OFKTStateSolver::root_

private

The root node of the tree

Definition at line 178 of file ofkt_state_solver.h.

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

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

◆ changeJointJerkLimits()

◆ changeJointOrigin()

◆ changeJointPositionLimits()

◆ changeJointVelocityLimits()

◆ clear()

◆ clone()

◆ cloneHelper()

◆ getActiveJointNames()

◆ getActiveLinkNames()

◆ getBaseLinkName()

◆ getFloatingJointNames()

◆ getJacobian() [1/3]

◆ getJacobian() [2/3]

◆ getJacobian() [3/3]

◆ getJointNames()

◆ getLimits()

◆ getLinkNames()

◆ getLinkTransform()

◆ getLinkTransforms()

◆ getRandomState()

◆ getRelativeLinkTransform()

◆ getRevision()

◆ getState() [1/5]

◆ getState() [2/5]

◆ getState() [3/5]

◆ getState() [4/5]

◆ getState() [5/5]

◆ 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/4]

◆ setState() [2/4]

◆ setState() [3/4]

◆ setState() [4/4]

◆ update() [1/2]

◆ update() [2/2]

Friends And Related Function Documentation

◆ ofkt_builder

Member Data Documentation

◆ active_joint_names_

◆ current_state_