Skip to content

Commit

Permalink
[geometry] Define alphabetical order to Shapes by convention (#18202)
Browse files Browse the repository at this point in the history 
* Alphabetize shapes

The ordering of the shapes has historically been chaotic (they were ordered
according to introduction order). This makes it hard to find the desired
shape and interferes with assessing if a shape implementation is missing
in a reifier.

The declarations of shapes are now alphabetized as well as the in the
ShapeReifier implementation. Tests and implementations have been updated to
reflect the same order.

* Incidental shape specification clean up

Missing instances of MeshcatCone for some reifiers and notation of other
possibly missing shapes.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI authored Nov 1, 2022
1 parent 08300d5 commit 4bc8764
Show file tree
Hide file tree
Showing 26 changed files with 650 additions and 625 deletions.
66 changes: 33 additions & 33 deletions geometry/drake_visualizer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -383,18 +383,30 @@ class ShapeToLcm : public ShapeReifier {

using ShapeReifier::ImplementGeometry;

void ImplementGeometry(const Sphere& sphere, void*) override {
geometry_data_.type = geometry_data_.SPHERE;
geometry_data_.num_float_data = 1;
geometry_data_.float_data.push_back(static_cast<float>(sphere.radius()));
void ImplementGeometry(const Box& box, void*) override {
geometry_data_.type = geometry_data_.BOX;
geometry_data_.num_float_data = 3;
// Box width, depth, and height.
geometry_data_.float_data.push_back(static_cast<float>(box.width()));
geometry_data_.float_data.push_back(static_cast<float>(box.depth()));
geometry_data_.float_data.push_back(static_cast<float>(box.height()));
}

void ImplementGeometry(const Ellipsoid& ellipsoid, void*) override {
geometry_data_.type = geometry_data_.ELLIPSOID;
void ImplementGeometry(const Capsule& capsule, void*) override {
geometry_data_.type = geometry_data_.CAPSULE;
geometry_data_.num_float_data = 2;
geometry_data_.float_data.push_back(static_cast<float>(capsule.radius()));
geometry_data_.float_data.push_back(static_cast<float>(capsule.length()));
}

// For visualization, Convex is the same as Mesh.
void ImplementGeometry(const Convex& mesh, void*) override {
geometry_data_.type = geometry_data_.MESH;
geometry_data_.num_float_data = 3;
geometry_data_.float_data.push_back(static_cast<float>(ellipsoid.a()));
geometry_data_.float_data.push_back(static_cast<float>(ellipsoid.b()));
geometry_data_.float_data.push_back(static_cast<float>(ellipsoid.c()));
geometry_data_.float_data.push_back(static_cast<float>(mesh.scale()));
geometry_data_.float_data.push_back(static_cast<float>(mesh.scale()));
geometry_data_.float_data.push_back(static_cast<float>(mesh.scale()));
geometry_data_.string_data = mesh.filename();
}

void ImplementGeometry(const Cylinder& cylinder, void*) override {
Expand All @@ -404,6 +416,14 @@ class ShapeToLcm : public ShapeReifier {
geometry_data_.float_data.push_back(static_cast<float>(cylinder.length()));
}

void ImplementGeometry(const Ellipsoid& ellipsoid, void*) override {
geometry_data_.type = geometry_data_.ELLIPSOID;
geometry_data_.num_float_data = 3;
geometry_data_.float_data.push_back(static_cast<float>(ellipsoid.a()));
geometry_data_.float_data.push_back(static_cast<float>(ellipsoid.b()));
geometry_data_.float_data.push_back(static_cast<float>(ellipsoid.c()));
}

void ImplementGeometry(const HalfSpace&, void*) override {
// Currently representing a half space as a big box. This assumes that the
// underlying box representation is centered on the origin.
Expand All @@ -423,22 +443,6 @@ class ShapeToLcm : public ShapeReifier {
X_PG_ = X_PG_ * box_xform;
}

void ImplementGeometry(const Box& box, void*) override {
geometry_data_.type = geometry_data_.BOX;
geometry_data_.num_float_data = 3;
// Box width, depth, and height.
geometry_data_.float_data.push_back(static_cast<float>(box.width()));
geometry_data_.float_data.push_back(static_cast<float>(box.depth()));
geometry_data_.float_data.push_back(static_cast<float>(box.height()));
}

void ImplementGeometry(const Capsule& capsule, void*) override {
geometry_data_.type = geometry_data_.CAPSULE;
geometry_data_.num_float_data = 2;
geometry_data_.float_data.push_back(static_cast<float>(capsule.radius()));
geometry_data_.float_data.push_back(static_cast<float>(capsule.length()));
}

void ImplementGeometry(const Mesh& mesh, void*) override {
geometry_data_.type = geometry_data_.MESH;
geometry_data_.num_float_data = 3;
Expand All @@ -448,14 +452,10 @@ class ShapeToLcm : public ShapeReifier {
geometry_data_.string_data = mesh.filename();
}

// For visualization, Convex is the same as Mesh.
void ImplementGeometry(const Convex& mesh, void*) override {
geometry_data_.type = geometry_data_.MESH;
geometry_data_.num_float_data = 3;
geometry_data_.float_data.push_back(static_cast<float>(mesh.scale()));
geometry_data_.float_data.push_back(static_cast<float>(mesh.scale()));
geometry_data_.float_data.push_back(static_cast<float>(mesh.scale()));
geometry_data_.string_data = mesh.filename();
void ImplementGeometry(const Sphere& sphere, void*) override {
geometry_data_.type = geometry_data_.SPHERE;
geometry_data_.num_float_data = 1;
geometry_data_.float_data.push_back(static_cast<float>(sphere.radius()));
}

private:
Expand Down
16 changes: 8 additions & 8 deletions geometry/make_mesh_for_deformable.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,14 @@ std::unique_ptr<VolumeMesh<double>> MeshBuilderForDeformable::Build(
return std::move(data.mesh);
}

void MeshBuilderForDeformable::ImplementGeometry(const Mesh& mesh_spec,
void* user_data) {
DRAKE_DEMAND(user_data != nullptr);
ReifyData& data = *static_cast<ReifyData*>(user_data);
data.mesh = std::make_unique<VolumeMesh<double>>(
MakeVolumeMeshFromVtk<double>(mesh_spec));
}

void MeshBuilderForDeformable::ImplementGeometry(const Sphere& sphere,
void* user_data) {
DRAKE_DEMAND(user_data != nullptr);
Expand All @@ -29,14 +37,6 @@ void MeshBuilderForDeformable::ImplementGeometry(const Sphere& sphere,
TessellationStrategy::kDenseInteriorVertices));
}

void MeshBuilderForDeformable::ImplementGeometry(const Mesh& mesh_spec,
void* user_data) {
DRAKE_DEMAND(user_data != nullptr);
ReifyData& data = *static_cast<ReifyData*>(user_data);
data.mesh = std::make_unique<VolumeMesh<double>>(
MakeVolumeMeshFromVtk<double>(mesh_spec));
}

void MeshBuilderForDeformable::ThrowUnsupportedGeometry(
const std::string& shape_name) {
throw std::logic_error(
Expand Down
2 changes: 1 addition & 1 deletion geometry/make_mesh_for_deformable.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@ class MeshBuilderForDeformable : public ShapeReifier {
};

using ShapeReifier::ImplementGeometry;
void ImplementGeometry(const Sphere& sphere, void* user_data) override;
void ImplementGeometry(const Mesh& mesh_spec, void* user_data) override;
void ImplementGeometry(const Sphere& sphere, void* user_data) override;
// TODO(xuchenhan-tri): As other shapes get supported, include their specific
// overrides here.

Expand Down
144 changes: 72 additions & 72 deletions geometry/meshcat.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -267,76 +267,6 @@ class MeshcatShapeReifier : public ShapeReifier {

using ShapeReifier::ImplementGeometry;

void ImplementGeometry(const Sphere& sphere, void* data) override {
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
lumped.object = internal::MeshData();

auto geometry = std::make_unique<internal::SphereGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->radius = sphere.radius();
lumped.geometry = std::move(geometry);
}

void ImplementGeometry(const Cylinder& cylinder, void* data) override {
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
auto& mesh = lumped.object.emplace<internal::MeshData>();

auto geometry = std::make_unique<internal::CylinderGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->radiusBottom = cylinder.radius();
geometry->radiusTop = cylinder.radius();
geometry->height = cylinder.length();
lumped.geometry = std::move(geometry);

// Meshcat cylinders have their long axis in y.
Eigen::Map<Eigen::Matrix4d>(mesh.matrix) =
RigidTransformd(RotationMatrixd::MakeXRotation(M_PI / 2.0))
.GetAsMatrix4();
}

void ImplementGeometry(const HalfSpace&, void*) override {
// TODO(russt): Use PlaneGeometry with fields width, height,
// widthSegments, heightSegments
drake::log()->warn("Meshcat does not display HalfSpace geometry (yet).");
}

void ImplementGeometry(const Box& box, void* data) override {
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
lumped.object = internal::MeshData();

auto geometry = std::make_unique<internal::BoxGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->width = box.width();
// Three.js uses height for the y axis; Drake uses depth.
geometry->height = box.depth();
geometry->depth = box.height();
lumped.geometry = std::move(geometry);
}

void ImplementGeometry(const Capsule&, void*) override {
drake::log()->warn("Meshcat does not display Capsule geometry (yet).");
}

void ImplementGeometry(const Ellipsoid& ellipsoid, void* data) override {
// Implemented as a Sphere stretched by a diagonal transform.
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
auto& mesh = lumped.object.emplace<internal::MeshData>();

auto geometry = std::make_unique<internal::SphereGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->radius = 1;
lumped.geometry = std::move(geometry);

Eigen::Map<Eigen::Matrix4d> matrix(mesh.matrix);
matrix(0, 0) = ellipsoid.a();
matrix(1, 1) = ellipsoid.b();
matrix(2, 2) = ellipsoid.c();
}

template <typename T>
void ImplementMesh(const T& mesh, void* data) {
DRAKE_DEMAND(data != nullptr);
Expand Down Expand Up @@ -466,14 +396,73 @@ class MeshcatShapeReifier : public ShapeReifier {
}
}

void ImplementGeometry(const Mesh& mesh, void* data) override {
ImplementMesh(mesh, data);
void ImplementGeometry(const Box& box, void* data) override {
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
lumped.object = internal::MeshData();

auto geometry = std::make_unique<internal::BoxGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->width = box.width();
// Three.js uses height for the y axis; Drake uses depth.
geometry->height = box.depth();
geometry->depth = box.height();
lumped.geometry = std::move(geometry);
}

void ImplementGeometry(const Capsule&, void*) override {
drake::log()->warn("Meshcat does not display Capsule geometry (yet).");
}

void ImplementGeometry(const Convex& mesh, void* data) override {
ImplementMesh(mesh, data);
}

void ImplementGeometry(const Cylinder& cylinder, void* data) override {
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
auto& mesh = lumped.object.emplace<internal::MeshData>();

auto geometry = std::make_unique<internal::CylinderGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->radiusBottom = cylinder.radius();
geometry->radiusTop = cylinder.radius();
geometry->height = cylinder.length();
lumped.geometry = std::move(geometry);

// Meshcat cylinders have their long axis in y.
Eigen::Map<Eigen::Matrix4d>(mesh.matrix) =
RigidTransformd(RotationMatrixd::MakeXRotation(M_PI / 2.0))
.GetAsMatrix4();
}

void ImplementGeometry(const Ellipsoid& ellipsoid, void* data) override {
// Implemented as a Sphere stretched by a diagonal transform.
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
auto& mesh = lumped.object.emplace<internal::MeshData>();

auto geometry = std::make_unique<internal::SphereGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->radius = 1;
lumped.geometry = std::move(geometry);

Eigen::Map<Eigen::Matrix4d> matrix(mesh.matrix);
matrix(0, 0) = ellipsoid.a();
matrix(1, 1) = ellipsoid.b();
matrix(2, 2) = ellipsoid.c();
}

void ImplementGeometry(const HalfSpace&, void*) override {
// TODO(russt): Use PlaneGeometry with fields width, height,
// widthSegments, heightSegments
drake::log()->warn("Meshcat does not display HalfSpace geometry (yet).");
}

void ImplementGeometry(const Mesh& mesh, void* data) override {
ImplementMesh(mesh, data);
}

void ImplementGeometry(const MeshcatCone& cone, void* data) override {
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
Expand All @@ -496,6 +485,17 @@ class MeshcatShapeReifier : public ShapeReifier {
.GetAsMatrix4();
}

void ImplementGeometry(const Sphere& sphere, void* data) override {
DRAKE_DEMAND(data != nullptr);
auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
lumped.object = internal::MeshData();

auto geometry = std::make_unique<internal::SphereGeometryData>();
geometry->uuid = uuids::to_string((*uuid_generator_)());
geometry->radius = sphere.radius();
lumped.geometry = std::move(geometry);
}

private:
uuids::uuid_random_generator* const uuid_generator_{};
};
Expand Down
14 changes: 7 additions & 7 deletions geometry/optimization/hpolyhedron.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -528,13 +528,6 @@ HPolyhedron::DoToShapeWithPose() const {
"class (to support in-memory mesh data, or file I/O).");
}

void HPolyhedron::ImplementGeometry(const HalfSpace&, void* data) {
auto* Ab = static_cast<std::pair<MatrixXd, VectorXd>*>(data);
// z <= 0.0.
Ab->first = Eigen::RowVector3d{0.0, 0.0, 1.0};
Ab->second = Vector1d{0.0};
}

void HPolyhedron::ImplementGeometry(const Box& box, void* data) {
Eigen::Matrix<double, 6, 3> A;
A << Eigen::Matrix3d::Identity(), -Eigen::Matrix3d::Identity();
Expand All @@ -548,6 +541,13 @@ void HPolyhedron::ImplementGeometry(const Box& box, void* data) {
Ab->second = b;
}

void HPolyhedron::ImplementGeometry(const HalfSpace&, void* data) {
auto* Ab = static_cast<std::pair<MatrixXd, VectorXd>*>(data);
// z <= 0.0.
Ab->first = Eigen::RowVector3d{0.0, 0.0, 1.0};
Ab->second = Vector1d{0.0};
}

HPolyhedron HPolyhedron::PontryaginDifference(const HPolyhedron& other) const {
/**
* The Pontryagin set difference of Polytope P = {x | Ax <= b} and
Expand Down
2 changes: 1 addition & 1 deletion geometry/optimization/hpolyhedron.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -233,8 +233,8 @@ class HPolyhedron final : public ConvexSet {

// Implement support shapes for the ShapeReifier interface.
using ShapeReifier::ImplementGeometry;
void ImplementGeometry(const HalfSpace&, void* data) final;
void ImplementGeometry(const Box& box, void* data) final;
void ImplementGeometry(const HalfSpace&, void* data) final;
// TODO(russt): Support ImplementGeometry(const Convex& convex, ...), but
// currently it would require e.g. digging ReadObjForConvex out of
// proximity_engine.cc.
Expand Down
10 changes: 5 additions & 5 deletions geometry/optimization/hyperellipsoid.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -267,11 +267,6 @@ Hyperellipsoid::DoToShapeWithPose() const {
return std::make_pair(std::move(shape), X_WG);
}

void Hyperellipsoid::ImplementGeometry(const Sphere& sphere, void* data) {
auto* A = static_cast<Eigen::Matrix3d*>(data);
*A = Eigen::Matrix3d::Identity() / sphere.radius();
}

void Hyperellipsoid::ImplementGeometry(const Ellipsoid& ellipsoid, void* data) {
// x²/a² + y²/b² + z²/c² = 1 in quadratic form is
// xᵀ * diag(1/a^2, 1/b^2, 1/c^2) * x = 1 and A is the square root.
Expand All @@ -280,6 +275,11 @@ void Hyperellipsoid::ImplementGeometry(const Ellipsoid& ellipsoid, void* data) {
1.0 / ellipsoid.a(), 1.0 / ellipsoid.b(), 1.0 / ellipsoid.c());
}

void Hyperellipsoid::ImplementGeometry(const Sphere& sphere, void* data) {
auto* A = static_cast<Eigen::Matrix3d*>(data);
*A = Eigen::Matrix3d::Identity() / sphere.radius();
}

} // namespace optimization
} // namespace geometry
} // namespace drake
2 changes: 1 addition & 1 deletion geometry/optimization/hyperellipsoid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,8 +126,8 @@ class Hyperellipsoid final : public ConvexSet {

// Implement support shapes for the ShapeReifier interface.
using ShapeReifier::ImplementGeometry;
void ImplementGeometry(const Sphere& sphere, void* data) final;
void ImplementGeometry(const Ellipsoid& ellipsoid, void* data) final;
void ImplementGeometry(const Sphere& sphere, void* data) final;

Eigen::MatrixXd A_{};
Eigen::VectorXd center_{};
Expand Down
Loading

0 comments on commit 4bc8764

Please sign in to comment.