Tesseract Scene Graph Package¶
Background¶
This package contains the scene graph and parsers. The scene graph is used to manage the connectivity of the environment. The scene graph inherits from boost graph so you are able to leverage boost graph utilities for searching.
Scene Graph (Tree)¶
Scene Graph (Acyclic)¶
Features¶
- Links - Get, Add, Remove, Modify, Show/Hide, and Enable/Disable Collision
- Joints - Get, Add, Remove, Move and Modify
- Allowed Collision Matrix - Get, Add, Remove
- Graph Functions
- Get Inbound/Outbound Joints for Link
- Check if acyclic
- Check if tree
- Get Adjacent/InvAdjacent Links for Joint
- Utility Functions
- Save to Graph to Graph Description Language (DOT)
- Get shortest path between two Links
- Parsers
- URDF Parser
- SRDF Parser
- KDL Parser
- Mesh Parser
Examples¶
Building A Scene Graph¶
#include <console_bridge/console.h>
#include <tesseract_scene_graph/graph.h>
#include <iostream>
using namespace tesseract_scene_graph;
std::string toString(const SceneGraph::Path& 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);
// Create scene graph
SceneGraph g;
// Create links
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
g.addLink(link_1);
g.addLink(link_2);
g.addLink(link_3);
g.addLink(link_4);
g.addLink(link_5);
// Create joints
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
g.addJoint(joint_1);
g.addJoint(joint_2);
g.addJoint(joint_3);
g.addJoint(joint_4);
// Check getAdjacentLinkNames Method
std::vector<std::string> adjacent_links = g.getAdjacentLinkNames("link_3");
for (const auto& adj : adjacent_links)
CONSOLE_BRIDGE_logInform(adj.c_str());
// Check getInvAdjacentLinkNames Method
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());
// Check getLinkChildrenNames
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());
// Check getJointChildrenNames
child_link_names = g.getJointChildrenNames("joint_1");
for (const auto& child_link : child_link_names)
CONSOLE_BRIDGE_logInform(child_link.c_str());
// Save Graph
g.saveDOT("/tmp/graph_acyclic_tree_example.dot");
// Test if the graph is Acyclic
bool is_acyclic = g.isAcyclic();
CONSOLE_BRIDGE_logInform(toString(is_acyclic).c_str());
// Test if the graph is Tree
bool is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
// Test for unused links
Link link_6("link_6");
g.addLink(link_6);
is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
// Remove unused link
g.removeLink(link_6.getName());
is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
// Add new joint
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 new graph
g.saveDOT("/tmp/graph_acyclic_not_tree_example.dot");
// Test again if the graph is Acyclic
is_acyclic = g.isAcyclic();
std::cout << toString(is_acyclic).c_str();
// Test again if the graph is Tree
is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
// Get Shortest Path
SceneGraph::Path path = g.getShortestPath("link_1", "link_4");
CONSOLE_BRIDGE_logInform(toString(path).c_str());
}
Example Explanation¶
Create Scene Graph¶
SceneGraph g;
Add Links¶
Create the links. The links are able to be configured see Link documentation.
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 the links to the scene graph
g.addLink(link_1);
g.addLink(link_2);
g.addLink(link_3);
g.addLink(link_4);
g.addLink(link_5);
Add Joints¶
Create the joints. The links are able to be configured see Joint documentation.
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 the joints to the scene graph_acyclic_tree_example
g.addJoint(joint_1);
g.addJoint(joint_2);
g.addJoint(joint_3);
g.addJoint(joint_4);
Inspect Scene Graph¶
Get the adjacent links for link_3 and print to terminal
std::vector<std::string> adjacent_links = g.getAdjacentLinkNames("link_3");
for (const auto& adj : adjacent_links)
CONSOLE_BRIDGE_logInform(adj.c_str());
Get the inverse adjacent links for link_3 and print to terminal
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());
Get child link names for link link_3 and print to terminal
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());
Get child link names for joint joint_1 and print to terminal
child_link_names = g.getJointChildrenNames("joint_1");
for (const auto& child_link : child_link_names)
CONSOLE_BRIDGE_logInform(child_link.c_str());
Save the graph to a file for visualization
g.saveDOT("/tmp/graph_acyclic_tree_example.dot");
Test if the graph is Acyclic and print to terminal
bool is_acyclic = g.isAcyclic();
CONSOLE_BRIDGE_logInform(toString(is_acyclic).c_str());
Test if the graph is a tree and print to terminal
bool is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
Detect Unused Links¶
First add a link but do not create joint and check if it is a tree. It should return false because the link is not associated with a joint.
Link link_6("link_6");
g.addLink(link_6);
is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
Remove link and check if it is a tree. It should return true.
g.removeLink(link_6.getName());
is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
Create Acyclic Graph¶
Add joint connecting link_5 and link_4 to create an Acyclic graph_acyclic_tree_example
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 Acyclic graph
g.saveDOT("/tmp/graph_acyclic_not_tree_example.dot");
Test to confirm it is acyclic, should return true.
is_acyclic = g.isAcyclic();
std::cout << toString(is_acyclic).c_str();
Test if it is a tree, should return false.
is_tree = g.isTree();
CONSOLE_BRIDGE_logInform(toString(is_tree).c_str());
Get Shortest Path¶
SceneGraph::Path path = g.getShortestPath("link_1", "link_4");
CONSOLE_BRIDGE_logInform(toString(path).c_str());
Create Scene Graph from URDF¶
Parse SRDF adding Allowed Collision Matrix to Graph¶
#include <console_bridge/console.h>
#include <tesseract_scene_graph/graph.h>
#include <tesseract_scene_graph/parser/srdf_parser.h>
#include <tesseract_scene_graph/utils.h>
#include <iostream>
using namespace tesseract_scene_graph;
std::string toString(const SceneGraph::Path& path)
{
std::stringstream ss;
ss << path;
return ss.str();
}
std::string toString(bool b) { return b ? "true" : "false"; }
// Define a resource locator function
std::string locateResource(const std::string& url)
{
std::string mod_url = url;
if (url.find("package://tesseract_support") == 0)
{
mod_url.erase(0, strlen("package://tesseract_support"));
size_t pos = mod_url.find("/");
if (pos == std::string::npos)
{
return std::string();
}
std::string package = mod_url.substr(0, pos);
mod_url.erase(0, pos);
std::string package_path = std::string(TESSERACT_SUPPORT_DIR);
if (package_path.empty())
{
return std::string();
}
mod_url = package_path + mod_url;
}
return mod_url;
}
int main(int /*argc*/, char** /*argv*/)
{
std::string srdf_file = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf";
// Create scene graph
ResourceLocatorFn locator = locateResource;
SceneGraph g;
g.setName("kuka_lbr_iiwa_14_r820");
Link base_link("base_link");
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");
Link link_6("link_6");
Link link_7("link_7");
Link tool0("tool0");
g.addLink(base_link);
g.addLink(link_1);
g.addLink(link_2);
g.addLink(link_3);
g.addLink(link_4);
g.addLink(link_5);
g.addLink(link_6);
g.addLink(link_7);
g.addLink(tool0);
Joint base_joint("base_joint");
base_joint.parent_link_name = "base_link";
base_joint.child_link_name = "link_1";
base_joint.type = JointType::FIXED;
g.addJoint(base_joint);
Joint joint_1("joint_1");
joint_1.parent_link_name = "link_1";
joint_1.child_link_name = "link_2";
joint_1.type = JointType::REVOLUTE;
g.addJoint(joint_1);
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::REVOLUTE;
g.addJoint(joint_2);
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::REVOLUTE;
g.addJoint(joint_3);
Joint joint_4("joint_4");
joint_4.parent_to_joint_origin_transform.translation()(1) = 1.25;
joint_4.parent_link_name = "link_4";
joint_4.child_link_name = "link_5";
joint_4.type = JointType::REVOLUTE;
g.addJoint(joint_4);
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_6";
joint_5.type = JointType::REVOLUTE;
g.addJoint(joint_5);
Joint joint_6("joint_6");
joint_6.parent_to_joint_origin_transform.translation()(1) = 1.25;
joint_6.parent_link_name = "link_6";
joint_6.child_link_name = "link_7";
joint_6.type = JointType::REVOLUTE;
g.addJoint(joint_6);
Joint joint_tool0("joint_tool0");
joint_tool0.parent_link_name = "link_7";
joint_tool0.child_link_name = "tool0";
joint_tool0.type = JointType::FIXED;
g.addJoint(joint_tool0);
// Parse the srdf
SRDFModel srdf;
bool success = srdf.initFile(g, srdf_file);
CONSOLE_BRIDGE_logInform("SRDF loaded: %s", toString(success).c_str());
processSRDFAllowedCollisions(g, srdf);
AllowedCollisionMatrix::ConstPtr acm = g.getAllowedCollisionMatrix();
const AllowedCollisionMatrix::AllowedCollisionEntries& acm_entries = acm->getAllAllowedCollisions();
CONSOLE_BRIDGE_logInform("ACM Number of entries: %d", acm_entries.size());
}
Example Explanation¶
Create Resource Locator¶
Because this is ROS agnostic you need to provide a resource locator for interpreting package:/.
std::string locateResource(const std::string& url)
{
std::string mod_url = url;
if (url.find("package://tesseract_support") == 0)
{
mod_url.erase(0, strlen("package://tesseract_support"));
size_t pos = mod_url.find("/");
if (pos == std::string::npos)
{
return std::string();
}
std::string package = mod_url.substr(0, pos);
mod_url.erase(0, pos);
std::string package_path = std::string(TESSERACT_SUPPORT_DIR);
if (package_path.empty())
{
return std::string();
}
mod_url = package_path + mod_url;
}
return mod_url;
}
Load URDF and SRDF¶
Get the file path to the URDF and SRDF file
Create Scene Graph from URDF
ResourceLocatorFn locator = locateResource;
SceneGraph g;
g.setName("kuka_lbr_iiwa_14_r820");
Link base_link("base_link");
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");
Link link_6("link_6");
Link link_7("link_7");
Link tool0("tool0");
g.addLink(base_link);
g.addLink(link_1);
g.addLink(link_2);
g.addLink(link_3);
g.addLink(link_4);
g.addLink(link_5);
g.addLink(link_6);
g.addLink(link_7);
g.addLink(tool0);
Joint base_joint("base_joint");
base_joint.parent_link_name = "base_link";
base_joint.child_link_name = "link_1";
base_joint.type = JointType::FIXED;
g.addJoint(base_joint);
Joint joint_1("joint_1");
joint_1.parent_link_name = "link_1";
joint_1.child_link_name = "link_2";
joint_1.type = JointType::REVOLUTE;
g.addJoint(joint_1);
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::REVOLUTE;
g.addJoint(joint_2);
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::REVOLUTE;
g.addJoint(joint_3);
Joint joint_4("joint_4");
joint_4.parent_to_joint_origin_transform.translation()(1) = 1.25;
joint_4.parent_link_name = "link_4";
joint_4.child_link_name = "link_5";
joint_4.type = JointType::REVOLUTE;
g.addJoint(joint_4);
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_6";
joint_5.type = JointType::REVOLUTE;
g.addJoint(joint_5);
Joint joint_6("joint_6");
joint_6.parent_to_joint_origin_transform.translation()(1) = 1.25;
joint_6.parent_link_name = "link_6";
joint_6.child_link_name = "link_7";
joint_6.type = JointType::REVOLUTE;
g.addJoint(joint_6);
Joint joint_tool0("joint_tool0");
joint_tool0.parent_link_name = "link_7";
joint_tool0.child_link_name = "tool0";
joint_tool0.type = JointType::FIXED;
g.addJoint(joint_tool0);
Parse SRDF
Add Allowed Collision Matrix to Scene Graph
Methods for getting Allowed Collision Matrix from Scene Graph
Parse Mesh from file¶
Example Explanation¶
Parse Mesh from File¶
Mesh files can contain multiple meshes. This is a critical difference between MoveIt which merges all shapes in to a single triangle list for collision checking. By keeping each mesh independent, each will have its own bounding box and if you want to convert to a convex hull you will get a closer representation of the geometry.