-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
scene_graph_inspector.cc
33 lines (28 loc) · 1.05 KB
/
scene_graph_inspector.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
#include "drake/geometry/scene_graph_inspector.h"
#include <memory>
namespace drake {
namespace geometry {
template <typename T>
std::unique_ptr<GeometryInstance>
SceneGraphInspector<T>::CloneGeometryInstance(GeometryId id) const {
const std::string name = GetName(id);
const math::RigidTransformd X_PG = GetPoseInFrame(id);
std::unique_ptr<Shape> shape = GetShape(id).Clone();
auto geometry_instance = std::make_unique<GeometryInstance>(
X_PG, std::move(shape), name);
if (const auto* props = GetProximityProperties(id)) {
geometry_instance->set_proximity_properties(*props);
}
if (const auto* props = GetIllustrationProperties(id)) {
geometry_instance->set_illustration_properties(*props);
}
if (const auto* props = GetPerceptionProperties(id)) {
geometry_instance->set_perception_properties(*props);
}
return geometry_instance;
}
// Explicitly instantiates on the most common scalar types.
template class SceneGraphInspector<double>;
template class SceneGraphInspector<AutoDiffXd>;
} // namespace geometry
} // namespace drake