Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
graph.h
Go to the documentation of this file.
1
26#ifndef TESSERACT_SCENE_GRAPH_GRAPH_H
27#define TESSERACT_SCENE_GRAPH_GRAPH_H
28
31#include <boost/graph/adjacency_list.hpp> // for customizable graphs
32#include <boost/graph/directed_graph.hpp> // A subclass to provide reasonable arguments to adjacency_list for a typical directed graph
33#include <boost/graph/properties.hpp>
34#include <boost/graph/depth_first_search.hpp>
35#include <boost/graph/breadth_first_search.hpp>
36#include <boost/serialization/access.hpp>
37#include <string>
38#include <list>
39#include <unordered_map>
41
45
46#ifndef SWIG
47
48/* definition of basic boost::graph properties */
49namespace boost
50{
52{
54};
56{
58};
60{
62};
64{
66};
68{
70};
71
73BOOST_INSTALL_PROPERTY(vertex, link_visible);
74BOOST_INSTALL_PROPERTY(vertex, link_collision_enabled);
77} // namespace boost
78
79#endif // SWIG
80
82{
83#ifndef SWIG
84
87 boost::property<boost::graph_name_t, std::string, boost::property<boost::graph_root_t, std::string>>;
88
90using VertexProperty = boost::property<
93 boost::property<boost::vertex_link_visible_t, bool, boost::property<boost::vertex_link_collision_enabled_t, bool>>>;
94
100using EdgeProperty = boost::property<boost::edge_joint_t, Joint::Ptr, boost::property<boost::edge_weight_t, double>>;
101
102using Graph = boost::
103 adjacency_list<boost::listS, boost::listS, boost::bidirectionalS, VertexProperty, EdgeProperty, GraphProperty>;
104
105#endif // SWIG
106
109{
111 std::vector<std::string> links;
112
114 std::vector<std::string> joints;
115
117 std::vector<std::string> active_joints;
118};
119
121#ifndef SWIG
122 : public Graph,
123 public boost::noncopyable
124#endif // SWIG
125{
126public:
127 using Vertex = SceneGraph::vertex_descriptor;
128 using Edge = SceneGraph::edge_descriptor;
129
130 using Ptr = std::shared_ptr<SceneGraph>;
131 using ConstPtr = std::shared_ptr<const SceneGraph>;
132 using UPtr = std::unique_ptr<SceneGraph>;
133 using ConstUPtr = std::unique_ptr<const SceneGraph>;
134
135 SceneGraph(const std::string& name = "");
136 ~SceneGraph() = default;
137 // SceneGraphs are non-copyable
138 SceneGraph(const SceneGraph& other) = delete;
139 SceneGraph& operator=(const SceneGraph& other) = delete;
140
141 SceneGraph(SceneGraph&& other) noexcept;
142 SceneGraph& operator=(SceneGraph&& other) noexcept;
143
148 SceneGraph::UPtr clone() const;
149
151 void clear();
152
157 void setName(const std::string& name);
158
163 const std::string& getName() const;
164
170 bool setRoot(const std::string& name);
171
176 const std::string& getRoot() const;
177
187 bool addLink(const Link& link, bool replace_allowed = false);
188
198 bool addLink(const Link& link, const Joint& joint);
199
205 Link::ConstPtr getLink(const std::string& name) const;
206
211 std::vector<Link::ConstPtr> getLinks() const;
212
217 std::vector<Link::ConstPtr> getLeafLinks() const;
218
228 bool removeLink(const std::string& name, bool recursive = false);
229
236 bool moveLink(const Joint& joint);
237
242 void setLinkVisibility(const std::string& name, bool visibility);
243
248 bool getLinkVisibility(const std::string& name) const;
249
254 void setLinkCollisionEnabled(const std::string& name, bool enabled);
255
260 bool getLinkCollisionEnabled(const std::string& name) const;
261
268 bool addJoint(const Joint& joint);
269
275 Joint::ConstPtr getJoint(const std::string& name) const;
276
283 bool removeJoint(const std::string& name, bool recursive = false);
284
291 bool moveJoint(const std::string& name, const std::string& parent_link);
292
297 std::vector<Joint::ConstPtr> getJoints() const;
298
303 std::vector<Joint::ConstPtr> getActiveJoints() const;
304
310 bool changeJointOrigin(const std::string& name, const Eigen::Isometry3d& new_origin);
311
318 bool changeJointLimits(const std::string& name, const JointLimits& limits);
319
326 bool changeJointPositionLimits(const std::string& name, double lower, double upper);
327
334 bool changeJointVelocityLimits(const std::string& name, double limit);
335
342 bool changeJointAccelerationLimits(const std::string& name, double limit);
343
349 JointLimits::ConstPtr getJointLimits(const std::string& name);
350
357 void addAllowedCollision(const std::string& link_name1, const std::string& link_name2, const std::string& reason);
358
364 void removeAllowedCollision(const std::string& link_name1, const std::string& link_name2);
365
370 void removeAllowedCollision(const std::string& link_name);
371
374
381 bool isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const;
382
388
394
400 Link::ConstPtr getSourceLink(const std::string& joint_name) const;
401
407 Link::ConstPtr getTargetLink(const std::string& joint_name) const;
408
418 std::vector<Joint::ConstPtr> getInboundJoints(const std::string& link_name) const;
419
429 std::vector<Joint::ConstPtr> getOutboundJoints(const std::string& link_name) const;
430
435 bool isAcyclic() const;
436
441 bool isTree() const;
442
447 bool isEmpty() const;
448
454 std::vector<std::string> getAdjacentLinkNames(const std::string& name) const;
455
461 std::vector<std::string> getInvAdjacentLinkNames(const std::string& name) const;
462
468 std::vector<std::string> getLinkChildrenNames(const std::string& name) const;
469
475 std::vector<std::string> getJointChildrenNames(const std::string& name) const;
476
483 std::unordered_map<std::string, std::string> getAdjacencyMap(const std::vector<std::string>& link_names) const;
484
491 std::vector<std::string> getJointChildrenNames(const std::vector<std::string>& names) const;
492
497 void saveDOT(const std::string& path) const;
498
505 ShortestPath getShortestPath(const std::string& root, const std::string& tip) const;
506
507#ifndef SWIG
513 Vertex getVertex(const std::string& name) const;
514
520 Edge getEdge(const std::string& name) const;
521#endif
532 bool insertSceneGraph(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& prefix = "");
533
545 const tesseract_scene_graph::Joint& joint,
546 const std::string& prefix = "");
547
548 bool operator==(const SceneGraph& rhs) const;
549 bool operator!=(const SceneGraph& rhs) const;
550
551protected:
561 bool addLinkHelper(const Link::Ptr& link_ptr, bool replace_allowed = false);
562
569 bool addJointHelper(const Joint::Ptr& joint_ptr);
570
571private:
572 std::unordered_map<std::string, std::pair<Link::Ptr, Vertex>> link_map_;
573 std::unordered_map<std::string, std::pair<Joint::Ptr, Edge>> joint_map_;
575
578
579 struct cycle_detector : public boost::dfs_visitor<>
580 {
581 cycle_detector(bool& ascyclic) : ascyclic_(ascyclic) {}
582
583 template <class e, class g>
584 void back_edge(e, g&)
585 {
586 ascyclic_ = false;
587 }
588
589 protected:
591 };
592
593 struct tree_detector : public boost::dfs_visitor<>
594 {
595 tree_detector(bool& tree) : tree_(tree) {}
596
597 template <class u, class g>
598 void discover_vertex(u vertex, const g& graph)
599 {
600 auto num_in_edges = static_cast<int>(boost::in_degree(vertex, graph));
601
602 if (num_in_edges > 1)
603 {
604 tree_ = false;
605 return;
606 }
607
608 // Check if more that one root exist
609 if (num_in_edges == 0 && found_root_)
610 {
611 tree_ = false;
612 return;
613 }
614
615 if (num_in_edges == 0)
616 found_root_ = true;
617
618 // Check if not vertex is unused.
619 if (num_in_edges == 0 && boost::out_degree(vertex, graph) == 0)
620 {
621 tree_ = false;
622 return;
623 }
624 }
625
626 template <class e, class g>
627 void back_edge(e, const g&)
628 {
629 tree_ = false;
630 }
631
632 protected:
633 bool& tree_;
634 bool found_root_{ false };
635 };
636
637 struct children_detector : public boost::default_bfs_visitor
638 {
639 children_detector(std::vector<std::string>& children) : children_(children) {}
640
641 template <class u, class g>
642 void discover_vertex(u vertex, const g& graph)
643 {
644 children_.push_back(boost::get(boost::vertex_link, graph)[vertex]->getName());
645 }
646
647 protected:
648 std::vector<std::string>& children_;
649 };
650
651 struct adjacency_detector : public boost::default_bfs_visitor
652 {
653 adjacency_detector(std::unordered_map<std::string, std::string>& adjacency_map,
654 std::map<Vertex, boost::default_color_type>& color_map,
655 const std::string& base_link_name,
656 const std::vector<std::string>& terminate_on_links)
657 : adjacency_map_(adjacency_map)
658 , color_map_(color_map)
660 , terminate_on_links_(terminate_on_links)
661 {
662 }
663
664 template <class u, class g>
665 void examine_vertex(u vertex, const g& graph)
666 {
667 for (auto vd : boost::make_iterator_range(adjacent_vertices(vertex, graph)))
668 {
669 std::string adj_link = boost::get(boost::vertex_link, graph)[vd]->getName();
670 if (std::find(terminate_on_links_.begin(), terminate_on_links_.end(), adj_link) != terminate_on_links_.end())
671 color_map_[vd] = boost::default_color_type::black_color;
672 }
673 }
674
675 template <class u, class g>
676 void discover_vertex(u vertex, const g& graph)
677 {
678 std::string adj_link = boost::get(boost::vertex_link, graph)[vertex]->getName();
680 }
681
682 protected:
683 std::unordered_map<std::string, std::string>& adjacency_map_;
684 std::map<Vertex, boost::default_color_type>& color_map_;
685 const std::string& base_link_name_;
686 const std::vector<std::string>& terminate_on_links_;
687 };
688
697 std::vector<std::string> getLinkChildrenHelper(Vertex start_vertex) const
698 {
699 const auto& graph = static_cast<const Graph&>(*this);
700 std::vector<std::string> child_link_names;
701
702 std::map<Vertex, size_t> index_map;
703 boost::associative_property_map<std::map<Vertex, size_t>> prop_index_map(index_map);
704
705 std::map<Vertex, boost::default_color_type> color_map;
706 boost::associative_property_map<std::map<Vertex, boost::default_color_type>> prop_color_map(color_map);
707
708 int c = 0;
709 Graph::vertex_iterator i, iend;
710 for (boost::tie(i, iend) = boost::vertices(graph); i != iend; ++i, ++c)
711 boost::put(prop_index_map, *i, c);
712
713 children_detector vis(child_link_names);
714 // NOLINTNEXTLINE
715 boost::breadth_first_search(
716 graph,
717 start_vertex,
718 boost::visitor(vis).root_vertex(start_vertex).vertex_index_map(prop_index_map).color_map(prop_color_map));
719
720 return child_link_names;
721 }
722
724 template <class Archive>
725 void save(Archive& ar, const unsigned int version) const; // NOLINT
726
727 template <class Archive>
728 void load(Archive& ar, const unsigned int version); // NOLINT
729
730 template <class Archive>
731 void serialize(Archive& ar, const unsigned int version); // NOLINT
732};
733
734inline std::ostream& operator<<(std::ostream& os, const ShortestPath& path)
735{
736 os << "Links:" << std::endl;
737 for (const auto& l : path.links)
738 os << " " << l << std::endl;
739
740 os << "Joints:" << std::endl;
741 for (const auto& j : path.joints)
742 os << " " << j << std::endl;
743
744 os << "Active Joints:" << std::endl;
745 for (const auto& j : path.active_joints)
746 os << " " << j << std::endl;
747 return os;
748}
749
750} // namespace tesseract_scene_graph
751
752#include <boost/serialization/export.hpp>
753#include <boost/serialization/tracking.hpp>
754BOOST_CLASS_EXPORT_KEY2(tesseract_scene_graph::SceneGraph, "SceneGraph")
755
756#endif // TESSERACT_SCENE_GRAPH_GRAPH_H
std::shared_ptr< const AllowedCollisionMatrix > ConstPtr
Definition: allowed_collision_matrix.h:30
std::shared_ptr< AllowedCollisionMatrix > Ptr
Definition: allowed_collision_matrix.h:29
std::shared_ptr< const JointLimits > ConstPtr
Definition: joint.h:82
Definition: joint.h:281
std::shared_ptr< const Joint > ConstPtr
Definition: joint.h:286
std::shared_ptr< Joint > Ptr
Definition: joint.h:285
Definition: graph.h:125
std::unordered_map< std::string, std::pair< Link::Ptr, Vertex > > link_map_
Definition: graph.h:572
void setName(const std::string &name)
Sets the graph name.
Definition: graph.cpp:129
bool operator!=(const SceneGraph &rhs) const
Definition: graph.cpp:1141
bool changeJointPositionLimits(const std::string &name, double lower, double upper)
Changes the position limits associated with a joint.
Definition: graph.cpp:543
bool removeJoint(const std::string &name, bool recursive=false)
Removes a joint from the graph.
Definition: graph.cpp:417
bool getLinkVisibility(const std::string &name) const
Get a given links visibility setting.
Definition: graph.cpp:322
void setLinkVisibility(const std::string &name, bool visibility)
Set a links visibility.
Definition: graph.cpp:315
void rebuildLinkAndJointMaps()
The rebuild the link and joint map by extraction information from the graph.
Definition: graph.cpp:1097
bool addJointHelper(const Joint::Ptr &joint_ptr)
Adds joint to the graph.
Definition: graph.cpp:349
void clearAllowedCollisions()
Remove all allowed collisions.
Definition: graph.cpp:642
tesseract_common::AllowedCollisionMatrix::ConstPtr getAllowedCollisionMatrix() const
Get the allowed collision matrix.
Definition: graph.cpp:649
std::shared_ptr< SceneGraph > Ptr
Definition: graph.h:130
std::vector< std::string > getJointChildrenNames(const std::string &name) const
Get all children link names for a given joint name.
Definition: graph.cpp:775
Edge getEdge(const std::string &name) const
Get the graph edge by name.
Definition: graph.cpp:966
bool changeJointOrigin(const std::string &name, const Eigen::Isometry3d &new_origin)
Changes the "origin" transform of the joint and recomputes the associated edge.
Definition: graph.cpp:491
Link::ConstPtr getTargetLink(const std::string &joint_name) const
Get the target link (child link) for a joint.
Definition: graph.cpp:660
bool removeLink(const std::string &name, bool recursive=false)
Removes a link from the graph.
Definition: graph.cpp:247
void setLinkCollisionEnabled(const std::string &name, bool enabled)
Set whether a link should be considered during collision checking.
Definition: graph.cpp:329
SceneGraph::UPtr clone() const
Clone the scene graph.
Definition: graph.cpp:99
ShortestPath getShortestPath(const std::string &root, const std::string &tip) const
Get the shortest path between two links.
Definition: graph.cpp:856
SceneGraph::edge_descriptor Edge
Definition: graph.h:128
std::unique_ptr< const SceneGraph > ConstUPtr
Definition: graph.h:133
SceneGraph(const SceneGraph &other)=delete
bool changeJointAccelerationLimits(const std::string &name, double limit)
Changes the acceleration limits associated with a joint.
Definition: graph.cpp:587
bool changeJointVelocityLimits(const std::string &name, double limit)
Changes the velocity limits associated with a joint.
Definition: graph.cpp:566
std::vector< std::string > getLinkChildrenHelper(Vertex start_vertex) const
Get the children of a vertex starting with start_vertex.
Definition: graph.h:697
std::vector< Link::ConstPtr > getLinks() const
Get a vector links in the scene graph.
Definition: graph.cpp:224
void addAllowedCollision(const std::string &link_name1, const std::string &link_name2, const std::string &reason)
Disable collision between two collision objects.
Definition: graph.cpp:628
std::unordered_map< std::string, std::string > getAdjacencyMap(const std::vector< std::string > &link_names) const
Create mapping between links in the scene to the provided links if they are directly affected if the ...
Definition: graph.cpp:796
void serialize(Archive &ar, const unsigned int version)
Definition: graph.cpp:1162
std::unordered_map< std::string, std::pair< Joint::Ptr, Edge > > joint_map_
Definition: graph.h:573
std::vector< Link::ConstPtr > getLeafLinks() const
Get a vector leaf links in the scene graph.
Definition: graph.cpp:234
bool isAcyclic() const
Determine if the graph contains cycles.
Definition: graph.cpp:709
std::vector< std::string > getLinkChildrenNames(const std::string &name) const
Get all children for a given link name.
Definition: graph.cpp:765
bool getLinkCollisionEnabled(const std::string &name) const
Get whether a link should be considered during collision checking.
Definition: graph.cpp:336
void load(Archive &ar, const unsigned int version)
Definition: graph.cpp:1152
bool addJoint(const Joint &joint)
Adds joint to the graph.
Definition: graph.cpp:343
std::shared_ptr< const SceneGraph > ConstPtr
Definition: graph.h:131
const std::string & getRoot() const
Gets the root link name (aka. World Coordinate Frame)
Definition: graph.cpp:151
bool isEmpty() const
Check if the graph is empty.
Definition: graph.cpp:707
SceneGraph & operator=(const SceneGraph &other)=delete
bool addLink(const Link &link, bool replace_allowed=false)
Adds a link to the graph.
Definition: graph.cpp:156
bool isCollisionAllowed(const std::string &link_name1, const std::string &link_name2) const
Check if two links are allowed to be in collision.
Definition: graph.cpp:644
std::unique_ptr< SceneGraph > UPtr
Definition: graph.h:132
bool insertSceneGraph(const tesseract_scene_graph::SceneGraph &scene_graph, const std::string &prefix="")
Merge a graph into the current graph.
Definition: graph.cpp:1006
const std::string & getName() const
Sets the graph name.
Definition: graph.cpp:134
bool setRoot(const std::string &name)
Sets the root link name (aka. World Coordinate Frame)
Definition: graph.cpp:139
bool addLinkHelper(const Link::Ptr &link_ptr, bool replace_allowed=false)
Adds a link to the graph.
Definition: graph.cpp:185
JointLimits::ConstPtr getJointLimits(const std::string &name)
Gets the limits of the joint specified by name.
Definition: graph.cpp:614
std::vector< Joint::ConstPtr > getJoints() const
Get a vector of joints in the scene graph.
Definition: graph.cpp:468
SceneGraph::vertex_descriptor Vertex
Definition: graph.h:127
friend class boost::serialization::access
Definition: graph.h:723
Vertex getVertex(const std::string &name) const
Get the graph vertex by name.
Definition: graph.cpp:957
void clear()
Clear the scene graph.
Definition: graph.cpp:121
bool changeJointLimits(const std::string &name, const JointLimits &limits)
Changes the limits of a joint. The JointLimits::Ptr remains the same, but the values passed in are as...
Definition: graph.cpp:514
bool isTree() const
Determine if the graph is a tree.
Definition: graph.cpp:727
bool operator==(const SceneGraph &rhs) const
Definition: graph.cpp:1121
void save(Archive &ar, const unsigned int version) const
Definition: graph.cpp:1144
Link::ConstPtr getLink(const std::string &name) const
Get a link in the graph.
Definition: graph.cpp:215
tesseract_common::AllowedCollisionMatrix::Ptr acm_
Definition: graph.h:574
std::vector< Joint::ConstPtr > getOutboundJoints(const std::string &link_name) const
Get outbound joints for a link.
Definition: graph.cpp:687
bool moveJoint(const std::string &name, const std::string &parent_link)
Move joint to new parent link.
Definition: graph.cpp:440
void saveDOT(const std::string &path) const
Saves Graph as Graph Description Language (DOT)
Definition: graph.cpp:825
bool moveLink(const Joint &joint)
Move link defined by provided joint This deletes all inbound joints on the parent link defined by the...
Definition: graph.cpp:291
void removeAllowedCollision(const std::string &link_name1, const std::string &link_name2)
Remove disabled collision pair from allowed collision matrix.
Definition: graph.cpp:635
Link::ConstPtr getSourceLink(const std::string &joint_name) const
Get the source link (parent link) for a joint.
Definition: graph.cpp:653
Joint::ConstPtr getJoint(const std::string &name) const
Get a joint in the graph.
Definition: graph.cpp:408
std::vector< Joint::ConstPtr > getInboundJoints(const std::string &link_name) const
Get inbound joints for a link.
Definition: graph.cpp:667
std::vector< std::string > getInvAdjacentLinkNames(const std::string &name) const
Geta a vectpr pf inverse adjacent link names provided a link name.
Definition: graph.cpp:755
std::vector< std::string > getAdjacentLinkNames(const std::string &name) const
Get a vector of adjacent link names provided a link name.
Definition: graph.cpp:745
std::vector< Joint::ConstPtr > getActiveJoints() const
Get a vector of active joints in the scene graph.
Definition: graph.cpp:478
results link_names[0]
Definition: collision_core_unit.cpp:146
std::string base_link_name
Definition: ikfast_kinematics_7dof_unit.cpp:52
auto scene_graph
Definition: ikfast_kinematics_7dof_unit.cpp:51
Common Tesseract Macros.
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
Definition: create_convex_hull.cpp:36
Definition: serialization.h:45
edge_joint_t
Definition: graph.h:64
@ edge_joint
Definition: graph.h:65
BOOST_INSTALL_PROPERTY(vertex, link)
vertex_link_visible_t
Definition: graph.h:56
@ vertex_link_visible
Definition: graph.h:57
graph_root_t
Definition: graph.h:68
@ graph_root
Definition: graph.h:69
vertex_link_t
Definition: graph.h:52
@ vertex_link
Definition: graph.h:53
vertex_link_collision_enabled_t
Definition: graph.h:60
@ vertex_link_collision_enabled
Definition: graph.h:61
Definition: graph.h:82
boost::property< boost::vertex_link_t, Link::Ptr, boost::property< boost::vertex_link_visible_t, bool, boost::property< boost::vertex_link_collision_enabled_t, bool > > > VertexProperty
Defines the boost graph vertex property.
Definition: graph.h:93
boost::property< boost::graph_name_t, std::string, boost::property< boost::graph_root_t, std::string > > GraphProperty
Defines the boost graph property.
Definition: graph.h:87
boost::property< boost::edge_joint_t, Joint::Ptr, boost::property< boost::edge_weight_t, double > > EdgeProperty
EdgeProperty.
Definition: graph.h:100
boost::adjacency_list< boost::listS, boost::listS, boost::bidirectionalS, VertexProperty, EdgeProperty, GraphProperty > Graph
Definition: graph.h:103
std::ostream & operator<<(std::ostream &os, const ShortestPath &path)
Definition: graph.h:734
std::unordered_map< std::string, std::string > & adjacency_map_
Definition: graph.h:683
void examine_vertex(u vertex, const g &graph)
Definition: graph.h:665
const std::string & base_link_name_
Definition: graph.h:685
std::map< Vertex, boost::default_color_type > & color_map_
Definition: graph.h:684
const std::vector< std::string > & terminate_on_links_
Definition: graph.h:686
adjacency_detector(std::unordered_map< std::string, std::string > &adjacency_map, std::map< Vertex, boost::default_color_type > &color_map, const std::string &base_link_name, const std::vector< std::string > &terminate_on_links)
Definition: graph.h:653
void discover_vertex(u vertex, const g &graph)
Definition: graph.h:676
std::vector< std::string > & children_
Definition: graph.h:648
void discover_vertex(u vertex, const g &graph)
Definition: graph.h:642
children_detector(std::vector< std::string > &children)
Definition: graph.h:639
cycle_detector(bool &ascyclic)
Definition: graph.h:581
void back_edge(e, g &)
Definition: graph.h:584
void back_edge(e, const g &)
Definition: graph.h:627
void discover_vertex(u vertex, const g &graph)
Definition: graph.h:598
tree_detector(bool &tree)
Definition: graph.h:595
Holds the shortest path information.
Definition: graph.h:109
std::vector< std::string > joints
A list of joints along the shortest path.
Definition: graph.h:114
std::vector< std::string > active_joints
A list of active joints along the shortest path.
Definition: graph.h:117
std::vector< std::string > links
a list of links along the shortest path
Definition: graph.h:111
joint_1 limits
Definition: tesseract_scene_graph_joint_unit.cpp:153
j lower
Definition: tesseract_scene_graph_joint_unit.cpp:39
JointDynamics j
Definition: tesseract_scene_graph_joint_unit.cpp:15
joint_1 mimic joint_name
Definition: tesseract_scene_graph_joint_unit.cpp:163
j upper
Definition: tesseract_scene_graph_joint_unit.cpp:40
auto vis
Definition: tesseract_scene_graph_serialization_unit.cpp:143
auto graph
Definition: tesseract_srdf_serialization_unit.cpp:153
SceneGraph g
Definition: tesseract_srdf_unit.cpp:239
tesseract_common::fs::path path
Definition: tesseract_srdf_unit.cpp:1992