26#ifndef TESSERACT_SCENE_GRAPH_GRAPH_H
27#define TESSERACT_SCENE_GRAPH_GRAPH_H
31#include <boost/graph/adjacency_list.hpp>
32#include <boost/graph/directed_graph.hpp>
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>
39#include <unordered_map>
87 boost::property<boost::graph_name_t, std::string, boost::property<boost::graph_root_t, std::string>>;
93 boost::property<boost::vertex_link_visible_t, bool, boost::property<boost::vertex_link_collision_enabled_t, bool>>>;
100using EdgeProperty = boost::property<boost::edge_joint_t, Joint::Ptr, boost::property<boost::edge_weight_t, double>>;
103 adjacency_list<boost::listS, boost::listS, boost::bidirectionalS, VertexProperty, EdgeProperty, GraphProperty>;
123 public boost::noncopyable
127 using Vertex = SceneGraph::vertex_descriptor;
128 using Edge = SceneGraph::edge_descriptor;
130 using Ptr = std::shared_ptr<SceneGraph>;
131 using ConstPtr = std::shared_ptr<const SceneGraph>;
132 using UPtr = std::unique_ptr<SceneGraph>;
163 const std::string&
getName()
const;
176 const std::string&
getRoot()
const;
187 bool addLink(
const Link& link,
bool replace_allowed =
false);
211 std::vector<Link::ConstPtr>
getLinks()
const;
291 bool moveJoint(
const std::string&
name,
const std::string& parent_link);
297 std::vector<Joint::ConstPtr>
getJoints()
const;
357 void addAllowedCollision(
const std::string& link_name1,
const std::string& link_name2,
const std::string& reason);
381 bool isCollisionAllowed(
const std::string& link_name1,
const std::string& link_name2)
const;
418 std::vector<Joint::ConstPtr>
getInboundJoints(
const std::string& link_name)
const;
429 std::vector<Joint::ConstPtr>
getOutboundJoints(
const std::string& link_name)
const;
497 void saveDOT(
const std::string& path)
const;
546 const std::string& prefix =
"");
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_;
583 template <
class e,
class g>
597 template <
class u,
class g>
600 auto num_in_edges =
static_cast<int>(boost::in_degree(vertex,
graph));
602 if (num_in_edges > 1)
615 if (num_in_edges == 0)
619 if (num_in_edges == 0 && boost::out_degree(vertex,
graph) == 0)
626 template <
class e,
class g>
641 template <
class u,
class g>
654 std::map<Vertex, boost::default_color_type>& color_map,
656 const std::vector<std::string>& terminate_on_links)
664 template <
class u,
class g>
667 for (
auto vd : boost::make_iterator_range(adjacent_vertices(vertex,
graph)))
671 color_map_[vd] = boost::default_color_type::black_color;
675 template <
class u,
class g>
699 const auto&
graph =
static_cast<const Graph&
>(*this);
700 std::vector<std::string> child_link_names;
702 std::map<Vertex, size_t> index_map;
703 boost::associative_property_map<std::map<Vertex, size_t>> prop_index_map(index_map);
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);
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);
715 boost::breadth_first_search(
718 boost::visitor(
vis).root_vertex(start_vertex).vertex_index_map(prop_index_map).color_map(prop_color_map));
720 return child_link_names;
724 template <
class Archive>
725 void save(Archive& ar,
const unsigned int version)
const;
727 template <
class Archive>
728 void load(Archive& ar,
const unsigned int version);
730 template <
class Archive>
731 void serialize(Archive& ar,
const unsigned int version);
736 os <<
"Links:" << std::endl;
737 for (
const auto&
l :
path.links)
738 os <<
" " <<
l << std::endl;
740 os <<
"Joints:" << std::endl;
741 for (
const auto&
j :
path.joints)
742 os <<
" " <<
j << std::endl;
744 os <<
"Active Joints:" << std::endl;
745 for (
const auto&
j :
path.active_joints)
746 os <<
" " <<
j << std::endl;
752#include <boost/serialization/export.hpp>
753#include <boost/serialization/tracking.hpp>
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
std::shared_ptr< const Joint > ConstPtr
Definition: joint.h:286
std::shared_ptr< Joint > Ptr
Definition: joint.h:285
std::shared_ptr< Link > Ptr
Definition: link.h:190
std::shared_ptr< const Link > ConstPtr
Definition: link.h:191
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
#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
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
bool & ascyclic_
Definition: graph.h:590
cycle_detector(bool &ascyclic)
Definition: graph.h:581
void back_edge(e, g &)
Definition: graph.h:584
bool found_root_
Definition: graph.h:634
void back_edge(e, const g &)
Definition: graph.h:627
bool & tree_
Definition: graph.h:633
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
Collision::Ptr c
Definition: tesseract_scene_graph_link_unit.cpp:130
m name
Definition: tesseract_scene_graph_link_unit.cpp:77
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