/
unimplemented_collision_checker.cc
75 lines (57 loc) · 2.11 KB
/
unimplemented_collision_checker.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
63
64
65
66
67
68
69
70
71
72
73
74
75
#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::RemoveAddedGeometries(
const std::vector<CollisionChecker::AddedShape>&) {
ThrowNotImplemented(__func__);
}
void UnimplementedCollisionChecker::UpdateCollisionFilters() {
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