-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
robot_diagram_builder.cc
62 lines (51 loc) · 1.71 KB
/
robot_diagram_builder.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
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
#include "drake/planning/robot_diagram_builder.h"
#include <stdexcept>
#include "drake/common/drake_throw.h"
namespace drake {
namespace planning {
using geometry::SceneGraph;
using multibody::AddMultibodyPlantSceneGraph;
using systems::DiagramBuilder;
using systems::System;
template <typename T>
RobotDiagramBuilder<T>::RobotDiagramBuilder(double time_step)
: builder_(std::make_unique<DiagramBuilder<T>>()),
pair_(AddMultibodyPlantSceneGraph<T>(builder_.get(), time_step)),
plant_(pair_.plant),
scene_graph_(pair_.scene_graph),
parser_(&plant_) {}
template <typename T>
RobotDiagramBuilder<T>::~RobotDiagramBuilder() = default;
template <typename T>
std::unique_ptr<RobotDiagram<T>> RobotDiagramBuilder<T>::Build() {
ThrowIfAlreadyBuilt();
if (!plant().is_finalized()) {
plant().Finalize();
}
return std::unique_ptr<RobotDiagram<T>>(
new RobotDiagram<T>(std::move(builder_)));
}
template <typename T>
bool RobotDiagramBuilder<T>::IsDiagramBuilt() const {
if (builder_ == nullptr) {
return true;
}
if (builder_->already_built()) {
throw std::logic_error(
"RobotDiagramBuilder: Do not call mutable_builder().Build() to create"
" a Diagram; instead, call Build() to create a RobotDiagram.");
}
return false;
}
template <typename T>
void RobotDiagramBuilder<T>::ThrowIfAlreadyBuilt() const {
if (IsDiagramBuilt()) {
throw std::logic_error(
"RobotDiagramBuilder: Build() has already been called to create a"
" RobotDiagram; this RobotDiagramBuilder may no longer be used.");
}
}
} // namespace planning
} // namespace drake
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::planning::RobotDiagramBuilder)