From b076de78d7b3123c9777cbbe5068f2779049aa5d Mon Sep 17 00:00:00 2001 From: Joe Masterjohn Date: Sat, 5 Aug 2023 12:51:04 -0400 Subject: [PATCH] [multibody] Implements SapCouplerConstraint and incorporates it into SapDriver (#19902) Also includes a fix for an existing bug in SapDriver::AddCouplerConstraint where the velocity index of a joint was incorrectly used to index the joint's positions. --- multibody/contact_solvers/sap/BUILD.bazel | 24 ++ .../sap/sap_coupler_constraint.cc | 95 +++++++ .../sap/sap_coupler_constraint.h | 117 ++++++++ .../sap/test/sap_coupler_constraint_test.cc | 252 ++++++++++++++++++ multibody/plant/sap_driver.cc | 52 +--- .../test/sap_driver_joint_limits_test.cc | 3 +- 6 files changed, 499 insertions(+), 44 deletions(-) create mode 100644 multibody/contact_solvers/sap/sap_coupler_constraint.cc create mode 100644 multibody/contact_solvers/sap/sap_coupler_constraint.h create mode 100644 multibody/contact_solvers/sap/test/sap_coupler_constraint_test.cc diff --git a/multibody/contact_solvers/sap/BUILD.bazel b/multibody/contact_solvers/sap/BUILD.bazel index 2ff3eacd64ae..7b3fc457a7e9 100644 --- a/multibody/contact_solvers/sap/BUILD.bazel +++ b/multibody/contact_solvers/sap/BUILD.bazel @@ -20,6 +20,7 @@ drake_cc_package_library( ":sap_constraint_bundle", ":sap_constraint_jacobian", ":sap_contact_problem", + ":sap_coupler_constraint", ":sap_distance_constraint", ":sap_friction_cone_constraint", ":sap_holonomic_constraint", @@ -106,6 +107,19 @@ drake_cc_library( ], ) +drake_cc_library( + name = "sap_coupler_constraint", + srcs = ["sap_coupler_constraint.cc"], + hdrs = ["sap_coupler_constraint.h"], + deps = [ + ":sap_constraint", + ":sap_constraint_jacobian", + ":sap_holonomic_constraint", + "//common:default_scalars", + "//common:essential", + ], +) + drake_cc_library( name = "sap_distance_constraint", srcs = ["sap_distance_constraint.cc"], @@ -251,6 +265,16 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "sap_coupler_constraint_test", + deps = [ + ":sap_coupler_constraint", + ":validate_constraint_gradients", + "//common:pointer_cast", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "sap_distance_constraint_test", deps = [ diff --git a/multibody/contact_solvers/sap/sap_coupler_constraint.cc b/multibody/contact_solvers/sap/sap_coupler_constraint.cc new file mode 100644 index 000000000000..b70ca1e1b012 --- /dev/null +++ b/multibody/contact_solvers/sap/sap_coupler_constraint.cc @@ -0,0 +1,95 @@ +#include "drake/multibody/contact_solvers/sap/sap_coupler_constraint.h" + +#include +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/eigen_types.h" + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { + +template +SapCouplerConstraint::SapCouplerConstraint(Kinematics kinematics) + : SapHolonomicConstraint( + MakeSapHolonomicConstraintKinematics(kinematics), + MakeSapHolonomicConstraintParameters(), {}), + kinematics_(std::move(kinematics)) {} + +template +typename SapHolonomicConstraint::Parameters +SapCouplerConstraint::MakeSapHolonomicConstraintParameters() { + // "Near-rigid" regime parameter, see [Castro et al., 2022]. + // TODO(amcastro-tri): consider exposing this parameter. + constexpr double kBeta = 0.1; + + // Coupler constraints do not have impulse limits, they are bi-lateral + // constraints. Each coupler constraint introduces a single constraint + // equation. + // + // N.B. `stiffness` is set to infinity to model this constraint in the + // "near-rigid" regime. `relaxation_time` is set arbitrarily to 0.0. + // SapHolonomicConstraint will use the time step as the relaxation time when + // it detects this constraint is "near-rigid". See + // SapHolonomicConstraint::DoMakeData(). + constexpr double kInf = std::numeric_limits::infinity(); + return typename SapHolonomicConstraint::Parameters{ + Vector1(-kInf), Vector1(kInf), Vector1(kInf), Vector1(0.0), + kBeta}; +} + +template +typename SapHolonomicConstraint::Kinematics +SapCouplerConstraint::MakeSapHolonomicConstraintKinematics( + const Kinematics& kinematics) { + Vector1 g(kinematics.q0 - kinematics.gear_ratio * kinematics.q1 - + kinematics.offset); // Constraint function. + Vector1 b = Vector1::Zero(); // Bias term. + + // Determine the single clique or two clique case. + if (kinematics.clique0 == kinematics.clique1) { + MatrixX J0 = MatrixX::Zero(1, kinematics.clique_nv0); + J0(0, kinematics.clique_dof0) = 1.0; + J0(0, kinematics.clique_dof1) = -kinematics.gear_ratio; + + return typename SapHolonomicConstraint::Kinematics( + std::move(g), + SapConstraintJacobian(kinematics.clique0, std::move(J0)), + std::move(b)); + } else { + MatrixX J0 = MatrixX::Zero(1, kinematics.clique_nv0); + MatrixX J1 = MatrixX::Zero(1, kinematics.clique_nv1); + J0(0, kinematics.clique_dof0) = 1.0; + J1(0, kinematics.clique_dof1) = -kinematics.gear_ratio; + + return typename SapHolonomicConstraint::Kinematics( + std::move(g), + SapConstraintJacobian(kinematics.clique0, std::move(J0), + kinematics.clique1, std::move(J1)), + std::move(b)); + } +} + +template +void SapCouplerConstraint::DoAccumulateGeneralizedImpulses( + int c, const Eigen::Ref>& gamma, + EigenPtr> tau) const { + // τ = Jᵀ⋅γ + if (c == 0) { + (*tau)(kinematics().clique_dof0) += gamma(0); + } + + if (this->num_cliques() == 1 || c == 1) { + (*tau)(kinematics().clique_dof1) -= kinematics().gear_ratio * gamma(0); + } +} + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::contact_solvers::internal::SapCouplerConstraint) diff --git a/multibody/contact_solvers/sap/sap_coupler_constraint.h b/multibody/contact_solvers/sap/sap_coupler_constraint.h new file mode 100644 index 000000000000..4c2775ee9b7c --- /dev/null +++ b/multibody/contact_solvers/sap/sap_coupler_constraint.h @@ -0,0 +1,117 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/multibody/contact_solvers/sap/sap_holonomic_constraint.h" + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { + +/* Implements a SAP (compliant) coupler constraint between two dofs. + Given positions of dof 0 and 1 as q₀ and q₁, respectively, this constraint + enforces: + q₀ = ρ⋅q₁ + Δq + where ρ is the gear ratio and Δq is a fixed offset. + + More precisely, this constraint enforces a single holonomic constraint + equation: + g = q₀ - ρ⋅q₁ - Δq = 0 + which produces a constraint impulse γ ∈ ℝ. + + The resulting generalized impulse on dof 0 is: + j₀ = γ + And the generalized impulse on dof 1 is: + j₁ = −ργ + + @tparam_nonsymbolic_scalar */ +template +class SapCouplerConstraint final : public SapHolonomicConstraint { + public: + /* We do not allow copy, move, or assignment generally to avoid slicing. */ + //@{ + SapCouplerConstraint& operator=(const SapCouplerConstraint&) = delete; + SapCouplerConstraint(SapCouplerConstraint&&) = delete; + SapCouplerConstraint& operator=(SapCouplerConstraint&&) = delete; + //@} + + /* Struct to store the kinematics of the the constraint in its current + configuration, when it gets constructed. */ + struct Kinematics { + /* Index of clique 0. */ + int clique0; + /* Clique local index of dof 0. */ + int clique_dof0; + /* Clique 0 number of velocities. */ + int clique_nv0; + /* Position of dof 0. */ + T q0; + /* Index of clique 1. */ + int clique1; + /* Clique local index of dof 1. */ + int clique_dof1; + /* Clique 1 number of velocities. */ + int clique_nv1; + /* Position of dof 1. */ + T q1; + /* Gear ratio ρ. */ + T gear_ratio; + /* Constraint function bias Δq. */ + T offset; + }; + + /* Constructs a coupler constraint given its kinematics in a particular + configuration. */ + explicit SapCouplerConstraint(Kinematics kinematics); + + const Kinematics& kinematics() const { return kinematics_; } + + private: + /* Private copy construction is enabled to use in the implementation of + DoClone(). */ + SapCouplerConstraint(const SapCouplerConstraint&) = default; + + /* Accumulates generalized impulses applied by this constraint on the c-th + clique. + @param[in] c The c-th clique of index clique(c). + @param[in] gamma Impulses for this constraint, of size + num_constraint_equations(). + @param[out] tau On output this function will accumulate the generalized + impulses applied by this constraint on the c-th clique. + */ + 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 {} + + /* Helper used at construction. This method makes the parameters needed by the + base class SapHolonomicConstraint. */ + static typename SapHolonomicConstraint::Parameters + MakeSapHolonomicConstraintParameters(); + + /* Helper used at construction. Makes the constraint function and Jacobian + needed to initialize the base class SapHolonomicConstraint. + @returns Holonomic constraint kinematics needed at construction of the + parent SapHolonomicConstraint. */ + static typename SapHolonomicConstraint::Kinematics + MakeSapHolonomicConstraintKinematics(const Kinematics& kinematics); + + std::unique_ptr> DoClone() const final { + return std::unique_ptr>( + new SapCouplerConstraint(*this)); + } + + Kinematics kinematics_; +}; + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/sap/test/sap_coupler_constraint_test.cc b/multibody/contact_solvers/sap/test/sap_coupler_constraint_test.cc new file mode 100644 index 000000000000..183a8e2ebbfc --- /dev/null +++ b/multibody/contact_solvers/sap/test/sap_coupler_constraint_test.cc @@ -0,0 +1,252 @@ +#include "drake/multibody/contact_solvers/sap/sap_coupler_constraint.h" + +#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/validate_constraint_gradients.h" + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { +namespace { + +template +typename SapCouplerConstraint::Kinematics MakeArbitraryKinematics( + int num_cliques) { + const int clique0 = 4; + const int clique_dof0 = 2; + const int clique_nv0 = 5; + const T q0 = 0.5; + const int clique1 = (num_cliques == 1 ? clique0 : 9); + const int clique_dof1 = 3; + const int clique_nv1 = (num_cliques == 1 ? clique_nv0 : 7); + const T q1 = 2.3; + const T gear_ratio = 1.2; + const T offset = 0.7; + + return typename SapCouplerConstraint::Kinematics{ + clique0, clique_dof0, clique_nv0, q0, clique1, + clique_dof1, clique_nv1, q1, gear_ratio, offset}; +} + +GTEST_TEST(SapCouplerConstraint, SingleCliqueConstraint) { + const int num_cliques = 1; + const SapCouplerConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapCouplerConstraint c(kinematics); + + EXPECT_EQ(c.num_objects(), 0); + EXPECT_EQ(c.num_constraint_equations(), 1); + EXPECT_EQ(c.num_cliques(), 1); + EXPECT_EQ(c.first_clique(), kinematics.clique0); + EXPECT_EQ(c.num_velocities(0), kinematics.clique_nv0); + EXPECT_THROW(c.second_clique(), std::exception); + + const MatrixX J0 = c.first_clique_jacobian().MakeDenseMatrix(); + + // The Jacobian should contain only two entries corresponding to the + // constraints two dofs. + EXPECT_EQ(J0(kinematics.clique_dof0), 1.0); + EXPECT_EQ(J0(kinematics.clique_dof1), -kinematics.gear_ratio); + EXPECT_EQ(J0.sum(), 1.0 - kinematics.gear_ratio); + + EXPECT_THROW(c.second_clique_jacobian(), std::exception); +} + +GTEST_TEST(SapCouplerConstraint, TwoCliquesConstraint) { + const int num_cliques = 2; + const SapCouplerConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapCouplerConstraint c(kinematics); + + EXPECT_EQ(c.num_objects(), 0); + EXPECT_EQ(c.num_constraint_equations(), 1); + EXPECT_EQ(c.num_cliques(), 2); + EXPECT_EQ(c.first_clique(), kinematics.clique0); + EXPECT_EQ(c.second_clique(), kinematics.clique1); + EXPECT_EQ(c.num_velocities(0), kinematics.clique_nv0); + EXPECT_EQ(c.num_velocities(1), kinematics.clique_nv1); + + const MatrixX J0 = c.first_clique_jacobian().MakeDenseMatrix(); + const MatrixX J1 = c.second_clique_jacobian().MakeDenseMatrix(); + + // J0 and J1 should each contain exactly one entry corresponding to dof 0 or + // dof 1, respectively. + EXPECT_EQ(J0(kinematics.clique_dof0), 1.0); + EXPECT_EQ(J0.sum(), 1.0); + EXPECT_EQ(J1(kinematics.clique_dof1), -kinematics.gear_ratio); + EXPECT_EQ(J1.sum(), -kinematics.gear_ratio); +} + +// This method validates analytical gradients implemented by +// SapCouplerConstraint using automatic differentiation. +void ValidateProjection(const Vector1d& vc) { + // Arbitrary kinematic values. + const int num_cliques = 1; + const SapCouplerConstraint::Kinematics kin_ad = + MakeArbitraryKinematics(num_cliques); + + // Instantiate constraint on AutoDiffXd for automatic differentiation. + SapCouplerConstraint c(kin_ad); + + // Verify cost gradients using AutoDiffXd. + ValidateConstraintGradients(c, vc); +} + +GTEST_TEST(SapCouplerConstraint, Gradients) { + // Arbitrary set of vc values. + { + const Vector1d vc(-1.4); + ValidateProjection(vc); + } + { + const Vector1d vc(2.3); + ValidateProjection(vc); + } + { + const Vector1d vc(6.2); + ValidateProjection(vc); + } +} + +GTEST_TEST(SapCouplerConstraint, SingleCliqueConstraintClone) { + const int num_cliques = 1; + const SapCouplerConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapCouplerConstraint c(kinematics); + + // 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>(c.Clone()); + ASSERT_NE(clone, nullptr); + EXPECT_EQ(clone->num_objects(), 0); + EXPECT_EQ(clone->num_constraint_equations(), 1); + EXPECT_EQ(clone->num_cliques(), 1); + EXPECT_EQ(clone->first_clique(), kinematics.clique0); + EXPECT_EQ(clone->num_velocities(0), kinematics.clique_nv0); + EXPECT_THROW(clone->second_clique(), std::exception); + + const MatrixX J0 = clone->first_clique_jacobian().MakeDenseMatrix(); + + // The Jacobian should contain only two entries corresponding to the + // constraints two dofs. + EXPECT_EQ(J0(kinematics.clique_dof0), 1.0); + EXPECT_EQ(J0(kinematics.clique_dof1), -kinematics.gear_ratio); + EXPECT_EQ(J0.sum(), 1.0 - kinematics.gear_ratio); + + EXPECT_THROW(clone->second_clique_jacobian(), std::exception); +} + +GTEST_TEST(SapCouplerConstraint, TwoCliquesConstraintClone) { + const int num_cliques = 2; + const SapCouplerConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapCouplerConstraint c(kinematics); + + auto clone = dynamic_pointer_cast>(c.Clone()); + ASSERT_NE(clone, nullptr); + EXPECT_EQ(clone->num_objects(), 0); + EXPECT_EQ(clone->num_constraint_equations(), 1); + EXPECT_EQ(clone->num_cliques(), 2); + EXPECT_EQ(clone->first_clique(), kinematics.clique0); + EXPECT_EQ(clone->second_clique(), kinematics.clique1); + EXPECT_EQ(clone->num_velocities(0), kinematics.clique_nv0); + EXPECT_EQ(clone->num_velocities(1), kinematics.clique_nv1); + + const MatrixX J0 = clone->first_clique_jacobian().MakeDenseMatrix(); + const MatrixX J1 = clone->second_clique_jacobian().MakeDenseMatrix(); + + // J0 and J1 should each contain exactly one entry corresponding to dof 0 or + // dof 1, respectively. + EXPECT_EQ(J0(kinematics.clique_dof0), 1.0); + EXPECT_EQ(J0.sum(), 1.0); + EXPECT_EQ(J1(kinematics.clique_dof1), -kinematics.gear_ratio); + EXPECT_EQ(J1.sum(), -kinematics.gear_ratio); +} + +GTEST_TEST(SapCouplerConstraint, SingleCliqueAccumulateGeneralizedImpulses) { + // Make a coupler constraint with an arbitrary kinamtics state, irrelevant for + // this test but needed at construction. + const int num_cliques = 1; + const SapCouplerConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapCouplerConstraint c(kinematics); + + // Arbitrary value of the impulse. + const Vector1d gamma(1.2345); + + // Expected generalized impulse on clique 0. + VectorXd tau_clique0 = VectorXd::Zero(kinematics.clique_nv0); + tau_clique0(kinematics.clique_dof0) = gamma(0); + + // Expected generalized impulse on clique 1. + VectorXd tau_clique1 = VectorXd::Zero(kinematics.clique_nv1); + tau_clique1(kinematics.clique_dof1) = -kinematics.gear_ratio * gamma(0); + + const VectorXd tau_0 = + VectorXd::LinSpaced(kinematics.clique_nv0, 1, kinematics.clique_nv0); + + VectorXd tau_accumulated = tau_0; // Initialize to non-zero value. + const VectorXd tau_expected = tau_0 + tau_clique0 + tau_clique1; + c.AccumulateGeneralizedImpulses(0, gamma, &tau_accumulated); + EXPECT_TRUE(CompareMatrices(tau_accumulated, tau_expected, + std::numeric_limits::epsilon(), + MatrixCompareType::relative)); + + // Calling on clique one when num_cliques() == 1 should throw an exception. + EXPECT_THROW(c.AccumulateGeneralizedImpulses(1, gamma, &tau_accumulated), + std::exception); +} + +GTEST_TEST(SapCouplerConstraint, TwoCliquesAccumulateGeneralizedImpulses) { + // Make a coupler constraint with an arbitrary kinamtics state, irrelevant for + // this test but needed at construction. + const int num_cliques = 2; + const SapCouplerConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapCouplerConstraint c(kinematics); + + // Arbitrary value of the impulse. + const Vector1d gamma(1.2345); + + // Expected generalized impulse on clique 0. + VectorXd tau_clique0 = VectorXd::Zero(kinematics.clique_nv0); + tau_clique0(kinematics.clique_dof0) = gamma(0); + + // Expected generalized impulse on clique 1. + VectorXd tau_clique1 = VectorXd::Zero(kinematics.clique_nv1); + tau_clique1(kinematics.clique_dof1) = -kinematics.gear_ratio * gamma(0); + + const VectorXd tau_0 = + VectorXd::LinSpaced(kinematics.clique_nv0, 1, kinematics.clique_nv0); + + VectorXd tau_accumulated = tau_0; // Initialize to non-zero value. + VectorXd tau_expected = tau_0 + tau_clique0; + c.AccumulateGeneralizedImpulses(0, gamma, &tau_accumulated); + EXPECT_TRUE(CompareMatrices(tau_accumulated, tau_expected, + std::numeric_limits::epsilon(), + MatrixCompareType::relative)); + + const VectorXd tau_1 = + VectorXd::LinSpaced(kinematics.clique_nv1, 1, kinematics.clique_nv1); + + tau_accumulated = tau_1; // Initialize to non-zero value. + tau_expected = tau_1 + tau_clique1; + c.AccumulateGeneralizedImpulses(1, gamma, &tau_accumulated); + EXPECT_TRUE(CompareMatrices(tau_accumulated, tau_expected, + std::numeric_limits::epsilon(), + MatrixCompareType::relative)); +} + +} // namespace +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/plant/sap_driver.cc b/multibody/plant/sap_driver.cc index 9665b7ed5dc0..f61ae9aeed98 100644 --- a/multibody/plant/sap_driver.cc +++ b/multibody/plant/sap_driver.cc @@ -13,6 +13,7 @@ #include "drake/multibody/contact_solvers/contact_configuration.h" #include "drake/multibody/contact_solvers/contact_solver_utils.h" #include "drake/multibody/contact_solvers/sap/sap_contact_problem.h" +#include "drake/multibody/contact_solvers/sap/sap_coupler_constraint.h" #include "drake/multibody/contact_solvers/sap/sap_distance_constraint.h" #include "drake/multibody/contact_solvers/sap/sap_friction_cone_constraint.h" #include "drake/multibody/contact_solvers/sap/sap_holonomic_constraint.h" @@ -33,6 +34,7 @@ using drake::multibody::contact_solvers::internal::MatrixBlock; using drake::multibody::contact_solvers::internal::SapConstraint; using drake::multibody::contact_solvers::internal::SapConstraintJacobian; using drake::multibody::contact_solvers::internal::SapContactProblem; +using drake::multibody::contact_solvers::internal::SapCouplerConstraint; using drake::multibody::contact_solvers::internal::SapDistanceConstraint; using drake::multibody::contact_solvers::internal::SapFrictionConeConstraint; using drake::multibody::contact_solvers::internal::SapHolonomicConstraint; @@ -335,20 +337,6 @@ void SapDriver::AddCouplerConstraints(const systems::Context& context, SapContactProblem* problem) const { DRAKE_DEMAND(problem != nullptr); - // Previous time step positions. - const VectorX q0 = plant().GetPositions(context); - - // Couplers do not have impulse limits, they are bi-lateral constraints. Each - // coupler constraint introduces a single constraint equation. - constexpr double kInfinity = std::numeric_limits::infinity(); - const Vector1 gamma_lower(-kInfinity); - const Vector1 gamma_upper(kInfinity); - - // Stiffness and dissipation are set so that the constraint is in the - // "near-rigid" regime, [Castro et al., 2022]. - const Vector1 stiffness(kInfinity); - const Vector1 relaxation_time(plant().time_step()); - const std::map& constraint_active_status = manager().GetConstraintActiveStatus(context); @@ -369,36 +357,16 @@ void SapDriver::AddCouplerConstraints(const systems::Context& context, const int tree_dof0 = dof0 - tree_topology().tree_velocities_start(tree0); const int tree_dof1 = dof1 - tree_topology().tree_velocities_start(tree1); - // Constraint function defined as g = q₀ - ρ⋅q₁ - Δq, with ρ the gear ratio - // and Δq a fixed position offset. - Vector1 g0(q0[dof0] - info.gear_ratio * q0[dof1] - info.offset); + const int tree_nv0 = tree_topology().num_tree_velocities(tree0); + const int tree_nv1 = tree_topology().num_tree_velocities(tree1); - // TODO(amcastro-tri): consider exposing this parameter. - const double beta = 0.1; + const typename SapCouplerConstraint::Kinematics kinematics{ + tree0, tree_dof0, tree_nv0, joint0.GetOnePosition(context), + tree1, tree_dof1, tree_nv1, joint1.GetOnePosition(context), + info.gear_ratio, info.offset}; - const typename SapHolonomicConstraint::Parameters parameters{ - gamma_lower, gamma_upper, stiffness, relaxation_time, beta}; - - if (tree0 == tree1) { - const int nv = tree_topology().num_tree_velocities(tree0); - MatrixX J0 = MatrixX::Zero(1, nv); - // J = dg/dv - J0(0, tree_dof0) = 1.0; - J0(0, tree_dof1) = -info.gear_ratio; - SapConstraintJacobian J(tree0, std::move(J0)); - problem->AddConstraint(std::make_unique>( - std::move(g0), std::move(J), parameters)); - } else { - const int nv0 = tree_topology().num_tree_velocities(tree0); - const int nv1 = tree_topology().num_tree_velocities(tree1); - MatrixX J0 = MatrixX::Zero(1, nv0); - MatrixX J1 = MatrixX::Zero(1, nv1); - J0(0, tree_dof0) = 1.0; - J1(0, tree_dof1) = -info.gear_ratio; - SapConstraintJacobian J(tree0, std::move(J0), tree1, std::move(J1)); - problem->AddConstraint(std::make_unique>( - std::move(g0), std::move(J), parameters)); - } + problem->AddConstraint( + std::make_unique>(std::move(kinematics))); } } diff --git a/multibody/plant/test/sap_driver_joint_limits_test.cc b/multibody/plant/test/sap_driver_joint_limits_test.cc index 182dbe3a9763..e7d4ba660943 100644 --- a/multibody/plant/test/sap_driver_joint_limits_test.cc +++ b/multibody/plant/test/sap_driver_joint_limits_test.cc @@ -674,8 +674,7 @@ TEST_F(KukaIiwaArmTests, CouplerConstraints) { EXPECT_EQ(params.impulse_lower_limits(), -kInfinity); EXPECT_EQ(params.impulse_upper_limits(), kInfinity); EXPECT_EQ(params.stiffnesses(), kInfinity); - EXPECT_EQ(params.relaxation_times(), - Vector1d::Constant(plant_.time_step())); + EXPECT_EQ(params.relaxation_times(), Vector1d::Zero()); EXPECT_EQ(params.beta(), 0.1); } }