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.
72 lines
2.0 KiB
72 lines
2.0 KiB
#include "drake/planning/unimplemented_collision_checker.h"
|
|
|
|
#include <stdexcept>
|
|
#include <utility>
|
|
|
|
#include <fmt/format.h>
|
|
|
|
namespace drake {
|
|
namespace planning {
|
|
namespace {
|
|
|
|
[[noreturn]] void ThrowNotImplemented(const char* func) {
|
|
throw std::runtime_error(fmt::format("{} is not implemented", func));
|
|
}
|
|
|
|
} // namespace
|
|
|
|
UnimplementedCollisionChecker::UnimplementedCollisionChecker(
|
|
CollisionCheckerParams params, bool supports_parallel_checking)
|
|
: CollisionChecker(std::move(params), supports_parallel_checking) {}
|
|
|
|
UnimplementedCollisionChecker::UnimplementedCollisionChecker(
|
|
const UnimplementedCollisionChecker&) = default;
|
|
|
|
UnimplementedCollisionChecker::~UnimplementedCollisionChecker() {}
|
|
|
|
std::unique_ptr<CollisionChecker> UnimplementedCollisionChecker::DoClone()
|
|
const {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
void UnimplementedCollisionChecker::DoUpdateContextPositions(
|
|
CollisionCheckerContext*) const {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
bool UnimplementedCollisionChecker::DoCheckContextConfigCollisionFree(
|
|
const CollisionCheckerContext&) const {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
std::optional<geometry::GeometryId>
|
|
UnimplementedCollisionChecker::DoAddCollisionShapeToBody(
|
|
const std::string&, const multibody::Body<double>&, const geometry::Shape&,
|
|
const math::RigidTransform<double>&) {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
void UnimplementedCollisionChecker::DoRemoveAddedGeometries(
|
|
const std::vector<CollisionChecker::AddedShape>&) {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
RobotClearance UnimplementedCollisionChecker::DoCalcContextRobotClearance(
|
|
const CollisionCheckerContext&, double) const {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
std::vector<RobotCollisionType>
|
|
UnimplementedCollisionChecker::DoClassifyContextBodyCollisions(
|
|
const CollisionCheckerContext&) const {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
int UnimplementedCollisionChecker::DoMaxContextNumDistances(
|
|
const CollisionCheckerContext&) const {
|
|
ThrowNotImplemented(__func__);
|
|
}
|
|
|
|
} // namespace planning
|
|
} // namespace drake
|