Allow to set joint rest position out of joint limits (#1418)

CHANGELOG.md

@@ -12,6 +12,7 @@
   * Fixed soft body simulation when command input is not resetted: [#1372](https://github.com/dartsim/dart/pull/1372)
   * Added joint velocity limit constraint support: [#1407](https://github.com/dartsim/dart/pull/1407)
   * Added type property to constrain classes: [#1415](https://github.com/dartsim/dart/pull/1415)
+  * Allowed to set joint rest position out of joint limits: [#1418](https://github.com/dartsim/dart/pull/1418)
 
 * GUI
 

dart/dynamics/detail/GenericJoint.hpp

@@ -1467,18 +1467,6 @@ void GenericJoint<ConfigSpaceT>::setRestPosition(size_t index, double q0)
     return;
   }
 
-  if (Base::mAspectProperties.mPositionLowerLimits[index] > q0
-      || Base::mAspectProperties.mPositionUpperLimits[index] < q0)
-  {
-    dtwarn << "[GenericJoint::setRestPosition] Value of _q0 [" << q0
-           << "], is out of the limit range ["
-           << Base::mAspectProperties.mPositionLowerLimits[index] << ", "
-           << Base::mAspectProperties.mPositionUpperLimits[index]
-           << "] for index [" << index << "] of Joint [" << this->getName()
-           << "].\n";
-    return;
-  }
-
   GenericJoint_SET_IF_DIFFERENT(mRestPositions[index], q0);
 }
 

unittests/comprehensive/test_Joints.cpp

@@ -829,6 +829,55 @@ SkeletonPtr createPendulum(Joint::ActuatorType actType)
   return pendulum;
 }
 
+//==============================================================================
+TEST_F(JOINTS, SpringRestPosition)
+{
+  using namespace math::suffixes;
+
+  auto skel = createPendulum(Joint::ActuatorType::PASSIVE);
+  ASSERT_NE(skel, nullptr);
+
+  auto joint = skel->getRootJoint();
+  ASSERT_NE(joint, nullptr);
+
+  auto world = simulation::World::create();
+  ASSERT_NE(world, nullptr);
+
+  world->addSkeleton(skel);
+  world->setGravity(Eigen::Vector3d::Zero());
+
+  joint->setPosition(0, 0);
+  joint->setRestPosition(0, -1.0_pi);
+  joint->setPositionLowerLimit(0, -0.5_pi);
+  joint->setPositionUpperLimit(0, +0.5_pi);
+  joint->setSpringStiffness(0, 5);
+
+  EXPECT_DOUBLE_EQ(joint->getPosition(0), 0);
+
+  const auto tol = 1e-3;
+
+  // Joint starts from 0 and rotates towards its spring rest position (i.e.,
+  // -pi), but it also should stay within the joint limits
+  // (i.e., [-0.5pi, 0.5pi]).
+  for (auto i = 0u; i < 1000; ++i)
+  {
+    world->step();
+    const auto pos = joint->getPosition(0);
+    EXPECT_GE(pos, joint->getPositionLowerLimit(0) - tol);
+    EXPECT_LE(pos, joint->getPositionUpperLimit(0) + tol);
+  }
+
+  // After a while, the joint should come to rest at its lower position limit.
+  for (auto i = 0u; i < 500; ++i)
+  {
+    world->step();
+    const auto pos = joint->getPosition(0);
+    EXPECT_GE(pos, joint->getPositionLowerLimit(0) - tol);
+    EXPECT_LE(pos, joint->getPositionUpperLimit(0) + tol);
+    EXPECT_NEAR(pos, joint->getPositionLowerLimit(0), tol);
+  }
+}
+
 //==============================================================================
 void testServoMotor()
 {