Revisit the near-rigid regime for implicit PD constraints

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc

@@ -53,30 +53,52 @@ std::unique_ptr<AbstractValue> SapPdControllerConstraint<T>::DoMakeData(
   constexpr double beta = 0.1;
 
   // Estimate regularization based on near-rigid regime threshold.
-  // Rigid approximation constant: Rₙ = β²/(4π²)⋅wᵢ when the contact frequency
-  // ωₙ is below the limit ωₙ⋅δt ≤ 2π. That is, the period is Tₙ = β⋅δt. See
-  // [Castro et al., 2021] for details.
+  // Rigid approximation constant: Rₙᵣ = β²/(4π²)⋅wᵢ when the contact frequency
+  // ωₙᵣ is below the limit ωₙᵣ⋅δt ≤ 2π. That is, the near-rigid period is
+  // Tₙᵣ = β⋅δt. See [Castro et al., 2021] for details.
   const double beta_factor = beta * beta / (4.0 * M_PI * M_PI);
-
-  // Effective gain values are clamped in the near-rigid regime.
-  T Kp_eff = parameters_.Kp();
-  T Kd_eff = parameters_.Kd();
-
-  // "Effective regularization" [Castro et al., 2021] for this constraint.
-  const T R = 1.0 / (dt * (dt * Kp_eff + Kd_eff));
-
-  // "Near-rigid" regularization, [Castro et al., 2021].
-  const T& w = delassus_estimation[0];
-  const T R_near_rigid = beta_factor * w;
-
-  // In the near rigid regime we clamp Kp and Kd so that the effective
-  // regularization is Rnr.
-  if (R < R_near_rigid) {
-    // Per [Castro et al., 2021], the relaxation time tau
-    // for a critically damped constraint equals the time step, tau = dt.
-    // With Kd = tau * Kp, and R = Rₙᵣ, we obtain Rₙᵣ⁻¹ = 2δt² Kₚ.
-    Kp_eff = 1.0 / R_near_rigid / (2.0 * dt * dt);
-    Kd_eff = dt * Kp_eff;
+  const T R_nr = beta_factor * delassus_estimation[0];
+
+  // "Effective regularization" [Castro et al., 2021] for this constraint based
+  // on user specified gains.
+  const T& Kp = parameters_.Kp();
+  const T& Kd = parameters_.Kd();
+  const T R = 1.0 / (dt * (dt * Kp + Kd));
+
+  // R determines numerical conditioning for this constraint. The SAP problem
+  // can become ill-conditioned for small enough values of R. To keep
+  // conditioning under control, [Castro et al., 2021] propose to use Rₙᵣ as a
+  // lower bound such that the time scales introduced by this constraint in the
+  // overall dynamics are of order Tₙᵣ = β⋅δt (i.e. under resolved for β < 1).
+  //
+  // However, we also want to respect the ratio Kd/Kp set by the user. In
+  // particular, if Kp = 0, that means the user wants velocity control only.
+  // Similarly, if Kd = 0, the user wants position control only. From the
+  // expression above for R, we observe we can write the effective
+  // regularization R in two different but equivalent ways:
+  //  1) R = 1/(δt⋅(δt +   Kd/Kp )⋅Kp),   if Kp > 0
+  //  2) R = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Kd),   if Kd > 0
+  // Keeping the ratio Kd/Kp (or its inverse) constant, and equating R = Rₙᵣ,
+  // we can obtain expressions for the near-rigid regime gains from:
+  //  1) Rₙᵣ = 1/(δt⋅(δt +   Kd/Kp )⋅Kpₙᵣ),   if Kp > 0
+  //  2) Rₙᵣ = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Kdₙᵣ),   if Kd > 0
+  // From thse we can obtain the desired gains in terms of Rₙᵣ as:
+  //  1) δt⋅Kpₙᵣ = 1 / ( (δt +   Kd/Kp )⋅Rₙᵣ) > 1/(2⋅δt⋅Rₙᵣ), if δt⋅Kp > Kd
+  //  2)    Kdₙᵣ = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Rₙᵣ) > 1/(2⋅δt⋅Rₙᵣ), if δt⋅Kp < Kd
+  // We therefore use δt⋅Kp < Kd as the criterion to use expression (1),
+  // keeping Kd/Kp constant, or (2), keeping Kp/Kd constant.
+  T Kp_eff = Kp;
+  T Kd_eff = Kd;
+  if (R < R_nr) {
+    if (dt * Kp > Kd) {
+      const T tau = Kd / Kp;
+      Kp_eff = 1.0 / (dt * (dt + tau) * R_nr);
+      Kd_eff = tau * Kp_eff;  // We keep ratio tau constant.
+    } else {
+      const T tau_inv = Kp / Kd;
+      Kd_eff = 1.0 / (dt * (1.0 + dt * tau_inv) * R_nr);
+      Kp_eff = tau_inv * Kd_eff;  // We keep ratio tau_inv constant.
+    }
   }
 
   // Make data.

multibody/contact_solvers/sap/sap_pd_controller_constraint.h

@@ -104,9 +104,12 @@ class SapPdControllerConstraint final : public SapConstraint<T> {
 
     // TODO(amcastro-tri): Consider extending support for both gains being zero.
     /* Constructs a valid set of parameters.
-     @param Kp Proportional gain. It must be strictly positive.
+     @param Kp Proportional gain. It must be non-negative.
      @param Kd Derivative gain. It must be non-negative.
      @param effort_limit Effort limit. It must be strictly positive.
+
+     @pre At least one of Kp and Kd must be strictly positive. That is, they
+     cannot both be zero.
      @note Units will depend on the joint type on which this constraint is
      added. E.g. For a prismatic joint, Kp will be in N/m, Kd in N⋅s/m, and
      effort_limit in N. */

multibody/contact_solvers/sap/test/sap_pd_controller_constraint_test.cc

@@ -183,7 +183,8 @@ GTEST_TEST(SapPdControllerConstraint, MakeDataNearRigidRegime) {
 
   const double dt = 0.015;
   const double large_Kp = 1e8;
-  const double large_Kd = 1e7;
+  const double tau = 0.13;
+  const double large_Kd = tau * large_Kp;
   const double effort_limit = 0.3;
   const Parameters p{large_Kp, large_Kd, effort_limit};
   // The configuration is irrelevant for this test.
@@ -193,19 +194,85 @@ GTEST_TEST(SapPdControllerConstraint, MakeDataNearRigidRegime) {
   const VectorXd w = Vector1d::Constant(1.6);
 
   const double Rnr = kBeta * kBeta / (4.0 * M_PI * M_PI) * w(0);
-  // Expected near-rigid parameters.
-  const double Kp_nr = 1.0 / Rnr / (2.0 * dt * dt);
-  const double Kd_nr = dt * Kp_nr;
+  // Expected near-rigid parameters. Time scale tau is kept constant.
+  const double Kp_nr = 1.0 / Rnr / (dt * (dt + tau));
+  const double Kd_nr = tau * Kp_nr;
 
   // Verify that gains were clamped to be in the near-rigid regime.
   auto abstract_data = c.MakeData(dt, w);
   const auto& data =
       abstract_data->get_value<SapPdControllerConstraintData<double>>();
-  EXPECT_NEAR(data.Kp_eff(), Kp_nr, kEps);
+  EXPECT_NEAR(data.Kp_eff(), Kp_nr, kEps * large_Kp);
+  EXPECT_NEAR(data.Kd_eff(), Kd_nr, kEps * large_Kd);
+  EXPECT_EQ(data.time_step(), dt);  // not affected.
+}
+
+// Verify the effective gains within the near-rigid regime when the user sets
+// the position gain Kp to zero. In particular, the effective position gain
+// should remain zero to respect the user's desire to do velocity control only.
+GTEST_TEST(SapPdControllerConstraint, MakeDataNearRigidRegimeWithZeroKp) {
+  // This value of near-rigid parameter is the one internally used by
+  // SapPdControllerConstraint and they must remain in sync.
+  constexpr double kBeta = 0.1;
+
+  const double dt = 0.015;
+  const double Kp = 0.0;
+  const double large_Kd = 1.0e8;
+  const double effort_limit = 0.3;
+  const Parameters p{Kp, large_Kd, effort_limit};
+  // The configuration is irrelevant for this test.
+  const Configuration k = MakeArbitraryConfiguration();
+  const SapPdControllerConstraint<double> c(k, p);
+  // Arbitrary value for the Delassus operator.
+  const VectorXd w = Vector1d::Constant(1.6);
+
+  const double Rnr = kBeta * kBeta / (4.0 * M_PI * M_PI) * w(0);
+  // Expected near-rigid parameters. The user wants velocity control, and we
+  // should respect that.
+  const double Kd_nr = 1.0 / (dt * Rnr);
+
+  // Verify that gains were clamped to be in the near-rigid regime.
+  auto abstract_data = c.MakeData(dt, w);
+  const auto& data =
+      abstract_data->get_value<SapPdControllerConstraintData<double>>();
+  EXPECT_EQ(data.Kp_eff(), 0.0);
   EXPECT_NEAR(data.Kd_eff(), Kd_nr, kEps);
   EXPECT_EQ(data.time_step(), dt);  // not affected.
 }
 
+// Verify the effective gains within the near-rigid regime when the user sets
+// the derivative gain Kd to zero. In particular, the effective derivate gain
+// should remain zero to respect the user's desire to do position control only.
+GTEST_TEST(SapPdControllerConstraint, MakeDataNearRigidRegimeWithZeroKd) {
+  // This value of near-rigid parameter is the one internally used by
+  // SapPdControllerConstraint and they must remain in sync.
+  constexpr double kBeta = 0.1;
+
+  const double dt = 0.015;
+  const double large_Kp = 1e8;
+  const double Kd = 0.0;
+  const double effort_limit = 0.3;
+  const Parameters p{large_Kp, Kd, effort_limit};
+  // The configuration is irrelevant for this test.
+  const Configuration k = MakeArbitraryConfiguration();
+  const SapPdControllerConstraint<double> c(k, p);
+  // Arbitrary value for the Delassus operator.
+  const VectorXd w = Vector1d::Constant(1.6);
+
+  const double Rnr = kBeta * kBeta / (4.0 * M_PI * M_PI) * w(0);
+  // Expected near-rigid parameters. The user wants position control, and we
+  // should respect that.
+  const double Kp_nr = 1.0 / (dt * dt * Rnr);
+
+  // Verify that gains were clamped to be in the near-rigid regime.
+  auto abstract_data = c.MakeData(dt, w);
+  const auto& data =
+      abstract_data->get_value<SapPdControllerConstraintData<double>>();
+  EXPECT_NEAR(data.Kp_eff(), Kp_nr, kEps);
+  EXPECT_EQ(data.Kd_eff(), 0.0);
+  EXPECT_EQ(data.time_step(), dt);  // not affected.
+}
+
 // This method validates analytical gradients implemented by
 // SapPdControllerConstraint using automatic differentiation.
 void ValidateGradients(double v) {