diff --git a/doc/modeling.rst b/doc/modeling.rst index 681534cd..b146407f 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -272,6 +272,7 @@ approximately .. math:: \ac + d \cdot (b v + k r) = (1 - d)\cdot \au + :label: eq:constraint Again, the parameters that are under the user's control are :math:`d, b, k`. The remaining quantities are functions of the system state and are computed automatically at each time step. @@ -343,12 +344,9 @@ Next we explain the setting of the stiffness :math:`k` and damping :math:`b` whi .. admonition:: Intuitive description of the **reference acceleration** - The *reference acceleration* :math:`\ar` determines the **motion that constraint is trying to achieve** in - order to rectify violation. For example, consider a contact between a motionless free body pulled down by gravity - onto a static plane geom. Since there is no motion, the penetration will be entirely determined by the impedance - while the reference has no effect. Now imagine that the body is dropped onto the plane. Upon impact the constraint - will generate a normal force which attempts to rectify the penetration using a particular motion; this motion is - the reference acceleration. + The *reference acceleration* :math:`\ar` determines the **motion that constraint is trying to achieve** in order to + rectify violation. Imagine a body dropped onto the plane. Upon impact the constraint will generate a normal force + which attempts to rectify the penetration using a particular motion; this motion is the reference acceleration. Another way of understanding the reference acceleration is to think of the unmodeled deformation variables described in the :ref:`Computation chapter`. Imagine two bodies pressed together, leading to deformation at @@ -393,7 +391,12 @@ and the damping ratio is ignored. Equivalently, in the direct format, the :math: can go unstable. This is enforced internally, unless the :ref:`refsafe` attribute of :ref:`flag ` is set to false. The :math:`\text{dampratio}` parameter would normally be set to 1, corresponding to critical damping. Smaller values result in under-damped or bouncy constraints, while larger values result in - over-damped constraints. + over-damped constraints. Combining the above formula with :eq:`eq:constraint`, we can derive the following result. + If the reference acceleration is given using the positive number format and the impedance is constant + :math:`d = d_0 = d_\text{width}`, then the penetration depth at rest is + + .. math:: + r = \au \cdot (1 - d) \cdot \text{timeconst}^2 \cdot \text{dampratio}^2 Next we describe the direct format where the two numbers are :math:`(-\text{stiffness}, -\text{damping})`. This allows direct control over restitution in particular. We still apply some scaling so that the same numbers can be @@ -406,6 +409,12 @@ and the damping ratio is ignored. Equivalently, in the direct format, the :math: k &= \text{stiffness} \cdot d(r) / d_\text{width}^2 \\ \end{aligned} + Similarly to the above derivation, if the reference acceleration is given using the negative number format and the + impedance is constant, then the penetration depth at rest is + + .. math:: + r = \au \cdot (1 - d) \cdot \text{stiffness} + .. tip:: In the positive-value default format, the :math:`\text{timeconst}` parameter controls constraint **softness**. It is specified in units of time and means "how quickly is the constraint trying to resolve the violation". Larger diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 6f62a739..6a7d3159 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -161,6 +161,62 @@ TEST_F(CoreConstraintTest, WeldRotJacobian) { mj_deleteModel(model); } +// test formulas for penetration at rest +TEST_F(CoreConstraintTest, RestPenetration) { + constexpr char xml[] = R"( + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, testing::NotNull()); + mjtNum gravity = -model->opt.gravity[2]; + mjtNum damping_ratio = 0.8; + mjData* data = mj_makeData(model); + + for (const mjtNum reference : {-100.0, -10.0, 0.1, 0.01}) { + for (const mjtNum impedance : {0.3, 0.9, 0.99}) { + // set solimp + for (int i=0; i < model->ngeom; i++) { + model->geom_solimp[i*mjNIMP + 0] = impedance; + model->geom_solimp[i*mjNIMP + 1] = impedance; + } + + // set solref + for (int i=0; i < model->ngeom; i++) { + model->geom_solref[i*mjNREF + 0] = reference; + model->geom_solref[i*mjNREF + 1] = reference < 0 ? -10 : damping_ratio; + } + + // simulate for 50 seconds + mj_resetData(model, data); + while (data->time < 50) { + mj_step(model, data); + } + + mjtNum depth = -data->contact[0].dist; + mjtNum expected_depth; + if (reference < 0) { + expected_depth = gravity * (1 - impedance) / -reference; + } else { + mjtNum tc_dr = reference * damping_ratio; + expected_depth = gravity * (1 - impedance) * tc_dr * tc_dr; + } + + EXPECT_THAT(depth, DoubleNear(expected_depth, 1e-10)); + } + } + + mj_deleteData(data); + mj_deleteModel(model); +} + static const char* const kDoflessContactPath = "engine/testdata/core_constraint/dofless_contact.xml"; static const char* const kDoflessTendonFrictionalPath =