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