Restore behavior of Box shapes in BulletModel.

Updating Bullet to 2.87 introduced a fix that caused sub-epsilon
differences in the vertices returned by `btBoxShape::getVertex()`.
SNOPT exhibits different behavior when run with constraints that use the
slightly different set of vertices introduced by the fix.

We were only using `btBoxShape` when adding boxes to the collision model
(the collision shape that was stored in the model was always a
`btConvexHullShape` due to an (unrelated?) issue with collision normals
in `btBoxShape`). It was basically a lazy way to convert box dimensions
to vertex coordinates. This PR explicitly converts the box dimensions to
vertex coordinates and locks down the behavior we expect when creating a
`btCollisionShape` from a `DrakeShapes::Box`.

multibody/collision/BUILD.bazel

@@ -121,6 +121,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "bullet_model_test",
+    deps = [
+        ":bullet_collision",
+    ],
+)
+
 drake_cc_googletest(
     name = "collision_filter_group_test",
     data = [":test_models"],

multibody/collision/bullet_model.cc

@@ -111,21 +111,24 @@ BulletCollisionWorldWrapper::BulletCollisionWorldWrapper()
 
 std::unique_ptr<btCollisionShape> BulletModel::newBulletBoxShape(
     const DrakeShapes::Box& geometry, bool use_margins) {
-  std::unique_ptr<btCollisionShape> bt_shape(new btConvexHullShape());
-  btBoxShape bt_box(btVector3(geometry.size(0) / 2, geometry.size(1) / 2,
-                              geometry.size(2) / 2));
   /* Strange things happen to the collision-normals when we use the
    * convex interface to the btBoxShape. Instead, we'll explicitly create
    * a btConvexHullShape.
    */
+  auto bt_shape = std::make_unique<btConvexHullShape>();
   if (use_margins)
     bt_shape->setMargin(kLargeMargin);
   else
     bt_shape->setMargin(kSmallMargin);
-  for (int i = 0; i < 8; ++i) {
-    btVector3 vtx;
-    bt_box.getVertex(i, vtx);
-    dynamic_cast<btConvexHullShape*>(bt_shape.get())->addPoint(vtx);
+
+  for (double z_sign : {1, -1}) {
+    for (double y_sign : {1, -1}) {
+      for (double x_sign : {1, -1}) {
+        bt_shape->addPoint({x_sign * geometry.size(0) / 2,
+                            y_sign * geometry.size(1) / 2,
+                            z_sign * geometry.size(2) / 2});
+      }
+    }
   }
 
   return bt_shape;

multibody/collision/bullet_model.h

@@ -136,6 +136,9 @@ class BulletModel : public Model {
       const std::vector<Eigen::Vector3d>& input_points,
       double collision_threshold) override;
 
+  static std::unique_ptr<btCollisionShape> newBulletBoxShape(
+      const DrakeShapes::Box& geometry, bool use_margins);
+
  private:
   enum DispatchMethod {
     kNotYetDecided,
@@ -157,8 +160,6 @@ class BulletModel : public Model {
       ElementId idA, ElementId idB, bool use_margins);
 
   BulletCollisionWorldWrapper& getBulletWorld(bool use_margins);
-  static std::unique_ptr<btCollisionShape> newBulletBoxShape(
-      const DrakeShapes::Box& geometry, bool use_margins);
   static std::unique_ptr<btCollisionShape> newBulletSphereShape(
       const DrakeShapes::Sphere& geometry, bool use_margins);
   static std::unique_ptr<btCollisionShape> newBulletCylinderShape(

multibody/collision/test/bullet_model_test.cc

@@ -0,0 +1,62 @@
+#include "drake/multibody/collision/bullet_model.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/common/eigen_types.h"
+#include "drake/multibody/shapes/geometry.h"
+
+namespace drake {
+namespace multibody {
+namespace collision {
+namespace {
+GTEST_TEST(BulletModelTest, newBulletBoxShapeTest) {
+  // Verifies that BulletModel::newBulletBoxShape() returns a pointer to a
+  // btConvexHullShape instance that satisfies the following criteria:
+  //    - contains 8 points
+  //    - the coordinates of those points are bit-wise equal to the permutations
+  //      of the positive and negative half-extents of the box in a specified
+  //      order.
+  const Vector3<double> half_extents{0.01, 0.04, 0.09};
+  // clang-format off
+  std::vector<Vector3<double>> expected_vertices{
+      { half_extents(0),  half_extents(1),  half_extents(2)},
+      {-half_extents(0),  half_extents(1),  half_extents(2)},
+      { half_extents(0), -half_extents(1),  half_extents(2)},
+      {-half_extents(0), -half_extents(1),  half_extents(2)},
+      { half_extents(0),  half_extents(1), -half_extents(2)},
+      {-half_extents(0),  half_extents(1), -half_extents(2)},
+      { half_extents(0), -half_extents(1), -half_extents(2)},
+      {-half_extents(0), -half_extents(1), -half_extents(2)},
+  };
+  // clang-format on
+  const int expected_num_vertices = static_cast<int>(expected_vertices.size());
+  std::unique_ptr<btCollisionShape> bt_shape =
+      BulletModel::newBulletBoxShape(DrakeShapes::Box{2 * half_extents}, true);
+  const auto bt_convex_hull_shape =
+      dynamic_cast<btConvexHullShape*>(bt_shape.get());
+
+  // Lock down implementation as btConvexHullShape as we'll need that to check
+  // the vertices below.
+  ASSERT_NE(bt_convex_hull_shape, nullptr);
+
+  // Check the number of vertices.
+  ASSERT_EQ(bt_convex_hull_shape->getNumVertices(), expected_num_vertices);
+
+  // Check that the vertices have the expected values. We intentionally use a
+  // tolerance of zero here because the results of queries on BulletModel may be
+  // used in solving nonlinear optimization problems, where sub-epsilon
+  // differences can lead to different results. We use EXPECT_NEAR rather than
+  // EXPECT_EQ because EXPECT_NEAR's error message is more informative in this
+  // case.
+  for (int i = 0; i < expected_num_vertices; ++i) {
+    btVector3 bullet_vertex;
+    bt_convex_hull_shape->getVertex(i, bullet_vertex);
+    EXPECT_NEAR(bullet_vertex.x(), expected_vertices[i].x(), 0.0);
+    EXPECT_NEAR(bullet_vertex.y(), expected_vertices[i].y(), 0.0);
+    EXPECT_NEAR(bullet_vertex.z(), expected_vertices[i].z(), 0.0);
+  }
+}
+}  // namespace
+}  // namespace collision
+}  // namespace multibody
+}  // namespace drake