Tesseract 0.28.4
Loading...
Searching...
No Matches
tesseract::scene_graph::KDLStateSolver Class Reference
Inheritance diagram for tesseract::scene_graph::KDLStateSolver:

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 (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.
void setState (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract::common::JointIdTransformMap &floating_joint_values={}) override final
 Set the current state of the solver.
void setState (const SceneState::JointValues &joint_values, const tesseract::common::JointIdTransformMap &floating_joint_values={}) override final
 Set the current state using a JointId-keyed map.
void setState (const std::vector< tesseract::common::JointId > &joint_ids, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract::common::JointIdTransformMap &floating_joint_values={}) override final
 Set the current state using a vector of JointIds.
void setState (const tesseract::common::JointIdTransformMap &floating_joint_values) override final
 Set the current state of the floating joint values.
SceneState getState (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract::common::JointIdTransformMap &floating_joint_values={}) const override final
 Get the state of the solver given the joint values.
SceneState getState (const SceneState::JointValues &joint_values, const tesseract::common::JointIdTransformMap &floating_joint_values={}) const override final
 Get the state using a JointId-keyed map.
SceneState getState (const std::vector< tesseract::common::JointId > &joint_ids, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract::common::JointIdTransformMap &floating_joint_values={}) const override final
 Get the state using a vector of JointIds.
SceneState getState (const tesseract::common::JointIdTransformMap &floating_joint_values) const override final
 Get the state given floating joint values.
SceneState getState () const override final
 Get the current state of the scene.
void getLinkTransforms (tesseract::common::LinkIdTransformMap &link_transforms, const std::vector< tesseract::common::JointId > &joint_ids, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract::common::JointIdTransformMap &floating_joint_values) const override final
 Get the link transforms of the scene for a given set or subset of joint values.
void getLinkTransforms (tesseract::common::LinkIdTransformMap &link_transforms, const std::vector< tesseract::common::JointId > &joint_ids, const Eigen::Ref< const Eigen::VectorXd > &joint_values) const override final
 Get link transforms without applying floating joint values.
SceneState getRandomState () const override final
 Get the random state of the environment.
Eigen::MatrixXd getJacobian (const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract::common::LinkId &link_id, const tesseract::common::JointIdTransformMap &floating_joint_values={}) const override final
 Get the jacobian for a link identified by LinkId.
Eigen::MatrixXd getJacobian (const std::vector< tesseract::common::JointId > &joint_ids, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const tesseract::common::LinkId &link_id, const tesseract::common::JointIdTransformMap &floating_joint_values={}) const override final
 Get the jacobian for a given set or subset of joint IDs.
std::vector< tesseract::common::JointId > getJointIds () const override final
 Get the vector of joint IDs.
std::vector< tesseract::common::JointId > getFloatingJointIds () const override final
 Get the vector of floating joint IDs.
std::vector< tesseract::common::JointId > getActiveJointIds () const override final
 Get the vector of active joint IDs which align with the limits.
tesseract::common::LinkId getBaseLinkId () const override final
 Get the base link ID.
std::vector< tesseract::common::LinkId > getLinkIds () const override final
 Get the vector of link IDs.
std::vector< tesseract::common::LinkId > getActiveLinkIds () const override final
 Get the vector of active link IDs.
std::vector< tesseract::common::LinkId > getStaticLinkIds () const override final
 Get a vector of static link IDs.
bool isActiveLinkId (const tesseract::common::LinkId &link_id) const override final
 Check if link is an active link.
bool hasLinkId (const tesseract::common::LinkId &link_id) const override final
 Check if link id exists.
tesseract::common::VectorIsometry3d getLinkTransforms () const override final
 Get all of the links transforms.
Eigen::Isometry3d getLinkTransform (const tesseract::common::LinkId &link_id) const override final
 Get the transform corresponding to the link.
Eigen::Isometry3d getRelativeLinkTransform (const tesseract::common::LinkId &from_link_id, const tesseract::common::LinkId &to_link_id) const override final
 Get transform between two links using the current state.
tesseract::common::KinematicLimits getLimits () const override final
 Getter for kinematic limits.
Public Member Functions inherited from tesseract::scene_graph::StateSolver
 StateSolver (const StateSolver &)=default
StateSolver & operator= (const StateSolver &)=default
 StateSolver (StateSolver &&)=default
StateSolver & operator= (StateSolver &&)=default

Member Function Documentation

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

◆ setState() [1/4]

void tesseract::scene_graph::KDLStateSolver::setState ( const Eigen::Ref< const Eigen::VectorXd > & joint_values,
const tesseract::common::JointIdTransformMap & floating_joint_values = {} )
finaloverridevirtual

Set the current state of the solver.

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

Parameters
joint_valuesThe joint values

Implements tesseract::scene_graph::StateSolver.

◆ setState() [2/4]

void tesseract::scene_graph::KDLStateSolver::setState ( const SceneState::JointValues & joint_values,
const tesseract::common::JointIdTransformMap & floating_joint_values = {} )
finaloverridevirtual

Set the current state using a JointId-keyed map.

Implements tesseract::scene_graph::StateSolver.

◆ setState() [3/4]

void tesseract::scene_graph::KDLStateSolver::setState ( const std::vector< tesseract::common::JointId > & joint_ids,
const Eigen::Ref< const Eigen::VectorXd > & joint_values,
const tesseract::common::JointIdTransformMap & floating_joint_values = {} )
finaloverridevirtual

Set the current state using a vector of JointIds.

Implements tesseract::scene_graph::StateSolver.

◆ setState() [4/4]

void tesseract::scene_graph::KDLStateSolver::setState ( const tesseract::common::JointIdTransformMap & floating_joint_values)
finaloverridevirtual

Set the current state of the floating joint values.

Parameters
floating_joint_valuesThe floating joint values to set

Implements tesseract::scene_graph::StateSolver.

◆ getState() [1/5]

SceneState tesseract::scene_graph::KDLStateSolver::getState ( const Eigen::Ref< const Eigen::VectorXd > & joint_values,
const tesseract::common::JointIdTransformMap & 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 getJointIds

Parameters
joint_valuesThe joint values
floating_joint_valuesThe floating joint origin transform

Implements tesseract::scene_graph::StateSolver.

◆ getState() [2/5]

SceneState tesseract::scene_graph::KDLStateSolver::getState ( const SceneState::JointValues & joint_values,
const tesseract::common::JointIdTransformMap & floating_joint_values = {} ) const
finaloverridevirtual

Get the state using a JointId-keyed map.

This does not change the internal state of the solver.

Implements tesseract::scene_graph::StateSolver.

◆ getState() [3/5]

SceneState tesseract::scene_graph::KDLStateSolver::getState ( const std::vector< tesseract::common::JointId > & joint_ids,
const Eigen::Ref< const Eigen::VectorXd > & joint_values,
const tesseract::common::JointIdTransformMap & floating_joint_values = {} ) const
finaloverridevirtual

Get the state using a vector of JointIds.

Implements tesseract::scene_graph::StateSolver.

◆ getState() [4/5]

SceneState tesseract::scene_graph::KDLStateSolver::getState ( const tesseract::common::JointIdTransformMap & floating_joint_values) const
finaloverridevirtual

Get the state given floating joint values.

Parameters
floating_joint_valuesThe floating joint values to leverage
Returns
A the state of the environment

Implements tesseract::scene_graph::StateSolver.

◆ getState() [5/5]

SceneState tesseract::scene_graph::KDLStateSolver::getState ( ) const
finaloverridevirtual

Get the current state of the scene.

Returns
The current state

Implements tesseract::scene_graph::StateSolver.

◆ getLinkTransforms() [1/3]

void tesseract::scene_graph::KDLStateSolver::getLinkTransforms ( tesseract::common::LinkIdTransformMap & link_transforms,
const std::vector< tesseract::common::JointId > & joint_ids,
const Eigen::Ref< const Eigen::VectorXd > & joint_values,
const tesseract::common::JointIdTransformMap & floating_joint_values ) const
finaloverridevirtual

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

This is provided to optimize motion planning where link_transforms are poplated multiple time

This does not change the internal state of the solver.

This overwrites the transform of every link in the solver's tree but does not clear link_transforms first; keys from a different tree persist. Reusing a scratch map without clearing is safe iff every key later read is a link of this solver.

Parameters
link_transformsThe link_transforms to populate with data.
joint_idsA list of joint IDs to change.
joint_valuesThe joint values
floating_joint_valuesThe floating joint origin transform

Implements tesseract::scene_graph::StateSolver.

◆ getLinkTransforms() [2/3]

void tesseract::scene_graph::KDLStateSolver::getLinkTransforms ( tesseract::common::LinkIdTransformMap & link_transforms,
const std::vector< tesseract::common::JointId > & joint_ids,
const Eigen::Ref< const Eigen::VectorXd > & joint_values ) const
finaloverridevirtual

Get link transforms without applying floating joint values.

This is provided to optimize motion planning where link_transforms are poplated multiple time

This does not change the internal state of the solver.

This overwrites the transform of every link in the solver's tree but does not clear link_transforms first; keys from a different tree persist. Reusing a scratch map without clearing is safe iff every key later read is a link of this solver.

Parameters
link_transformsThe link_transforms to populate with data.
joint_idsA list of joint IDs to change.
joint_valuesThe joint values

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.

◆ getJacobian() [1/2]

Eigen::MatrixXd tesseract::scene_graph::KDLStateSolver::getJacobian ( const Eigen::Ref< const Eigen::VectorXd > & joint_values,
const tesseract::common::LinkId & link_id,
const tesseract::common::JointIdTransformMap & floating_joint_values = {} ) const
finaloverridevirtual

Get the jacobian for a link identified by LinkId.

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

Parameters
joint_valuesThe joint values
link_idThe link ID to calculate the jacobian for
floating_joint_valuesThe floating joint origin transform

Implements tesseract::scene_graph::StateSolver.

◆ getJacobian() [2/2]

Eigen::MatrixXd tesseract::scene_graph::KDLStateSolver::getJacobian ( const std::vector< tesseract::common::JointId > & joint_ids,
const Eigen::Ref< const Eigen::VectorXd > & joint_values,
const tesseract::common::LinkId & link_id,
const tesseract::common::JointIdTransformMap & floating_joint_values = {} ) const
finaloverridevirtual

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

This does not change the internal state of the solver.

Parameters
joint_idsA list of joint IDs to change.
joint_valuesThe joint values
link_idThe link ID to calculate the jacobian for
floating_joint_valuesThe floating joint origin transform

Implements tesseract::scene_graph::StateSolver.

◆ getJointIds()

std::vector< JointId > tesseract::scene_graph::KDLStateSolver::getJointIds ( ) const
finaloverridevirtual

Get the vector of joint IDs.

Implements tesseract::scene_graph::StateSolver.

◆ getFloatingJointIds()

std::vector< JointId > tesseract::scene_graph::KDLStateSolver::getFloatingJointIds ( ) const
finaloverridevirtual

Get the vector of floating joint IDs.

Implements tesseract::scene_graph::StateSolver.

◆ getActiveJointIds()

std::vector< JointId > tesseract::scene_graph::KDLStateSolver::getActiveJointIds ( ) const
finaloverridevirtual

Get the vector of active joint IDs which align with the limits.

Implements tesseract::scene_graph::StateSolver.

◆ getBaseLinkId()

LinkId tesseract::scene_graph::KDLStateSolver::getBaseLinkId ( ) const
finaloverridevirtual

Get the base link ID.

Implements tesseract::scene_graph::StateSolver.

◆ getLinkIds()

std::vector< LinkId > tesseract::scene_graph::KDLStateSolver::getLinkIds ( ) const
finaloverridevirtual

Get the vector of link IDs.

Implements tesseract::scene_graph::StateSolver.

◆ getActiveLinkIds()

std::vector< LinkId > tesseract::scene_graph::KDLStateSolver::getActiveLinkIds ( ) const
finaloverridevirtual

Get the vector of active link IDs.

Implements tesseract::scene_graph::StateSolver.

◆ getStaticLinkIds()

std::vector< LinkId > tesseract::scene_graph::KDLStateSolver::getStaticLinkIds ( ) const
finaloverridevirtual

Get a vector of static link IDs.

Implements tesseract::scene_graph::StateSolver.

◆ isActiveLinkId()

bool tesseract::scene_graph::KDLStateSolver::isActiveLinkId ( const tesseract::common::LinkId & link_id) const
finaloverridevirtual

Check if link is an active link.

Parameters
link_idThe link id to check
Returns
True if active, otherwise false

Implements tesseract::scene_graph::StateSolver.

◆ hasLinkId()

bool tesseract::scene_graph::KDLStateSolver::hasLinkId ( const tesseract::common::LinkId & link_id) const
finaloverridevirtual

Check if link id exists.

Parameters
link_idThe link id to check for
Returns
True if it exists, otherwise false

Implements tesseract::scene_graph::StateSolver.

◆ getLinkTransforms() [3/3]

tesseract::common::VectorIsometry3d tesseract::scene_graph::KDLStateSolver::getLinkTransforms ( ) const
finaloverridevirtual

Get all of the links transforms.

Order should be the same as getLinkIds()

Returns
Get a vector of transforms for all links.

Implements tesseract::scene_graph::StateSolver.

◆ getLinkTransform()

Eigen::Isometry3d tesseract::scene_graph::KDLStateSolver::getLinkTransform ( const tesseract::common::LinkId & link_id) const
finaloverridevirtual

Get the transform corresponding to the link.

Returns
Transform and is identity when no transform is available.

Implements tesseract::scene_graph::StateSolver.

◆ getRelativeLinkTransform()

Eigen::Isometry3d tesseract::scene_graph::KDLStateSolver::getRelativeLinkTransform ( const tesseract::common::LinkId & from_link_id,
const tesseract::common::LinkId & to_link_id ) const
finaloverridevirtual

Get transform between two links using the current state.

Parameters
from_link_idThe link id the transform should be relative to
to_link_idThe link id to get transform
Returns
The relative transform = inv(Transform(from_link_id)) * Transform(to_link_id)

Implements tesseract::scene_graph::StateSolver.

◆ getLimits()

tesseract::common::KinematicLimits tesseract::scene_graph::KDLStateSolver::getLimits ( ) const
finaloverridevirtual

Getter for kinematic limits.

Returns
The kinematic limits

Implements tesseract::scene_graph::StateSolver.