Add regression coverage for issue 870 (#2283)

Author: jslee02

tests/integration/CMakeLists.txt

@@ -224,6 +224,9 @@ dart_build_tests(
     simulation/test_Building.cpp
 )
 
+dart_add_test(
+  "integration" INTEGRATION_simulation_Issue870 simulation/test_Issue870.cpp)
+
 if(TARGET dart-utils)
   dart_add_test("integration" INTEGRATION_simulation_FileInfoWorld simulation/test_FileInfoWorld.cpp)
   target_link_libraries(INTEGRATION_simulation_FileInfoWorld dart-utils)

tests/integration/simulation/test_Issue870.cpp

@@ -0,0 +1,251 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dart/All.hpp>
+
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <sstream>
+
+using namespace dart;
+using namespace dart::dynamics;
+using namespace dart::simulation;
+
+namespace {
+
+constexpr double kBoxSize = 0.12;
+constexpr double kBoxMass = 8.0;
+constexpr double kWeldedOffset = 1.6;
+constexpr double kFreeOffset = 0.6;
+
+struct BoxBounceWorld
+{
+  WorldPtr world;
+  BodyNode* leftFree{};
+  BodyNode* rightFree{};
+};
+
+// Creates the exact four-box setup described in the issue:
+// two free boxes between two welded boxes, zero gravity, restitution 1.0.
+BoxBounceWorld makeFourBoxBounceWorld(double pitch)
+{
+  auto world = World::create("issue870_boxes");
+  world->setGravity(Eigen::Vector3d::Zero());
+  // Use a small time step to keep the symmetric configuration stable.
+  world->setTimeStep(0.00025);
+
+  auto makeBoxSkeleton = [&](const std::string& name,
+                             bool weld,
+                             double xTranslation) -> BodyNode* {
+    auto skel = Skeleton::create(name);
+    std::pair<Joint*, BodyNode*> pair;
+
+    if (weld) {
+      pair = skel->createJointAndBodyNodePair<WeldJoint>();
+    } else {
+      pair = skel->createJointAndBodyNodePair<FreeJoint>();
+    }
+
+    auto* joint = pair.first;
+    auto* body = pair.second;
+
+    auto shape
+        = std::make_shared<BoxShape>(Eigen::Vector3d::Constant(kBoxSize));
+    auto* shapeNode = body->createShapeNodeWith<
+        VisualAspect,
+        CollisionAspect,
+        DynamicsAspect>(shape);
+    // Use elastic contact as in the original regression reproducer.
+    shapeNode->getDynamicsAspect()->setRestitutionCoeff(1.0);
+    shapeNode->getDynamicsAspect()->setFrictionCoeff(0.0);
+
+    Inertia inertia;
+    inertia.setMass(kBoxMass);
+    inertia.setMoment(shape->computeInertia(kBoxMass));
+    body->setInertia(inertia);
+
+    Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
+    tf.translation() = Eigen::Vector3d(xTranslation, 0.1, 0.0);
+    tf.linear()
+        = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()).toRotationMatrix();
+
+    if (auto* weldJoint = dynamic_cast<WeldJoint*>(joint)) {
+      weldJoint->setTransformFromParentBodyNode(tf);
+    } else {
+      joint->setPositions(FreeJoint::convertToPositions(tf));
+    }
+
+    if (!weld) {
+      auto* freeJoint = dynamic_cast<FreeJoint*>(joint);
+      DART_ASSERT(freeJoint != nullptr);
+      Eigen::Vector6d spatial = Eigen::Vector6d::Zero();
+      spatial.tail<3>()
+          = Eigen::Vector3d(xTranslation < 0 ? 1.0 : -1.0, 0.0, 0.0);
+      freeJoint->setSpatialVelocity(spatial, Frame::World(), Frame::World());
+    }
+
+    world->addSkeleton(skel);
+    return body;
+  };
+
+  BoxBounceWorld result;
+  result.world = world;
+  makeBoxSkeleton("box3", /*weld=*/true, -kWeldedOffset);
+  makeBoxSkeleton("box4", /*weld=*/true, kWeldedOffset);
+  result.leftFree = makeBoxSkeleton("box1", /*weld=*/false, -kFreeOffset);
+  result.rightFree = makeBoxSkeleton("box2", /*weld=*/false, kFreeOffset);
+  return result;
+}
+
+WorldPtr makeFreeFallWorld(bool spinning)
+{
+  auto world = World::create(spinning ? "spinning" : "baseline");
+  world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
+  world->setTimeStep(0.001);
+
+  SkeletonPtr sphere
+      = Skeleton::create(spinning ? "spinning_sphere" : "baseline_sphere");
+  auto pair = sphere->createJointAndBodyNodePair<FreeJoint>();
+  auto* joint = pair.first;
+  auto* body = pair.second;
+
+  auto shape = std::make_shared<EllipsoidShape>(Eigen::Vector3d::Constant(0.2));
+  auto* shapeNode = body->createShapeNodeWith<
+      VisualAspect,
+      CollisionAspect,
+      DynamicsAspect>(shape);
+  shapeNode->getDynamicsAspect()->setFrictionCoeff(0.0);
+
+  const double mass = 2.0;
+  Inertia inertia;
+  inertia.setMass(mass);
+  inertia.setMoment(shape->computeInertia(mass));
+  body->setInertia(inertia);
+
+  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
+  tf.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
+  joint->setPositions(FreeJoint::convertToPositions(tf));
+
+  if (spinning) {
+    Eigen::Vector6d velocities = Eigen::Vector6d::Zero();
+    velocities.head<3>() = Eigen::Vector3d(0.0, 0.0, 10.0);
+    joint->setVelocities(velocities);
+  }
+
+  world->addSkeleton(sphere);
+  return world;
+}
+
+} // namespace
+
+// Regression for https://github.com/dartsim/dart/issues/870 (box bounce
+// symmetry): rotating every box by pi/2 should not affect the symmetric
+// translation of the free boxes or allow them to bypass the welded boxes.
+TEST(Issue870, RotatedBoxesRemainSymmetricBetweenWeldedStops)
+{
+  BoxBounceWorld baseline = makeFourBoxBounceWorld(0.0);
+  BoxBounceWorld rotated = makeFourBoxBounceWorld(dart::math::pi / 2.0);
+
+  const double barrier = kWeldedOffset;
+  const double softLimit = barrier + 1e-2; // allow tiny penetration tolerance
+  // Run long enough to cover multiple bounces without letting numerical drift
+  // dominate the signal.
+  // Keep total simulated time roughly consistent with the original test.
+  const int steps = 7200;
+
+  double maxSymmetryError = 0.0;
+  double maxAbsPosition = 0.0;
+
+  for (int i = 0; i < steps; ++i) {
+    baseline.world->step();
+    rotated.world->step();
+
+    const auto updateStats = [&](const BoxBounceWorld& w) {
+      const auto lp = w.leftFree->getWorldTransform().translation();
+      const auto rp = w.rightFree->getWorldTransform().translation();
+      const auto lv
+          = w.leftFree->getSpatialVelocity(Frame::World(), Frame::World());
+      const auto rv
+          = w.rightFree->getSpatialVelocity(Frame::World(), Frame::World());
+
+      maxSymmetryError = std::max(
+          maxSymmetryError,
+          std::abs((lp + rp).x()) + std::abs((lv + rv).tail<3>().x()));
+
+      maxAbsPosition
+          = std::max({maxAbsPosition, std::abs(lp.x()), std::abs(rp.x())});
+    };
+
+    updateStats(baseline);
+    updateStats(rotated);
+  }
+
+  std::ostringstream oss;
+  oss << "Issue #870 metrics: maxSymmetryError=" << maxSymmetryError
+      << ", maxAbsPosition=" << maxAbsPosition;
+  SCOPED_TRACE(oss.str());
+
+  EXPECT_LT(maxSymmetryError, 1e-4);
+  EXPECT_LT(maxAbsPosition, softLimit);
+}
+
+// Regression for https://github.com/dartsim/dart/issues/870: A spinning body in
+// free fall should not pick up lateral translation.
+TEST(Issue870, SpinningSphereFreeFallDoesNotDriftSideways)
+{
+  auto baselineWorld = makeFreeFallWorld(false);
+  auto spinningWorld = makeFreeFallWorld(true);
+
+  const int steps = 4000;
+  double maxHorizontalSeparation = 0.0;
+
+  for (int i = 0; i < steps; ++i) {
+    baselineWorld->step();
+    spinningWorld->step();
+
+    const auto* baselineBody = baselineWorld->getSkeleton(0)->getRootBodyNode();
+    const auto* spinningBody = spinningWorld->getSkeleton(0)->getRootBodyNode();
+
+    const Eigen::Vector3d baselinePos
+        = baselineBody->getWorldTransform().translation();
+    const Eigen::Vector3d spinningPos
+        = spinningBody->getWorldTransform().translation();
+
+    const Eigen::Vector2d horizDiff(
+        spinningPos.x() - baselinePos.x(), spinningPos.z() - baselinePos.z());
+    maxHorizontalSeparation
+        = std::max(maxHorizontalSeparation, horizDiff.norm());
+  }
+
+  EXPECT_LT(maxHorizontalSeparation, 1e-7);
+}