-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
collision_checker_context.cc
52 lines (41 loc) · 1.52 KB
/
collision_checker_context.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
#include "drake/planning/collision_checker_context.h"
#include <memory>
#include <utility>
namespace drake {
namespace planning {
using geometry::QueryObject;
using std::unique_ptr;
using systems::Context;
namespace {
template <typename T>
T* NonNull(T* pointer) {
DRAKE_DEMAND(pointer != nullptr);
return pointer;
}
} // namespace
CollisionCheckerContext::CollisionCheckerContext(
const RobotDiagram<double>* model)
: CollisionCheckerContext(NonNull(model), model->CreateDefaultContext()) {}
CollisionCheckerContext::~CollisionCheckerContext() = default;
const QueryObject<double>& CollisionCheckerContext::GetQueryObject() const {
return model_.plant()
.get_geometry_query_input_port()
.template Eval<QueryObject<double>>(plant_context());
}
CollisionCheckerContext::CollisionCheckerContext(
const CollisionCheckerContext& other)
: CollisionCheckerContext(&other.model_, other.model_context_->Clone()) {}
unique_ptr<CollisionCheckerContext> CollisionCheckerContext::DoClone() const {
return unique_ptr<CollisionCheckerContext>(
new CollisionCheckerContext(*this));
}
CollisionCheckerContext::CollisionCheckerContext(
const RobotDiagram<double>* model,
unique_ptr<Context<double>> model_context)
: model_(*NonNull(model)),
model_context_(std::move(model_context)),
plant_context_(&model_.mutable_plant_context(model_context_.get())),
scene_graph_context_(
&model_.mutable_scene_graph_context(model_context_.get())) {}
} // namespace planning
} // namespace drake