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

Public Types

using Vertex = SceneGraph::vertex_descriptor
using Edge = SceneGraph::edge_descriptor
using Ptr = std::shared_ptr<SceneGraph>
using ConstPtr = std::shared_ptr<const SceneGraph>
using UPtr = std::unique_ptr<SceneGraph>
using ConstUPtr = std::unique_ptr<const SceneGraph>

Public Member Functions

 SceneGraph (const std::string &name="")
 SceneGraph (const SceneGraph &other)=delete
SceneGraph & operator= (const SceneGraph &other)=delete
 SceneGraph (SceneGraph &&other) noexcept
SceneGraph & operator= (SceneGraph &&other) noexcept
SceneGraph::UPtr clone () const
 Clone the scene graph.
void clear ()
 Clear the scene graph.
void setName (const std::string &name)
 Sets the graph name.
const std::string & getName () const
 Sets the graph name.
bool setRoot (const common::LinkId &id)
 Sets the root link name (aka. World Coordinate Frame).
const common::LinkId & getRoot () const
 Gets the root link name (aka. World Coordinate Frame).
bool addLink (const Link &link, bool replace_allowed=false)
 Adds a link to the graph.
bool addLink (const Link &link, const Joint &joint)
 Adds a link/joint to the graph.
std::shared_ptr< const Link > getLink (const common::LinkId &id) const
 Get a link in the graph.
std::vector< std::shared_ptr< const Link > > getLinks () const
 Get a vector links in the scene graph.
std::vector< std::shared_ptr< const Link > > getLeafLinks () const
 Get a vector leaf links in the scene graph.
bool removeLink (const common::LinkId &id, bool recursive=false)
 Removes a link from the graph.
bool moveLink (const Joint &joint)
 Move link defined by provided joint This deletes all inbound joints on the parent link defined by the joint.
void setLinkVisibility (const common::LinkId &id, bool visibility)
 Set a links visibility.
bool getLinkVisibility (const common::LinkId &id) const
 Get a given links visibility setting.
void setLinkCollisionEnabled (const common::LinkId &id, bool enabled)
 Set whether a link should be considered during collision checking.
bool getLinkCollisionEnabled (const common::LinkId &id) const
 Get whether a link should be considered during collision checking.
bool addJoint (const Joint &joint)
 Adds joint to the graph.
std::shared_ptr< const Joint > getJoint (const common::JointId &id) const
 Get a joint in the graph.
bool removeJoint (const common::JointId &id, bool recursive=false)
 Removes a joint from the graph.
bool moveJoint (const common::JointId &id, const common::LinkId &parent_link_id)
 Move joint to new parent link.
std::vector< std::shared_ptr< const Joint > > getJoints () const
 Get a vector of joints in the scene graph.
std::vector< std::shared_ptr< const Joint > > getActiveJoints () const
 Get a vector of active joints in the scene graph.
bool changeJointOrigin (const common::JointId &id, const Eigen::Isometry3d &new_origin)
 Changes the "origin" transform of the joint and recomputes the associated edge.
bool changeJointLimits (const common::JointId &id, const JointLimits &limits)
 Changes the limits of a joint. The JointLimits::Ptr remains the same, but the values passed in are assigned.
bool changeJointPositionLimits (const common::JointId &id, double lower, double upper)
 Changes the position limits associated with a joint.
bool changeJointVelocityLimits (const common::JointId &id, double limit)
 Changes the velocity limits associated with a joint.
bool changeJointAccelerationLimits (const common::JointId &id, double limit)
 Changes the acceleration limits associated with a joint.
bool changeJointJerkLimits (const common::JointId &id, double limit)
 Changes the jerk limits associated with a joint.
std::shared_ptr< const JointLimits > getJointLimits (const common::JointId &id)
 Gets the limits of the joint specified by name.
void setAllowedCollisionMatrix (std::shared_ptr< tesseract::common::AllowedCollisionMatrix > acm)
 Set the allowed collision matrix.
void addAllowedCollision (const common::LinkId &link_id1, const common::LinkId &link_id2, const std::string &reason)
 Disable collision between two collision objects.
void addAllowedCollision (const tesseract::common::LinkIdPair &pair, const std::string &reason)
 Add allowed collision for a canonically ordered link-id pair.
void removeAllowedCollision (const common::LinkId &link_id1, const common::LinkId &link_id2)
 Remove disabled collision pair from allowed collision matrix.
void removeAllowedCollision (const tesseract::common::LinkIdPair &pair)
 Remove disabled collision pair using a pre-built canonical pair.
void removeAllowedCollision (const common::LinkId &link_id)
 Remove disabled collision for any pair with link_name from allowed collision matrix.
void clearAllowedCollisions ()
 Remove all allowed collisions.
bool isCollisionAllowed (const tesseract::common::LinkIdPair &pair) const
 Check if a link pair is allowed to be in collision.
std::shared_ptr< const tesseract::common::AllowedCollisionMatrix > getAllowedCollisionMatrix () const
 Get the allowed collision matrix.
std::shared_ptr< tesseract::common::AllowedCollisionMatrix > getAllowedCollisionMatrix ()
 Get the allowed collision matrix.
std::shared_ptr< const Link > getSourceLink (const common::JointId &id) const
 Get the source link (parent link) for a joint.
std::shared_ptr< const Link > getTargetLink (const common::JointId &id) const
 Get the target link (child link) for a joint.
std::vector< std::shared_ptr< const Joint > > getInboundJoints (const common::LinkId &id) const
 Get inbound joints for a link.
std::vector< std::shared_ptr< const Joint > > getOutboundJoints (const common::LinkId &id) const
 Get outbound joints for a link.
bool isAcyclic () const
 Determine if the graph contains cycles.
bool isTree () const
 Determine if the graph is a tree.
bool isEmpty () const
 Check if the graph is empty.
std::vector< common::LinkId > getAdjacentLinkIds (const common::LinkId &id) const
 Get a vector of adjacent link ids provided a link name.
std::vector< common::LinkId > getInvAdjacentLinkIds (const common::LinkId &id) const
 Geta a vector of inverse adjacent link ids provided a link id.
std::vector< common::LinkId > getLinkChildrenIds (const common::LinkId &id) const
 Get all children for a given link id.
std::vector< common::LinkId > getJointChildrenIds (const common::JointId &id) const
 Get all children link ids for a given joint id.
std::unordered_map< common::LinkId, common::LinkId > getAdjacencyMap (const std::vector< common::LinkId > &link_ids) const
 Create mapping between links in the scene to the provided links if they are directly affected if the link moves.
std::vector< common::LinkId > getJointChildrenIds (const std::vector< common::JointId > &ids) const
 Get all children link ids for the given joint ids.
void saveDOT (const std::string &path) const
 Saves Graph as Graph Description Language (DOT).
ShortestPath getShortestPath (const common::LinkId &root, const common::LinkId &tip) const
 Get the shortest path between two links.
Vertex getVertex (const common::LinkId &id) const
 Get the graph vertex by name or ID.
Edge getEdge (const common::JointId &id) const
 Get the graph edge by joint ID.
bool insertSceneGraph (const tesseract::scene_graph::SceneGraph &scene_graph, const std::string &prefix="")
 Merge a graph into the current graph.
bool insertSceneGraph (const tesseract::scene_graph::SceneGraph &scene_graph, const tesseract::scene_graph::Joint &joint, const std::string &prefix="")
 Merge a graph into the current environment.
bool operator== (const SceneGraph &rhs) const
bool operator!= (const SceneGraph &rhs) const

Protected Member Functions

bool addLinkHelper (const std::shared_ptr< Link > &link_ptr, bool replace_allowed=false)
 Adds a link to the graph.
bool addJointHelper (const std::shared_ptr< Joint > &joint_ptr)
 Adds joint to the graph.

Friends

template<class Archive>
friend void::tesseract::scene_graph::serialize (Archive &ar, SceneGraph &obj)

Member Function Documentation

◆ clone()

SceneGraph::UPtr tesseract::scene_graph::SceneGraph::clone ( ) const

Clone the scene graph.

Returns
The cloned scene graph

◆ setName()

void tesseract::scene_graph::SceneGraph::setName ( const std::string & name)

Sets the graph name.

Parameters
nameThe name of the graph

◆ getName()

const std::string & tesseract::scene_graph::SceneGraph::getName ( ) const

Sets the graph name.

Parameters
nameThe name of the graph

◆ setRoot()

bool tesseract::scene_graph::SceneGraph::setRoot ( const common::LinkId & id)

Sets the root link name (aka. World Coordinate Frame).

Parameters
nameThe name of the link
Returns
Return False if a link does not exists, otherwise true

◆ getRoot()

const common::LinkId & tesseract::scene_graph::SceneGraph::getRoot ( ) const

Gets the root link name (aka. World Coordinate Frame).

Returns
The root link name

◆ addLink() [1/2]

bool tesseract::scene_graph::SceneGraph::addLink ( const Link & link,
bool replace_allowed = false )

Adds a link to the graph.

The first link added to the graph is set as the root by default. Use setRoot to change the root link of the graph.

Parameters
linkThe link to be added to the graph
replace_allowedIf true and the link exist it will be replaced
Returns
Return False if a link with the same name already exists and replace is not allowed, otherwise true

◆ addLink() [2/2]

bool tesseract::scene_graph::SceneGraph::addLink ( const Link & link,
const Joint & joint )

Adds a link/joint to the graph.

The first link added to the graph is set as the root by default. Use setRoot to change the root link of the graph.

Parameters
linkThe link to be added to the graph
jointThe associated joint to be added to the graph
Returns
Return False if a link with the same name allready exists, otherwise true

◆ getLink()

std::shared_ptr< const Link > tesseract::scene_graph::SceneGraph::getLink ( const common::LinkId & id) const

Get a link in the graph.

Parameters
nameThe name of the link
Returns
Return nullptr if link name does not exists, otherwise a pointer to the link

◆ getLinks()

std::vector< std::shared_ptr< const Link > > tesseract::scene_graph::SceneGraph::getLinks ( ) const

Get a vector links in the scene graph.

Returns
A vector of links

◆ getLeafLinks()

std::vector< std::shared_ptr< const Link > > tesseract::scene_graph::SceneGraph::getLeafLinks ( ) const

Get a vector leaf links in the scene graph.

Returns
A vector of links

◆ removeLink()

bool tesseract::scene_graph::SceneGraph::removeLink ( const common::LinkId & id,
bool recursive = false )

Removes a link from the graph.

Note: this will remove all inbound and outbound edges

Parameters
nameName of the link to be removed
recursiveIf true all children are removed if it only has a single joint
Returns
Return False if a link does not exists, otherwise true

◆ moveLink()

bool tesseract::scene_graph::SceneGraph::moveLink ( const Joint & joint)

Move link defined by provided joint This deletes all inbound joints on the parent link defined by the joint.

Parameters
jointThe joint defining the link move
Returns
Returns true if successful, otherwise false.

◆ setLinkVisibility()

void tesseract::scene_graph::SceneGraph::setLinkVisibility ( const common::LinkId & id,
bool visibility )

Set a links visibility.

Parameters
visibilityTrue if should be visible, otherwise false

◆ getLinkVisibility()

bool tesseract::scene_graph::SceneGraph::getLinkVisibility ( const common::LinkId & id) const

Get a given links visibility setting.

Returns
True if should be visible, otherwise false

◆ setLinkCollisionEnabled()

void tesseract::scene_graph::SceneGraph::setLinkCollisionEnabled ( const common::LinkId & id,
bool enabled )

Set whether a link should be considered during collision checking.

Parameters
enabledTrue if should be considered during collision checking, otherwise false

◆ getLinkCollisionEnabled()

bool tesseract::scene_graph::SceneGraph::getLinkCollisionEnabled ( const common::LinkId & id) const

Get whether a link should be considered during collision checking.

Returns
True if should be considered during collision checking, otherwise false

◆ addJoint()

bool tesseract::scene_graph::SceneGraph::addJoint ( const Joint & joint)

Adds joint to the graph.

Parameters
jointThe joint to be added
Returns
Return False if parent or child link does not exists and if joint name already exists in the graph, otherwise true

◆ getJoint()

std::shared_ptr< const Joint > tesseract::scene_graph::SceneGraph::getJoint ( const common::JointId & id) const

Get a joint in the graph.

Parameters
nameThe name of the joint
Returns
Return nullptr if joint name does not exists, otherwise a pointer to the joint

◆ removeJoint()

bool tesseract::scene_graph::SceneGraph::removeJoint ( const common::JointId & id,
bool recursive = false )

Removes a joint from the graph.

Parameters
nameName of the joint to be removed
recursiveIf true all children are removed if this this is the only joint of the child link
Returns
Return False if a joint does not exists, otherwise true

◆ moveJoint()

bool tesseract::scene_graph::SceneGraph::moveJoint ( const common::JointId & id,
const common::LinkId & parent_link_id )

Move joint to new parent link.

Parameters
nameName of the joint to move
parent_linkName of parent link to move to
Returns
Returns true if successful, otherwise false.

◆ getJoints()

std::vector< std::shared_ptr< const Joint > > tesseract::scene_graph::SceneGraph::getJoints ( ) const

Get a vector of joints in the scene graph.

Returns
A vector of joints

◆ getActiveJoints()

std::vector< std::shared_ptr< const Joint > > tesseract::scene_graph::SceneGraph::getActiveJoints ( ) const

Get a vector of active joints in the scene graph.

Returns
A vector of active joints

◆ changeJointOrigin()

bool tesseract::scene_graph::SceneGraph::changeJointOrigin ( const common::JointId & id,
const Eigen::Isometry3d & new_origin )

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

Parameters
nameName of the joint to be changed
new_originThe new transform associated with the joint
Returns
True if successful.

◆ changeJointLimits()

bool tesseract::scene_graph::SceneGraph::changeJointLimits ( const common::JointId & id,
const JointLimits & limits )

Changes the limits of a joint. The JointLimits::Ptr remains the same, but the values passed in are assigned.

Parameters
nameName of the joint to be changed
limitsThe new limits associated with the joint
Returns
True if successful.

◆ changeJointPositionLimits()

bool tesseract::scene_graph::SceneGraph::changeJointPositionLimits ( const common::JointId & id,
double lower,
double upper )

Changes the position limits associated with a joint.

Parameters
joint_nameName of the joint to be updated
limitsNew position limits to be set as the joint limits @returnTrue if successful.

◆ changeJointVelocityLimits()

bool tesseract::scene_graph::SceneGraph::changeJointVelocityLimits ( const common::JointId & id,
double limit )

Changes the velocity limits associated with a joint.

Parameters
joint_nameName of the joint to be updated
limitsNew velocity limits to be set as the joint limits
Returns

◆ changeJointAccelerationLimits()

bool tesseract::scene_graph::SceneGraph::changeJointAccelerationLimits ( const common::JointId & id,
double limit )

Changes the acceleration limits associated with a joint.

Parameters
joint_nameName of the joint to be updated
limitsNew acceleration limits to be set as the joint limits
Returns

◆ changeJointJerkLimits()

bool tesseract::scene_graph::SceneGraph::changeJointJerkLimits ( const common::JointId & id,
double limit )

Changes the jerk limits associated with a joint.

Parameters
joint_nameName of the joint to be updated
limitsNew jerk limits to be set as the joint limits
Returns

◆ getJointLimits()

std::shared_ptr< const JointLimits > tesseract::scene_graph::SceneGraph::getJointLimits ( const common::JointId & id)

Gets the limits of the joint specified by name.

Parameters
nameName of the joint which limits will be retrieved
Returns
Limits of the joint. Returns nullptr is joint is not found.

◆ setAllowedCollisionMatrix()

void tesseract::scene_graph::SceneGraph::setAllowedCollisionMatrix ( std::shared_ptr< tesseract::common::AllowedCollisionMatrix > acm)

Set the allowed collision matrix.

Parameters
acmThe allowed collision matrix to assign

◆ addAllowedCollision()

void tesseract::scene_graph::SceneGraph::addAllowedCollision ( const common::LinkId & link_id1,
const common::LinkId & link_id2,
const std::string & reason )

Disable collision between two collision objects.

Parameters
link_id1Collision object id
link_id2Collision object id
reasonThe reason for disabling collision

◆ removeAllowedCollision() [1/3]

void tesseract::scene_graph::SceneGraph::removeAllowedCollision ( const common::LinkId & link_id1,
const common::LinkId & link_id2 )

Remove disabled collision pair from allowed collision matrix.

Parameters
link_name1Collision object name
link_name2Collision object name

◆ removeAllowedCollision() [2/3]

void tesseract::scene_graph::SceneGraph::removeAllowedCollision ( const tesseract::common::LinkIdPair & pair)

Remove disabled collision pair using a pre-built canonical pair.

Forwarder to tesseract::common::AllowedCollisionMatrix::removeAllowedCollision(const LinkIdPair&). Useful when iterating an existing ACM and erasing by key without reconstructing LinkId objects from strings.

Parameters
pairCanonically ordered link-id pair (the map key)

◆ removeAllowedCollision() [3/3]

void tesseract::scene_graph::SceneGraph::removeAllowedCollision ( const common::LinkId & link_id)

Remove disabled collision for any pair with link_name from allowed collision matrix.

Parameters
link_nameCollision object name

◆ isCollisionAllowed()

bool tesseract::scene_graph::SceneGraph::isCollisionAllowed ( const tesseract::common::LinkIdPair & pair) const

Check if a link pair is allowed to be in collision.

Parameters
pairCanonically ordered link-id pair
Returns
True if the two links are allowed to be in collision, otherwise false

◆ getAllowedCollisionMatrix() [1/2]

std::shared_ptr< const tesseract::common::AllowedCollisionMatrix > tesseract::scene_graph::SceneGraph::getAllowedCollisionMatrix ( ) const

Get the allowed collision matrix.

Returns
AllowedCollisionMatrixConstPtr

◆ getAllowedCollisionMatrix() [2/2]

std::shared_ptr< tesseract::common::AllowedCollisionMatrix > tesseract::scene_graph::SceneGraph::getAllowedCollisionMatrix ( )

Get the allowed collision matrix.

Returns
AllowedCollisionMatrixPtr

◆ getSourceLink()

std::shared_ptr< const Link > tesseract::scene_graph::SceneGraph::getSourceLink ( const common::JointId & id) const

Get the source link (parent link) for a joint.

Parameters
idThe joint ID
Returns
The source link

◆ getTargetLink()

std::shared_ptr< const Link > tesseract::scene_graph::SceneGraph::getTargetLink ( const common::JointId & id) const

Get the target link (child link) for a joint.

Parameters
joint_nameThe name of the joint
Returns
The target link

◆ getInboundJoints()

std::vector< std::shared_ptr< const Joint > > tesseract::scene_graph::SceneGraph::getInboundJoints ( const common::LinkId & id) const

Get inbound joints for a link.

The inbound joints are all joints that have the link identified as the child link

Parameters
link_nameThe name of the link
Returns
Vector of joints

◆ getOutboundJoints()

std::vector< std::shared_ptr< const Joint > > tesseract::scene_graph::SceneGraph::getOutboundJoints ( const common::LinkId & id) const

Get outbound joints for a link.

The outbound joints are all joins that have the link identified as the parent link

Parameters
idThe link ID
Returns
Vector of joints

◆ isAcyclic()

bool tesseract::scene_graph::SceneGraph::isAcyclic ( ) const

Determine if the graph contains cycles.

Returns
True if graph is acyclic (no cycles) otherwise false

◆ isTree()

bool tesseract::scene_graph::SceneGraph::isTree ( ) const

Determine if the graph is a tree.

Returns
True if graph is tree otherwise false

◆ isEmpty()

bool tesseract::scene_graph::SceneGraph::isEmpty ( ) const

Check if the graph is empty.

Returns
True if empty, otherwise false

◆ getAdjacentLinkIds()

std::vector< LinkId > tesseract::scene_graph::SceneGraph::getAdjacentLinkIds ( const common::LinkId & id) const

Get a vector of adjacent link ids provided a link name.

Parameters
idId of link
Returns
A vector of adjacent link ids

◆ getInvAdjacentLinkIds()

std::vector< LinkId > tesseract::scene_graph::SceneGraph::getInvAdjacentLinkIds ( const common::LinkId & id) const

Geta a vector of inverse adjacent link ids provided a link id.

Parameters
id
Returns

◆ getLinkChildrenIds()

std::vector< LinkId > tesseract::scene_graph::SceneGraph::getLinkChildrenIds ( const common::LinkId & id) const

Get all children for a given link id.

Parameters
idId of Link
Returns
A vector of child link ids

◆ getJointChildrenIds() [1/2]

std::vector< LinkId > tesseract::scene_graph::SceneGraph::getJointChildrenIds ( const common::JointId & id) const

Get all children link ids for a given joint id.

Parameters
idId of joint
Returns
A vector of child link ids

◆ getAdjacencyMap()

std::unordered_map< LinkId, LinkId > tesseract::scene_graph::SceneGraph::getAdjacencyMap ( const std::vector< common::LinkId > & link_ids) const

Create mapping between links in the scene to the provided links if they are directly affected if the link moves.

Parameters
link_namesThe links to map other links to
Returns
A map of affected links to on of the provided link ids.

◆ getJointChildrenIds() [2/2]

std::vector< common::LinkId > tesseract::scene_graph::SceneGraph::getJointChildrenIds ( const std::vector< common::JointId > & ids) const

Get all children link ids for the given joint ids.

Todo
Need to create custom visitor so already process joint_ids do not get processed again.
Parameters
idsIds of joints
Returns
A vector of child link ids

◆ saveDOT()

void tesseract::scene_graph::SceneGraph::saveDOT ( const std::string & path) const

Saves Graph as Graph Description Language (DOT).

Parameters
pathThe file path

◆ getShortestPath()

ShortestPath tesseract::scene_graph::SceneGraph::getShortestPath ( const common::LinkId & root,
const common::LinkId & tip ) const

Get the shortest path between two links.

Parameters
rootThe base link
tipThe tip link
Returns
The shortest path between the two links

◆ getVertex()

SceneGraph::Vertex tesseract::scene_graph::SceneGraph::getVertex ( const common::LinkId & id) const

Get the graph vertex by name or ID.

Parameters
nameThe vertex/link name
Returns
Graph Vertex

◆ getEdge()

SceneGraph::Edge tesseract::scene_graph::SceneGraph::getEdge ( const common::JointId & id) const

Get the graph edge by joint ID.

Parameters
idThe edge/joint ID
Returns
Graph Edge

◆ insertSceneGraph() [1/2]

bool tesseract::scene_graph::SceneGraph::insertSceneGraph ( const tesseract::scene_graph::SceneGraph & scene_graph,
const std::string & prefix = "" )

Merge a graph into the current graph.

Parameters
scene_graphConst ref to the graph to be merged (said graph will be copied)
prefixstring Will prepend to every link and joint of the merged graph
Returns
Return False if any link or joint name collides with current environment, otherwise True Merge a sub-graph into the current environment, considering that the root of the merged graph is attached to the root of the environment by a fixed joint and no displacement. Every joint and link of the sub-graph will be copied into the environment graph. The prefix argument is meant to allow adding multiple copies of the same sub-graph with different names

◆ insertSceneGraph() [2/2]

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

Merge a graph into the current environment.

Parameters
scene_graphConst ref to the graph to be merged (said graph will be copied)
jointThe joint that connects current environment with the inserted graph
prefixstring Will prepend to every link and joint of the merged graph
Returns
Return False if any link or joint name collides with current environment, otherwise True Merge a sub-graph into the current environment. Every joint and link of the sub-graph will be copied into the environment graph. The prefix argument is meant to allow adding multiple copies of the same sub-graph with different names

◆ addLinkHelper()

bool tesseract::scene_graph::SceneGraph::addLinkHelper ( const std::shared_ptr< Link > & link_ptr,
bool replace_allowed = false )
protected

Adds a link to the graph.

The first link added to the graph is set as the root by default. Use setRoot to change the root link of the graph.

Parameters
link_ptrShared pointer to the link to be added to the graph
replace_allowedIf true and the link exist it will be replaced
Returns
Return False if a link with the same name already exists and replace is not allowed, otherwise true

◆ addJointHelper()

bool tesseract::scene_graph::SceneGraph::addJointHelper ( const std::shared_ptr< Joint > & joint_ptr)
protected

Adds joint to the graph.

Parameters
joint_ptrShared pointer to the joint to be added
Returns
Return False if parent or child link does not exists and if joint name already exists in the graph, otherwise true