From eccd6542461c99da594531c9280f2263a0c62fa5 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 26 Aug 2026 11:29:01 -0700 Subject: [PATCH] [multibody] Add SapJointFrictionConstraint for joint dry friction Adds a single equation SAP constraint that models load independent dry (Coulomb) friction on one degree of freedom, the analog of MuJoCo's frictionloss. The cost is the Huber regularization of the maximum dissipation cost dt*tau_c*|v|, so the impulse is clamp(-v/R, +-dt*tau_c) with R = sigma*w, the same scale independent regularization SAP applies to contact friction. Friction saturates at the bound while sliding and holds the DOF at rest, up to a small regularization creep, in stiction. The constraint is not yet used by SapDriver; that follows separately. --- multibody/contact_solvers/sap/BUILD.bazel | 25 ++ .../sap/sap_joint_friction_constraint.cc | 137 ++++++++++ .../sap/sap_joint_friction_constraint.h | 200 +++++++++++++++ .../sap_joint_friction_constraint_test.cc | 239 ++++++++++++++++++ 4 files changed, 601 insertions(+) create mode 100644 multibody/contact_solvers/sap/sap_joint_friction_constraint.cc create mode 100644 multibody/contact_solvers/sap/sap_joint_friction_constraint.h create mode 100644 multibody/contact_solvers/sap/test/sap_joint_friction_constraint_test.cc diff --git a/multibody/contact_solvers/sap/BUILD.bazel b/multibody/contact_solvers/sap/BUILD.bazel index 073b3d1f9778..901386f7898a 100644 --- a/multibody/contact_solvers/sap/BUILD.bazel +++ b/multibody/contact_solvers/sap/BUILD.bazel @@ -27,6 +27,7 @@ drake_cc_package_library( ":sap_friction_cone_constraint", ":sap_holonomic_constraint", ":sap_hunt_crossley_constraint", + ":sap_joint_friction_constraint", ":sap_limit_constraint", ":sap_model", ":sap_pd_controller_constraint", @@ -218,6 +219,19 @@ drake_cc_library( ], ) +drake_cc_library( + name = "sap_joint_friction_constraint", + srcs = ["sap_joint_friction_constraint.cc"], + hdrs = ["sap_joint_friction_constraint.h"], + deps = [ + ":sap_constraint", + ":sap_constraint_jacobian", + "//common:default_scalars", + "//common:essential", + "//common:extract_double", + ], +) + drake_cc_library( name = "sap_limit_constraint", srcs = ["sap_limit_constraint.cc"], @@ -447,6 +461,17 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "sap_joint_friction_constraint_test", + deps = [ + ":expect_equal", + ":sap_joint_friction_constraint", + ":validate_constraint_gradients", + "//common:pointer_cast", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "sap_limit_constraint_test", deps = [ diff --git a/multibody/contact_solvers/sap/sap_joint_friction_constraint.cc b/multibody/contact_solvers/sap/sap_joint_friction_constraint.cc new file mode 100644 index 000000000000..1aeef0d97438 --- /dev/null +++ b/multibody/contact_solvers/sap/sap_joint_friction_constraint.cc @@ -0,0 +1,137 @@ +#include "drake/multibody/contact_solvers/sap/sap_joint_friction_constraint.h" + +#include +#include +#include +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/eigen_types.h" +#include "drake/common/extract_double.h" + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { + +template +SapJointFrictionConstraint::SapJointFrictionConstraint(int clique, + int clique_dof, + int clique_nv, + Parameters parameters) + : SapConstraint(CalcConstraintJacobian(clique, clique_dof, clique_nv), + {}), + parameters_(std::move(parameters)), + clique_dof_(clique_dof) { + DRAKE_DEMAND(parameters_.friction > 0.0); + DRAKE_DEMAND(parameters_.sigma > 0.0); +} + +template +SapConstraintJacobian SapJointFrictionConstraint::CalcConstraintJacobian( + int clique, int clique_dof, int clique_nv) { + DRAKE_DEMAND(clique >= 0); + DRAKE_DEMAND(0 <= clique_dof && clique_dof < clique_nv); + MatrixX J = MatrixX::Zero(1, clique_nv); + J(0, clique_dof) = 1.0; + return SapConstraintJacobian(clique, std::move(J)); +} + +template +std::unique_ptr SapJointFrictionConstraint::DoMakeData( + const T& time_step, + const Eigen::Ref>& delassus_estimation) const { + // Regularization R = σ⋅w, with w the diagonal approximation of the Delassus + // operator for this constraint, which estimates the inverse effective inertia + // of the constrained DOF. See class documentation. + const T R = parameters_.sigma * delassus_estimation(0); + // The Delassus estimation w = 1/Aᵢᵢ is positive and finite for any DOF with + // a positive diagonal entry in the (positive definite) dynamics matrix A. + // Both are needed for R⁻¹ and the cost to be well defined. + constexpr double kInf = std::numeric_limits::infinity(); + DRAKE_DEMAND(R > 0.0 && R < kInf); + const T gamma_max = time_step * parameters_.friction; + SapJointFrictionConstraintData data(R, gamma_max); + return SapConstraint::MoveAndMakeAbstractValue(std::move(data)); +} + +template +void SapJointFrictionConstraint::DoCalcData( + const Eigen::Ref>& vc, + AbstractValue* abstract_data) const { + using std::max; + using std::min; + auto& data = + abstract_data->get_mutable_value>(); + const T& gamma_max = data.gamma_max(); + data.mutable_vc() = vc(0); + data.mutable_y() = -vc(0) * data.R_inv(); + data.mutable_gamma() = max(-gamma_max, min(gamma_max, data.y())); +} + +template +T SapJointFrictionConstraint::DoCalcCost( + const AbstractValue& abstract_data) const { + using std::abs; + const auto& data = + abstract_data.get_value>(); + const T& R = data.R(); + const T& gamma_max = data.gamma_max(); + const T& vc = data.vc(); + const T& y = data.y(); + // The stiction condition |vc| ≤ R⋅γₘₐₓ is equivalent to |y| ≤ γₘₐₓ. + if (abs(y) <= gamma_max) { + return 0.5 * R * y * y; // Equals vc²/(2R). + } + return gamma_max * abs(vc) - 0.5 * R * gamma_max * gamma_max; +} + +template +void SapJointFrictionConstraint::DoCalcImpulse( + const AbstractValue& abstract_data, EigenPtr> gamma) const { + const auto& data = + abstract_data.get_value>(); + (*gamma)(0) = data.gamma(); +} + +template +void SapJointFrictionConstraint::DoCalcCostHessian( + const AbstractValue& abstract_data, MatrixX* G) const { + using std::abs; + const auto& data = + abstract_data.get_value>(); + // The Hessian is R⁻¹ in stiction and zero in sliding. At the transition we + // pick the stiction value, consistently with the branch used in DoCalcCost(). + (*G)(0, 0) = abs(data.y()) <= data.gamma_max() ? data.R_inv() : T(0.0); +} + +template +void SapJointFrictionConstraint::DoAccumulateGeneralizedImpulses( + int, const Eigen::Ref>& gamma, + EigenPtr> tau) const { + // N.B. The NVI guarantees the clique index is correct, and since there is + // only one, we don't need to use it. + // For this constraint the generalized impulses are τ = Jᵀ⋅γ = eᵢ⋅γ. + (*tau)(clique_dof_) += gamma(0); +} + +template +std::unique_ptr> +SapJointFrictionConstraint::DoToDouble() const { + SapJointFrictionConstraint::Parameters p_to_double{ + ExtractDoubleOrThrow(parameters_.friction), parameters_.sigma}; + // N.B. Joint friction constraints always act on a single clique. + const int clique = this->first_clique(); + const int clique_nv = this->num_velocities(0); + return std::make_unique>( + clique, clique_dof(), clique_nv, std::move(p_to_double)); +} + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::contact_solvers::internal:: + SapJointFrictionConstraint); diff --git a/multibody/contact_solvers/sap/sap_joint_friction_constraint.h b/multibody/contact_solvers/sap/sap_joint_friction_constraint.h new file mode 100644 index 000000000000..1cc91e5041bb --- /dev/null +++ b/multibody/contact_solvers/sap/sap_joint_friction_constraint.h @@ -0,0 +1,200 @@ +#pragma once + +#include +#include + +#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 { + +/* Structure to store data needed for SapJointFrictionConstraint computations. + @tparam_nonsymbolic_scalar */ +template +class SapJointFrictionConstraintData { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SapJointFrictionConstraintData); + + /* Constructs data for a SapJointFrictionConstraint. + Refer to SapJointFrictionConstraint's documentation for further details. + @param R Regularization parameter. It must be strictly positive. + @param gamma_max Friction impulse bound γₘₐₓ = δt⋅τ_c. */ + SapJointFrictionConstraintData(const T& R, const T& gamma_max) + : parameters_{R, 1.0 / R, gamma_max} {} + + /* Regularization R. */ + const T& R() const { return parameters_.R; } + + /* Inverse of the regularization, R⁻¹. */ + const T& R_inv() const { return parameters_.R_inv; } + + /* Friction impulse bound γₘₐₓ. */ + const T& gamma_max() const { return parameters_.gamma_max; } + + /* Const access. */ + const T& vc() const { return vc_; } + const T& y() const { return y_; } + const T& gamma() const { return gamma_; } + + /* Mutable access. */ + T& mutable_vc() { return vc_; } + T& mutable_y() { return y_; } + T& mutable_gamma() { return gamma_; } + + private: + // Values stored in this struct remain const after construction. + struct ConstParameters { + T R; // Regularization R. + T R_inv; // Inverse of the regularization R⁻¹. + T gamma_max; // Friction impulse bound γₘₐₓ = δt⋅τ_c. + }; + ConstParameters parameters_; + + T vc_{}; // Constraint velocity, i.e. the velocity of the constrained DOF. + T y_{}; // Un-projected impulse y = −vc/R. + T gamma_{}; // Projected impulse γ = clamp(y, −γₘₐₓ, γₘₐₓ). +}; + +/* Implements a dry (Coulomb) friction constraint on the i-th degree of freedom + (DOF) of a given clique in a SapContactProblem, for the SAP solver [Castro et + al., 2021]. This constraint models load-independent friction within a joint, + such as gearbox friction, in the same spirit as MuJoCo's `frictionloss` + attribute. The friction generalized force τ opposes the joint velocity and its + magnitude is bounded by a constant τ_c ≥ 0 with units of generalized force + (N⋅m for a revolute DOF or N for a prismatic DOF). In stiction the friction + force takes whatever value in [−τ_c, τ_c] is needed to hold the DOF at rest, + and during sliding it saturates at τ = −τ_c⋅sign(v). + + Constraint kinematics: + We consider the i-th DOF of a clique with m DOFs. The constraint velocity is + simply the generalized velocity of that DOF: + vc = vᵢ = eᵢᵀ⋅v + where eᵢ is the i-th element of the standard basis of ℝᵐ. Therefore the + constraint Jacobian is J = eᵢᵀ and this constraint has a single equation. + + Regularized friction: + With δt the time step of the SapContactProblem, the friction impulse is + bounded by γₘₐₓ = δt⋅τ_c. The ideal maximum dissipation cost γₘₐₓ⋅|vc| is not + differentiable at vc = 0 and therefore cannot be used directly by SAP. As for + the friction in contact constraints (see SapFrictionConeConstraint), we + regularize it. We use the convex cost: + ℓ(vc) = vc²/(2R) if |vc| ≤ R⋅γₘₐₓ (stiction) + γₘₐₓ⋅|vc| − R⋅γₘₐₓ²/2 otherwise (sliding) + which has a continuous first derivative, so that the impulse + γ(vc) = −∂ℓ/∂vc = clamp(−vc/R, −γₘₐₓ, γₘₐₓ) + is continuous, and a piecewise constant second derivative, the Hessian + G(vc) = ∂²ℓ/∂vc² = R⁻¹ in stiction and zero in sliding. + Notice that: + 1. The friction impulse always opposes the constraint velocity, satisfying + the principle of maximum dissipation. + 2. It obeys |γ| ≤ γₘₐₓ, i.e. the friction force γ/δt is bounded by τ_c. + + The regularization R is parameterized in a scale independent manner as + R = σ⋅w + where w is the diagonal approximation of the Delassus operator for this + constraint (an estimate of the inverse effective inertia of the constrained + DOF) and σ is a dimensionless parameter, see Parameters::sigma. During + stiction the DOF creeps with a residual velocity bounded by + vₛ = R⋅γₘₐₓ = σ⋅w⋅δt⋅τ_c, + that is, σ times the change in velocity that the friction force alone would + produce on the DOF in a single time step. Under a sustained load τₐ with + |τₐ| < τ_c the DOF settles to a steady creep velocity v∞ = σ⋅w⋅δt⋅τₐ, which + is below vₛ. + + [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 + + @tparam_nonsymbolic_scalar */ +template +class SapJointFrictionConstraint final : public SapConstraint { + public: + /* We do not allow copy, move, or assignment generally to avoid slicing. + Protected copy construction is enabled for sub-classes to use in their + implementation of DoClone(). */ + //@{ + SapJointFrictionConstraint& operator=(const SapJointFrictionConstraint&) = + delete; + SapJointFrictionConstraint(SapJointFrictionConstraint&&) = delete; + SapJointFrictionConstraint& operator=(SapJointFrictionConstraint&&) = delete; + //@} + + /* Numerical parameters that define the constraint. Refer to this class's + documentation for details. */ + struct Parameters { + bool operator==(const Parameters&) const = default; + + /* Dry friction bound τ_c on the generalized force of the constrained DOF. + It has units of generalized force, i.e. N⋅m for a revolute DOF and N for a + prismatic DOF. It must be strictly positive. */ + T friction{0.0}; + /* Dimensionless parameterization of the regularization of friction, R = + σ⋅w. Refer to this class's documentation for details. It must be strictly + positive. */ + double sigma{1.0e-3}; + }; + + /* Constructs a dry friction constraint for the DOF with index `clique_dof` + within the clique with index `clique` in a given SapContactProblem. + @param[in] clique The clique involved in the constraint. 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] parameters Parameters of the constraint. + @pre parameters.friction > 0. + @pre parameters.sigma > 0. */ + SapJointFrictionConstraint(int clique, int clique_dof, int clique_nv, + Parameters parameters); + + const Parameters& parameters() const { return parameters_; } + + /* Returns the degree of freedom, DOF, that this constraint acts on. Provided + at construction. */ + int clique_dof() const { return clique_dof_; } + + private: + /* Private copy construction is enabled to use in the implementation of + DoClone(). */ + SapJointFrictionConstraint(const SapJointFrictionConstraint&) = default; + + /* Computes the constraint Jacobian J = eᵢᵀ, with i = clique_dof. */ + static SapConstraintJacobian CalcConstraintJacobian(int clique, + int clique_dof, + int clique_nv); + + /* Implementations of SapConstraint NVI functions. */ + std::unique_ptr DoMakeData( + const T& time_step, + const Eigen::Ref>& delassus_estimation) const override; + void DoCalcData(const Eigen::Ref>& vc, + AbstractValue* abstract_data) const override; + T DoCalcCost(const AbstractValue& abstract_data) const override; + void DoCalcImpulse(const AbstractValue& abstract_data, + EigenPtr> gamma) const override; + void DoCalcCostHessian(const AbstractValue& abstract_data, + MatrixX* G) const override; + std::unique_ptr> DoClone() const final { + return std::unique_ptr>( + new SapJointFrictionConstraint(*this)); + } + std::unique_ptr> DoToDouble() const final; + void DoAccumulateGeneralizedImpulses( + int c, const Eigen::Ref>& gamma, + EigenPtr> tau) const final; + // no-op for this constraint. + void DoAccumulateSpatialImpulses(int, const Eigen::Ref>&, + SpatialForce*) const final {}; + + Parameters parameters_; + int clique_dof_{-1}; // Initialized to an invalid value. +}; + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/sap/test/sap_joint_friction_constraint_test.cc b/multibody/contact_solvers/sap/test/sap_joint_friction_constraint_test.cc new file mode 100644 index 000000000000..68bfe7947482 --- /dev/null +++ b/multibody/contact_solvers/sap/test/sap_joint_friction_constraint_test.cc @@ -0,0 +1,239 @@ +#include "drake/multibody/contact_solvers/sap/sap_joint_friction_constraint.h" + +#include +#include +#include + +#include + +#include "drake/common/autodiff.h" +#include "drake/common/pointer_cast.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/math/autodiff_gradient.h" +#include "drake/multibody/contact_solvers/sap/expect_equal.h" +#include "drake/multibody/contact_solvers/sap/validate_constraint_gradients.h" + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { +namespace { + +constexpr double kEps = std::numeric_limits::epsilon(); + +void ExpectEqual(const SapJointFrictionConstraint& c1, + const SapJointFrictionConstraint& c2) { + ExpectBaseIsEqual(c1, c2); + EXPECT_EQ(c1.clique_dof(), c2.clique_dof()); + EXPECT_EQ(c1.parameters(), c2.parameters()); +} + +class SapJointFrictionConstraintTest : public ::testing::Test { + public: + void SetUp() override { + dut_ = std::make_unique>( + clique_, clique_dof_, clique_nv_, parameters_); + SapJointFrictionConstraint::Parameters p_ad{ + parameters_.friction, parameters_.sigma}; + dut_ad_ = std::make_unique>( + clique_, clique_dof_, clique_nv_, p_ad); + } + + // Expected values from the class documentation. + double R() const { return parameters_.sigma * delassus_estimation_; } + double gamma_max() const { return time_step_ * parameters_.friction; } + // Stiction velocity: constraint velocities of magnitude below this value are + // in stiction, above it are in sliding. + double vs() const { return R() * gamma_max(); } + + // Makes data with the fixture's time step and Delassus estimation and + // computes cost, impulse and Hessian at vc. + struct Evaluation { + double cost; + double gamma; + double G; + }; + Evaluation Evaluate(double vc) const { + std::unique_ptr data = + dut_->MakeData(time_step_, Vector1d(delassus_estimation_)); + dut_->CalcData(Vector1d(vc), data.get()); + Evaluation e; + e.cost = dut_->CalcCost(*data); + VectorXd gamma(1); + dut_->CalcImpulse(*data, &gamma); + e.gamma = gamma(0); + MatrixXd G(1, 1); + dut_->CalcCostHessian(*data, &G); + e.G = G(0, 0); + return e; + } + + protected: + // Arbitrary set of indexes and parameters. + const int clique_{12}; + const int clique_dof_{3}; + const int clique_nv_{7}; + const double time_step_{2.0e-3}; + const double delassus_estimation_{1.5}; + const SapJointFrictionConstraint::Parameters parameters_{ + .friction = 2.5, .sigma = 1.0e-2}; + std::unique_ptr> dut_; + std::unique_ptr> dut_ad_; +}; + +// Bare minimum sanity checks on a newly constructed constraint. +TEST_F(SapJointFrictionConstraintTest, Construction) { + EXPECT_EQ(dut_->num_constraint_equations(), 1); + EXPECT_EQ(dut_->num_cliques(), 1); + EXPECT_EQ(dut_->first_clique(), clique_); + EXPECT_THROW(dut_->second_clique(), std::exception); + EXPECT_EQ(dut_->num_objects(), 0); + EXPECT_EQ(dut_->clique_dof(), clique_dof_); + EXPECT_EQ(dut_->parameters(), parameters_); + const MatrixXd J_expected = + VectorXd::Unit(clique_nv_, clique_dof_).transpose(); + EXPECT_EQ(dut_->first_clique_jacobian().MakeDenseMatrix(), J_expected); + EXPECT_THROW(dut_->second_clique_jacobian(), std::exception); +} + +// Verifies the regularization and impulse bound computed by MakeData(). +TEST_F(SapJointFrictionConstraintTest, MakeData) { + std::unique_ptr abstract_data = + dut_->MakeData(time_step_, Vector1d(delassus_estimation_)); + const auto& data = + abstract_data->get_value>(); + EXPECT_NEAR(data.R(), R(), kEps); + EXPECT_NEAR(data.R_inv(), 1.0 / R(), kEps); + EXPECT_NEAR(data.gamma_max(), gamma_max(), kEps); +} + +// Verifies cost, impulse and Hessian against the analytical expressions in the +// class documentation, in both the stiction and the sliding regimes. +TEST_F(SapJointFrictionConstraintTest, StictionRegime) { + for (const double vc : {-0.7 * vs(), 0.0, 0.3 * vs()}) { + const Evaluation e = Evaluate(vc); + EXPECT_NEAR(e.cost, vc * vc / (2.0 * R()), kEps); + // The impulse is computed as -vc⋅R⁻¹, which incurs a few roundoff errors + // relative to -vc/R. + EXPECT_NEAR(e.gamma, -vc / R(), 4 * kEps * gamma_max()); + EXPECT_NEAR(e.G, 1.0 / R(), kEps / R()); + // In stiction the impulse is strictly within its bounds. + EXPECT_LT(std::abs(e.gamma), gamma_max()); + } +} + +TEST_F(SapJointFrictionConstraintTest, SlidingRegime) { + for (const double vc : {-3.5 * vs(), 1.2 * vs()}) { + const Evaluation e = Evaluate(vc); + const double sign = vc > 0 ? 1.0 : -1.0; + EXPECT_NEAR( + e.cost, + gamma_max() * std::abs(vc) - 0.5 * R() * gamma_max() * gamma_max(), + kEps); + // The impulse saturates at the bound and opposes motion. + EXPECT_NEAR(e.gamma, -sign * gamma_max(), kEps * gamma_max()); + EXPECT_EQ(e.G, 0.0); + } +} + +// The cost and impulse must be continuous at the transition between stiction +// and sliding, |vc| = vs. The Hessian jumps from 1/R to zero. +TEST_F(SapJointFrictionConstraintTest, ContinuityAtTransition) { + for (const double sign : {-1.0, 1.0}) { + const double delta = 1.0e-10 * vs(); + const Evaluation below = Evaluate(sign * (vs() - delta)); + const Evaluation above = Evaluate(sign * (vs() + delta)); + // The cost has a continuous first derivative of magnitude γₘₐₓ at the + // transition, so across the width 2⋅delta it changes by 2⋅γₘₐₓ⋅delta up + // to second order terms (delta²/(2R), far below roundoff here). + EXPECT_NEAR(above.cost - below.cost, 2.0 * gamma_max() * delta, + 16 * kEps * above.cost); + // The impulse is Lipschitz with constant 1/R. + EXPECT_NEAR(below.gamma, above.gamma, 2.0 * delta / R()); + EXPECT_NEAR(below.G, 1.0 / R(), kEps / R()); + EXPECT_EQ(above.G, 0.0); + } +} + +// Verifies the principle of maximum dissipation: the impulse always opposes +// the constraint velocity and is bounded by gamma_max. +TEST_F(SapJointFrictionConstraintTest, MaximumDissipation) { + for (const double vc : VectorXd::LinSpaced(21, -5.0 * vs(), 5.0 * vs())) { + const Evaluation e = Evaluate(vc); + EXPECT_LE(e.gamma * vc, 0.0); + EXPECT_LE(std::abs(e.gamma), gamma_max() * (1.0 + kEps)); + } +} + +// This test validates the implementation of analytical constraint gradient and +// Hessian against numerical results obtained with automatic differentiation. +TEST_F(SapJointFrictionConstraintTest, ValidateConstraintGradients) { + std::unique_ptr abstract_data = + dut_ad_->MakeData(time_step_, Vector1(delassus_estimation_)); + + // Helper to validate gradients at the constraint velocity vc. + // It returns the impulse at vc. + auto validate_gradients = [&](double vc) { + VectorX gamma_ad(1); + VectorX vc_ad = math::InitializeAutoDiff(Vector1d(vc)); + dut_ad_->CalcData(vc_ad, abstract_data.get()); + dut_ad_->CalcImpulse(*abstract_data, &gamma_ad); + ValidateConstraintGradients(*dut_ad_, *abstract_data); + return math::ExtractValue(gamma_ad)(0); + }; + + // Stiction. + { + const double gamma = validate_gradients(0.4 * vs()); + EXPECT_LT(std::abs(gamma), gamma_max()); + EXPECT_LT(gamma, 0.0); + } + // Sliding in the positive direction. + { + const double gamma = validate_gradients(2.0 * vs()); + EXPECT_NEAR(gamma, -gamma_max(), kEps * gamma_max()); + } + // Sliding in the negative direction. + { + const double gamma = validate_gradients(-2.0 * vs()); + EXPECT_NEAR(gamma, gamma_max(), kEps * gamma_max()); + } +} + +TEST_F(SapJointFrictionConstraintTest, Clone) { + // N.B. Here we dynamic cast to the derived type so that we can test that the + // clone is a deep-copy of the original constraint. + auto clone = + dynamic_pointer_cast>(dut_->Clone()); + ASSERT_NE(clone, nullptr); + ExpectEqual(*dut_, *clone); + + // Test ToDouble. + auto clone_from_ad = dynamic_pointer_cast>( + dut_ad_->ToDouble()); + ASSERT_NE(clone_from_ad, nullptr); + ExpectEqual(*dut_, *clone_from_ad); +} + +TEST_F(SapJointFrictionConstraintTest, AccumulateGeneralizedImpulses) { + const MatrixXd J = dut_->first_clique_jacobian().MakeDenseMatrix(); + const int nv = dut_->num_velocities(0); + // Arbitrary impulse. + const Vector1d gamma(1.7); + // Arbitrary initial value. + const VectorXd tau0 = VectorXd::LinSpaced(nv, -5.0, 3.7); + const VectorXd tau_expected = tau0 + J.transpose() * gamma; + VectorXd tau = tau0; + dut_->AccumulateGeneralizedImpulses(0 /* Only one clique */, gamma, &tau); + EXPECT_TRUE( + CompareMatrices(tau, tau_expected, kEps, MatrixCompareType::relative)); +} + +} // namespace +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake