Tesseract Scene Graph Building Example

Overview

This example demonstrates how to build and manipulate a scene graph, the fundamental data structure that represents the kinematic relationships between robot links and joints. A scene graph encodes how a robot is constructed: which links are connected, what constraints govern their motion, and how they relate spatially. Understanding scene graphs is essential for motion planning, collision checking, and robot simulation.

The example builds a kinematic structure, queries adjacency relationships, validates graph topology, and demonstrates how to find paths between links.

Key Concepts

• Links: Rigid bodies in the robot. Each link has a name and can have visual/collision geometry associated with it. Think of a link as a solid piece of the robot (arm segment, gripper jaw, end effector, etc.).
• Joints: Connections between links that define the kinematic relationships. Each joint has a parent link, child link, type (fixed, revolute, prismatic, etc.), and a transform from parent to child.
• Joint Types:
  • FIXED: Rigid connection, no motion
  • REVOLUTE: Rotational motion around one axis
  • CONTINUOUS: Revolute without limits (can spin indefinitely)
  • PRISMATIC: Linear motion along one axis
  • PLANAR: 2D motion (x, y, rotation)
  • FLOATING: 6DOF unconstrained motion
• Scene Graph: A directed graph representing the kinematic tree/chain. Nodes are links, edges are joints. A valid graph must be acyclic (no loops) and connected (all links reachable).
• Acyclic Graph: Contains no cycles (loops). All kinematic chains should be acyclic.
• Tree: An acyclic connected graph. Valid robot kinematic structures form trees where every link (except the root) has exactly one parent.

Typical Workflow

1. Create a scene graph - Initialize the kinematic structure container
2. Define links - Create Link objects representing rigid bodies
3. Add links to graph - Register links in the scene graph
4. Define joints - Create Joint objects with parent/child relationships and transforms
5. Add joints to graph - Connect links via joints
6. Validate topology - Check for cycles and connectivity
7. Query relationships - Find adjacent links, children, paths
8. Visualize (optional) - Save as DOT format for visualization

Creating and Populating the Graph

Start by creating an empty scene graph:

  SceneGraph g;

Create links (rigid bodies). Each link is identified by a unique name:

  Link link_1("link_1");
  Link link_2("link_2");
  Link link_3("link_3");
  Link link_4("link_4");
  Link link_5("link_5");

Add links to the graph:

  g.addLink(link_1);
  g.addLink(link_2);
  g.addLink(link_3);
  g.addLink(link_4);
  g.addLink(link_5);

Create joints that connect links. Each joint specifies:

• Name: Unique identifier
• Parent link: Which link this joint connects from
• Child link: Which link this joint connects to
• Type: How the child can move relative to parent
• Transform: Spatial relationship from parent to child

  Joint joint_1("joint_1");
  joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
  joint_1.parent_link_name = "link_1";
  joint_1.child_link_name = "link_2";
  joint_1.type = JointType::FIXED;
 
  Joint joint_2("joint_2");
  joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25;
  joint_2.parent_link_name = "link_2";
  joint_2.child_link_name = "link_3";
  joint_2.type = JointType::PLANAR;
 
  Joint joint_3("joint_3");
  joint_3.parent_to_joint_origin_transform.translation()(0) = 1.25;
  joint_3.parent_link_name = "link_3";
  joint_3.child_link_name = "link_4";
  joint_3.type = JointType::FLOATING;
 
  Joint joint_4("joint_4");
  joint_4.parent_to_joint_origin_transform.translation()(1) = 1.25;
  joint_4.parent_link_name = "link_2";
  joint_4.child_link_name = "link_5";
  joint_4.type = JointType::REVOLUTE;

Add joints to establish kinematic relationships:

  g.addJoint(joint_1);
  g.addJoint(joint_2);
  g.addJoint(joint_3);
  g.addJoint(joint_4);

Querying Graph Structure

Find links directly connected to a given link:

  std::vector<std::string> adjacent_links = g.getAdjacentLinkNames("link_3");
  for (const auto& adj : adjacent_links)
    CONSOLE_BRIDGE_logInform(adj.c_str());

Find parent links (inverse adjacent):

  std::vector<std::string> inv_adjacent_links = g.getInvAdjacentLinkNames("link_3");
  for (const auto& inv_adj : inv_adjacent_links)
    CONSOLE_BRIDGE_logInform(inv_adj.c_str());

Find all child links of a link:

  std::vector<std::string> child_link_names = g.getLinkChildrenNames("link_2");
  for (const auto& child_link : child_link_names)
    CONSOLE_BRIDGE_logInform(child_link.c_str());

Find all child links branching from a joint:

  child_link_names = g.getJointChildrenNames("joint_1");
  for (const auto& child_link : child_link_names)
    CONSOLE_BRIDGE_logInform(child_link.c_str());

Validating Graph Topology

Save the graph structure as a DOT file for visualization:

  g.saveDOT(tesseract::common::getTempPath() + "graph_acyclic_tree_example.dot");

Test if the graph is acyclic (no loops):

  bool is_acyclic = g.isAcyclic();
  CONSOLE_BRIDGE_logInform(toString(is_acyclic).c_str());

Test if the graph is a valid tree (acyclic + connected):

  bool is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());

Handling Disconnected Components

If you add a link without connecting it via a joint, the graph is no longer a tree:

  Link link_6("link_6");
  g.addLink(link_6);
  is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());

Remove disconnected links to restore tree structure:

  g.removeLink("link_6");
  is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());

Creating Cycles

Add a joint that creates a cycle (kinematic loop). This is valid for some mechanisms (like parallel robots) but creates a more complex graph:

  Joint joint_5("joint_5");
  joint_5.parent_to_joint_origin_transform.translation()(1) = 1.25;
  joint_5.parent_link_name = "link_5";
  joint_5.child_link_name = "link_4";
  joint_5.type = JointType::CONTINUOUS;
  g.addJoint(joint_5);

Save the modified graph:

  g.saveDOT(tesseract::common::getTempPath() + "graph_acyclic_not_tree_example.dot");

Verify the graph is still acyclic (it is—cycles are loops, not detected here):

  is_acyclic = g.isAcyclic();
  CONSOLE_BRIDGE_logInform(toString(is_acyclic).c_str());

But it is no longer a tree:

  is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());

Finding Paths

Find the shortest kinematic path between two links:

  ShortestPath path = g.getShortestPath("link_1", "link_4");
  CONSOLE_BRIDGE_logInform(toString(path).c_str());

This is useful for finding which joints affect the relative motion between two links.

Common Patterns

Building a robot model: Load links from a URDF file and extract the scene graph, or programmatically construct it as shown here.

Validating a model: Use isTree() to ensure your kinematic structure is valid.

Collision checking setup: Use query methods to determine which links can collide and which are always adjacent (connected by a joint).

Trajectory planning: Use path queries to understand kinematic dependencies when planning joint-space trajectories.

Kinematics chains: Use adjacency queries to traverse chains from based to end-effector.

Tips and Best Practices

• Root link: Typically a fixed base or world frame. Other links connect downstream from it.
• Unique names: All link and joint names must be unique within the graph.
• Validate early: Check isTree() after building to catch structural errors.
• Understand joint types: Choose appropriate types (FIXED for rigid, REVOLUTE for movable joints).
• Transforms matter: The parent_to_joint_origin_transform defines spatial relationships.
• DOT visualization: Save graphs as DOT files and view with Graphviz to debug structure.
• Kinematic loops: Some robots (parallel manipulators) intentionally have loops. Know your robot!

Integration with Other Systems

Scene graphs are used throughout Tesseract:

• Environment: Holds the scene graph and manages collision objects
• Kinematics: Uses graph traversal to compute forward kinematics
• Planning: Queries relationships to determine which joints affect which links
• Collision: Uses adjacency info to skip unnecessary checks between connected links

Summary

This example demonstrates the complete workflow for building a kinematic scene graph:

1. Create empty graph
2. Add links and joints
3. Validate graph topology (tree vs acyclic vs cyclic)
4. Query relationships (adjacent, children, paths)
5. Handle invalid configurations and fix them
6. Visualize structure for debugging

A valid scene graph is the foundation for all downstream motion planning and simulation operations in Tesseract.

Full source code

For reference, here is the complete source of this example:

#include <tesseract/common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <console_bridge/console.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract/scene_graph/graph.h>
#include <tesseract/scene_graph/link.h>
#include <tesseract/scene_graph/joint.h>
#include <tesseract/common/utils.h>
 
using namespace tesseract::scene_graph;
 
std::string toString(const ShortestPath& path)
{
  std::stringstream ss;
  ss << path;
  return ss.str();
}
 
std::string toString(bool b) { return b ? "true" : "false"; }
 
int main(int /*argc*/, char** /*argv*/)
{
  console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
 
  SceneGraph g;
 
  Link link_1("link_1");
  Link link_2("link_2");
  Link link_3("link_3");
  Link link_4("link_4");
  Link link_5("link_5");
 
  g.addLink(link_1);
  g.addLink(link_2);
  g.addLink(link_3);
  g.addLink(link_4);
  g.addLink(link_5);
 
  Joint joint_1("joint_1");
  joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
  joint_1.parent_link_name = "link_1";
  joint_1.child_link_name = "link_2";
  joint_1.type = JointType::FIXED;
 
  Joint joint_2("joint_2");
  joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25;
  joint_2.parent_link_name = "link_2";
  joint_2.child_link_name = "link_3";
  joint_2.type = JointType::PLANAR;
 
  Joint joint_3("joint_3");
  joint_3.parent_to_joint_origin_transform.translation()(0) = 1.25;
  joint_3.parent_link_name = "link_3";
  joint_3.child_link_name = "link_4";
  joint_3.type = JointType::FLOATING;
 
  Joint joint_4("joint_4");
  joint_4.parent_to_joint_origin_transform.translation()(1) = 1.25;
  joint_4.parent_link_name = "link_2";
  joint_4.child_link_name = "link_5";
  joint_4.type = JointType::REVOLUTE;
 
  g.addJoint(joint_1);
  g.addJoint(joint_2);
  g.addJoint(joint_3);
  g.addJoint(joint_4);
 
  std::vector<std::string> adjacent_links = g.getAdjacentLinkNames("link_3");
  for (const auto& adj : adjacent_links)
    CONSOLE_BRIDGE_logInform(adj.c_str());
 
  std::vector<std::string> inv_adjacent_links = g.getInvAdjacentLinkNames("link_3");
  for (const auto& inv_adj : inv_adjacent_links)
    CONSOLE_BRIDGE_logInform(inv_adj.c_str());
 
  std::vector<std::string> child_link_names = g.getLinkChildrenNames("link_2");
  for (const auto& child_link : child_link_names)
    CONSOLE_BRIDGE_logInform(child_link.c_str());
 
  child_link_names = g.getJointChildrenNames("joint_1");
  for (const auto& child_link : child_link_names)
    CONSOLE_BRIDGE_logInform(child_link.c_str());
 
  g.saveDOT(tesseract::common::getTempPath() + "graph_acyclic_tree_example.dot");
 
  bool is_acyclic = g.isAcyclic();
  CONSOLE_BRIDGE_logInform(toString(is_acyclic).c_str());
 
  bool is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
 
  Link link_6("link_6");
  g.addLink(link_6);
  is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
 
  g.removeLink("link_6");
  is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
 
  Joint joint_5("joint_5");
  joint_5.parent_to_joint_origin_transform.translation()(1) = 1.25;
  joint_5.parent_link_name = "link_5";
  joint_5.child_link_name = "link_4";
  joint_5.type = JointType::CONTINUOUS;
  g.addJoint(joint_5);
 
  g.saveDOT(tesseract::common::getTempPath() + "graph_acyclic_not_tree_example.dot");
 
  is_acyclic = g.isAcyclic();
  CONSOLE_BRIDGE_logInform(toString(is_acyclic).c_str());
 
  is_tree = g.isTree();
  CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
 
  ShortestPath path = g.getShortestPath("link_1", "link_4");
  CONSOLE_BRIDGE_logInform(toString(path).c_str());
}