forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
53 lines
1.5 KiB
53 lines
1.5 KiB
#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
|