Implements SapLimitConstraint (RobotLocomotion#17051)

multibody/contact_solvers/sap/BUILD.bazel

@@ -23,6 +23,7 @@ drake_cc_package_library(
         ":sap_constraint_bundle",
         ":sap_contact_problem",
         ":sap_friction_cone_constraint",
+        ":sap_limit_constraint",
         ":sap_model",
         ":sap_solver",
         ":sap_solver_results",
@@ -84,6 +85,17 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "sap_limit_constraint",
+    srcs = ["sap_limit_constraint.cc"],
+    hdrs = ["sap_limit_constraint.h"],
+    deps = [
+        ":sap_constraint",
+        "//common:default_scalars",
+        "//common:essential",
+    ],
+)
+
 drake_cc_library(
     name = "sap_friction_cone_constraint",
     srcs = ["sap_friction_cone_constraint.cc"],
@@ -171,6 +183,15 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "sap_limit_constraint_test",
+    deps = [
+        ":sap_limit_constraint",
+        "//common:pointer_cast",
+        "//common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "sap_friction_cone_constraint_test",
     deps = [

multibody/contact_solvers/sap/sap_limit_constraint.cc

@@ -0,0 +1,148 @@
+#include "drake/multibody/contact_solvers/sap/sap_limit_constraint.h"
+
+#include <algorithm>
+#include <limits>
+#include <utility>
+
+#include "drake/common/default_scalars.h"
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace internal {
+
+template <typename T>
+SapLimitConstraint<T>::Parameters::Parameters(const T& lower_limit,
+                                              const T& upper_limit,
+                                              const T& stiffness,
+                                              const T& dissipation_time_scale,
+                                              double beta)
+    : lower_limit_(lower_limit),
+      upper_limit_(upper_limit),
+      stiffness_(stiffness),
+      dissipation_time_scale_(dissipation_time_scale),
+      beta_(beta) {
+  constexpr double kInf = std::numeric_limits<double>::infinity();
+  DRAKE_DEMAND(lower_limit < kInf);
+  DRAKE_DEMAND(upper_limit > -kInf);
+  DRAKE_DEMAND(lower_limit <= upper_limit);
+  DRAKE_DEMAND(stiffness > 0);
+  DRAKE_DEMAND(dissipation_time_scale > 0);
+}
+
+template <typename T>
+SapLimitConstraint<T>::SapLimitConstraint(int clique, int clique_dof,
+                                          int clique_nv, const T& q0,
+                                          Parameters parameters)
+    : SapConstraint<T>(clique,
+                       CalcConstraintFunction(q0, parameters.lower_limit(),
+                                              parameters.upper_limit()),
+                       CalcConstraintJacobian(clique_dof, clique_nv,
+                                              parameters.lower_limit(),
+                                              parameters.upper_limit())),
+      parameters_(std::move(parameters)) {}
+
+template <typename T>
+VectorX<T> SapLimitConstraint<T>::CalcBiasTerm(const T& time_step,
+                                               const T&) const {
+  return -this->constraint_function() /
+         (time_step + parameters_.dissipation_time_scale());
+}
+
+template <typename T>
+VectorX<T> SapLimitConstraint<T>::CalcDiagonalRegularization(
+    const T& time_step, const T& wi) const {
+  using std::max;
+
+  // 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.
+  const double beta_factor =
+      parameters_.beta() * parameters_.beta() / (4.0 * M_PI * M_PI);
+
+  const T& k = parameters_.stiffness();
+  const T& taud = parameters_.dissipation_time_scale();
+
+  const T R = max(beta_factor * wi, 1.0 / (time_step * k * (time_step + taud)));
+
+  return VectorX<T>::Constant(this->num_constraint_equations(), R);
+}
+
+template <typename T>
+void SapLimitConstraint<T>::Project(const Eigen::Ref<const VectorX<T>>& y,
+                                    const Eigen::Ref<const VectorX<T>>&,
+                                    EigenPtr<VectorX<T>> gamma,
+                                    MatrixX<T>* dPdy) const {
+  DRAKE_DEMAND(gamma != nullptr);
+  DRAKE_DEMAND(gamma->size() == this->num_constraint_equations());
+  constexpr double kInf = std::numeric_limits<double>::infinity();
+  const T& ql = parameters_.lower_limit();
+  const T& qu = parameters_.upper_limit();
+
+  *gamma = y.array().max(0.0);
+  if (dPdy != nullptr) {
+    const int nk = this->num_constraint_equations();
+    // Resizing is no-op if already the proper size.
+    (*dPdy) = MatrixX<T>::Zero(nk, nk);
+    int i = 0;
+    if (ql > -kInf) {
+      if (y(i) > 0.0) (*dPdy)(i, i) = 1;
+      i++;
+    }
+    if (qu < kInf) {
+      if (y(i) > 0.0) (*dPdy)(i, i) = 1;
+    }
+  }
+}
+
+template <typename T>
+std::unique_ptr<SapConstraint<T>> SapLimitConstraint<T>::Clone() const {
+  return std::make_unique<SapLimitConstraint<T>>(*this);
+}
+
+template <typename T>
+VectorX<T> SapLimitConstraint<T>::CalcConstraintFunction(const T& q0,
+                                                         const T& ql,
+                                                         const T& qu) {
+  constexpr double kInf = std::numeric_limits<double>::infinity();
+  DRAKE_DEMAND(ql < kInf);
+  DRAKE_DEMAND(qu > -kInf);
+
+  const int nk = ql > -kInf && qu < kInf ? 2 : 1;
+  VectorX<T> g0(nk);
+
+  int i = 0;
+  if (ql > -kInf) g0(i++) = q0 - ql;  // lower limit.
+  if (qu < kInf) g0(i) = qu - q0;     // upper limit.
+
+  return g0;
+}
+
+template <typename T>
+MatrixX<T> SapLimitConstraint<T>::CalcConstraintJacobian(int clique_dof,
+                                                         int clique_nv,
+                                                         const T& ql,
+                                                         const T& qu) {
+  constexpr double kInf = std::numeric_limits<double>::infinity();
+  DRAKE_DEMAND(ql < kInf);
+  DRAKE_DEMAND(qu > -kInf);
+  DRAKE_DEMAND(ql <= qu);
+
+  const int nk = ql > -kInf && qu < kInf ? 2 : 1;
+  MatrixX<T> J = MatrixX<T>::Zero(nk, clique_nv);
+
+  int i = 0;
+  if (ql > -kInf) J(i++, clique_dof) = 1;
+  if (qu < kInf) J(i, clique_dof) = -1;
+
+  return J;
+}
+
+}  // namespace internal
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::multibody::contact_solvers::internal::SapLimitConstraint)

multibody/contact_solvers/sap/sap_limit_constraint.h

@@ -0,0 +1,162 @@
+#pragma once
+
+#include <limits>
+#include <memory>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+#include "drake/multibody/contact_solvers/sap/sap_constraint.h"
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace internal {
+
+/* Implements limit constraints for the SAP solver, [Castro et al., 2021]. This
+ constraint is used to impose a (compliant, see below) limit on the i-th degree
+ of freedom (DOF) of a given clique in a SapContactModel. This constraint
+ assumes that the rate of change of the i-th configuration exactly equals its
+ corresponding generalized velocities, i.e. q̇ᵢ = vᵢ. This is specially true for
+ 1-DOF joints such as revolute and prismatic.
+
+ Constraint kinematics:
+  We consider the i-th DOF of a clique with m DOFs.
+  We denote the configuration with qᵢ its lower limit with qₗ and its upper
+  limit with qᵤ, where qₗ < qᵤ. The limit constraint defines a constraint
+  function g(q) ∈ ℝ² as:
+    g = |qᵢ - qₗ|   for the lower limit and,
+        |qᵤ - qᵢ|   for the upper limit
+  such that g(qᵢ) < 0 (componentwise) if the constraint is violated. The
+  constraint velocity therefore is:
+    ġ = | q̇ᵢ|
+        |-q̇ᵢ|
+  And therefore the constraint Jacobian is:
+    J = | eᵢᵀ|
+        |-eᵢᵀ|
+  where eᵢ is the i-th element of the standard basis of ℝᵐ whose components are
+  all zero, except for the i-th component that equals to 1.
+
+  If one of the limits is infinite (qₗ = -∞ or qᵤ = +∞) then only one of the
+  equations is considered (the one with finite bound) and g(q) ∈ ℝ i.e.
+  num_constraint_equations() = 1.
+
+ Compliant impulse:
+  Limit constraints in the SAP formulation model a compliant impulse γ according
+  to:
+    y/dt = −k⋅g−d⋅ġ
+    γ/δt = P(y)
+    P(y) = (y)₊
+  where δt is the time step used in the formulation, k is the constraint
+  stiffness (in N/m), d is the dissipation (in N⋅s/m) and (x)₊ = max(0, x),
+  componentwise. Dissipation is parameterized as d = tau_d⋅k, where tau_d is the
+  "dissipation time scale". Notice that these impulses are positive when the
+  constraint is active and zero otherwise.
+  For this constraint the components of γ = [γₗ, γᵤ]ᵀ are constrained to live in
+  ℝ⁺ and therefore the projection can trivially be computed analytically as
+  P(y) = (y)₊, independent of the compliant regularization, see [Todorov, 2014].
+
+ [Castro et al., 2021] Castro A., Permenter F. and Han X., 2021. An
+   Unconstrained Convex Formulation of Compliant Contact. Available at
+   https://arxiv.org/abs/2110.10107
+ [Todorov, 2014] Todorov, E., 2014, May. Convex and analytically-invertible
+   dynamics with contacts and constraints: Theory and implementation in mujoco.
+   In 2014 IEEE International Conference on Robotics and Automation (ICRA) (pp.
+   6054-6061). IEEE.
+
+ @tparam_nonsymbolic_scalar */
+template <typename T>
+class SapLimitConstraint final : public SapConstraint<T> {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SapLimitConstraint);
+
+  /* Numerical parameters that define the constraint. Refer to this class's
+   documentation for details. */
+  class Parameters {
+   public:
+    DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Parameters);
+
+    /* Constructs a valid set of parameters.
+     @pre lower_limit < +∞
+     @pre upper_limit > -∞
+     @pre at least one of lower_limit and upper_limit is finite.
+     @pre lower_limit <= upper_limit
+     @pre stiffness > 0
+     @pre dissipation_time_scale >= 0
+     @pre beta > 0 */
+    Parameters(const T& lower_limit, const T& upper_limit, const T& stiffness,
+               const T& dissipation_time_scale, double beta = 0.1);
+
+    const T& lower_limit() const { return lower_limit_; }
+    const T& upper_limit() const { return upper_limit_; }
+    const T& stiffness() const { return stiffness_; }
+    const T& dissipation_time_scale() const { return dissipation_time_scale_; }
+    double beta() const { return beta_; }
+
+   private:
+    T lower_limit_;
+    T upper_limit_;
+    /* Contact stiffness k, in N/m. It must be strictly positive. */
+    T stiffness_;
+    /* Dissipation time scale tau_d, in seconds. It must be non-negative. */
+    T dissipation_time_scale_;
+    /* Rigid approximation constant: Rₙ = β²/(4π²)⋅w when the constraint
+     frequency ωₙ is below the limit ωₙ⋅δt ≤ 2π. That is, the period is Tₙ =
+     β⋅δt. w corresponds to a diagonal approximation of the Delassuss operator
+     for each contact. See [Castro et al., 2021] for details. */
+    double beta_{0.1};
+  };
+
+  /* Constructs a limit constraint for DOF with index `clique_dof` within clique
+   with index `clique` in a given SapContactProblem. If one of the limits is
+   infinite (qₗ = -∞ or qᵤ = +∞) then only one of the equations is considered
+   (the one with finite bound) and g(q) ∈ ℝ i.e. num_constraint_equations() = 1.
+   @param[in] clique The clique involved in the contact. Must be non-negative.
+   @param[in] clique_dof DOF in `clique` to be constrained. It must be in [0,
+   clique_nv).
+   @param[in] clique_nv Number of generalized velocities for `clique`.
+   @param[in] q0 Current configuration of the constraint.
+   @param[in] parameters Parameters of the constraint. */
+  SapLimitConstraint(int clique, int clique_dof, int clique_nv, const T& q0,
+                     Parameters parameters);
+
+  const Parameters& parameters() const { return parameters_; }
+
+  /* Implements the projection operation. In this case P(y) = (y)₊, independent
+   of the regularization R. Refer to SapConstraint::Project() for details. */
+  void Project(const Eigen::Ref<const VectorX<T>>& y,
+               const Eigen::Ref<const VectorX<T>>& R,
+               EigenPtr<VectorX<T>> gamma,
+               MatrixX<T>* dPdy = nullptr) const final;
+
+  /* Computes bias term. Refer to SapConstraint::CalcBiasTerm() for details. */
+  VectorX<T> CalcBiasTerm(const T& time_step, const T& wi) const final;
+
+  /* Computes the diagonal of the regularization matrix (positive diagonal) R.
+   This computes R = [Ri, Ri] (or R = [Ri] if only one limit is imposed) with
+     Ri = max(β²/(4π²)⋅wᵢ, (δt⋅(δt+tau_d)⋅k)⁻¹),
+   Refer to [Castro et al., 2021] for details. */
+  VectorX<T> CalcDiagonalRegularization(const T& time_step,
+                                        const T& wi) const final;
+
+  std::unique_ptr<SapConstraint<T>> Clone() const final;
+
+ private:
+  /* Computes the constraint function g(q0) as a function of q0 for given lower
+   limit ql and upper limit qu.
+   @pre lower_limit < +∞
+   @pre upper_limit > -∞ */
+  static VectorX<T> CalcConstraintFunction(const T& q0, const T& ql,
+                                           const T& qu);
+
+  /* Computes the constraint Jacobian, independent of the configuration for this
+   constraint. */
+  static MatrixX<T> CalcConstraintJacobian(int clique_dof, int clique_nv,
+                                           const T& ql, const T& qu);
+
+  Parameters parameters_;
+};
+
+}  // namespace internal
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake