From dbadf199b36729cd6307d4f99c3c0371a272b2b5 Mon Sep 17 00:00:00 2001 From: Alejandro Castro Date: Tue, 1 Aug 2023 11:31:22 -0400 Subject: [PATCH] Implements SapDistanceConstraint and incorporates it into SapDriver (#19846) --- multibody/contact_solvers/sap/BUILD.bazel | 24 ++ .../sap/sap_constraint_jacobian.cc | 27 ++ .../sap/sap_constraint_jacobian.h | 9 + .../sap/sap_distance_constraint.cc | 93 +++++++ .../sap/sap_distance_constraint.h | 209 ++++++++++++++ .../sap/sap_holonomic_constraint.cc | 9 + .../sap/sap_holonomic_constraint.h | 37 ++- .../sap/test/sap_distance_constraint_test.cc | 260 ++++++++++++++++++ multibody/plant/sap_driver.cc | 126 ++++----- 9 files changed, 715 insertions(+), 79 deletions(-) create mode 100644 multibody/contact_solvers/sap/sap_distance_constraint.cc create mode 100644 multibody/contact_solvers/sap/sap_distance_constraint.h create mode 100644 multibody/contact_solvers/sap/test/sap_distance_constraint_test.cc diff --git a/multibody/contact_solvers/sap/BUILD.bazel b/multibody/contact_solvers/sap/BUILD.bazel index 90a8ea64bc0a..2ff3eacd64ae 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_distance_constraint", ":sap_friction_cone_constraint", ":sap_holonomic_constraint", ":sap_limit_constraint", @@ -105,6 +106,19 @@ drake_cc_library( ], ) +drake_cc_library( + name = "sap_distance_constraint", + srcs = ["sap_distance_constraint.cc"], + hdrs = ["sap_distance_constraint.h"], + deps = [ + ":sap_constraint", + ":sap_constraint_jacobian", + ":sap_holonomic_constraint", + "//common:default_scalars", + "//common:essential", + ], +) + drake_cc_library( name = "sap_holonomic_constraint", srcs = ["sap_holonomic_constraint.cc"], @@ -237,6 +251,16 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "sap_distance_constraint_test", + deps = [ + ":sap_distance_constraint", + ":validate_constraint_gradients", + "//common:pointer_cast", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "sap_holonomic_constraint_test", deps = [ diff --git a/multibody/contact_solvers/sap/sap_constraint_jacobian.cc b/multibody/contact_solvers/sap/sap_constraint_jacobian.cc index 0dc46f4c4058..60ae70029c90 100644 --- a/multibody/contact_solvers/sap/sap_constraint_jacobian.cc +++ b/multibody/contact_solvers/sap/sap_constraint_jacobian.cc @@ -37,6 +37,33 @@ SapConstraintJacobian::SapConstraintJacobian(int first_clique, first_clique, MatrixBlock(std::move(J_first_clique)), second_clique, MatrixBlock(std::move(J_second_clique))) {} +template +bool SapConstraintJacobian::blocks_are_dense() const { + bool is_dense = clique_jacobian(0).is_dense(); + if (num_cliques() == 2) { + is_dense = is_dense && clique_jacobian(1).is_dense(); + } + return is_dense; +} + +template +SapConstraintJacobian SapConstraintJacobian::LeftMultiplyByTranspose( + const Eigen::Ref>& A) const { + DRAKE_THROW_UNLESS(blocks_are_dense()); + + // Contribution from first the clique. + const MatrixX J_first_clique = clique_jacobian(0).MakeDenseMatrix(); + MatrixX ATJ_first_clique = A.transpose() * J_first_clique; + if (num_cliques() == 1) { + return SapConstraintJacobian(clique(0), std::move(ATJ_first_clique)); + } + // Jacobian involving two cliques, second clique. + const MatrixX J_second_clique = clique_jacobian(1).MakeDenseMatrix(); + MatrixX ATJ_second_clique = A.transpose() * J_second_clique; + return SapConstraintJacobian(clique(0), std::move(ATJ_first_clique), + clique(1), std::move(ATJ_second_clique)); +} + } // namespace internal } // namespace contact_solvers } // namespace multibody diff --git a/multibody/contact_solvers/sap/sap_constraint_jacobian.h b/multibody/contact_solvers/sap/sap_constraint_jacobian.h index 2dd352ef6e4b..c5a904ced69e 100644 --- a/multibody/contact_solvers/sap/sap_constraint_jacobian.h +++ b/multibody/contact_solvers/sap/sap_constraint_jacobian.h @@ -100,6 +100,15 @@ class SapConstraintJacobian { return clique_jacobians_[local_clique].block; } + /* Returns `true` iff both clique blocks of this Jacobian are dense. */ + bool blocks_are_dense() const; + + // TODO(amcastro-tri): extend this method to support non-dense blocks. + /* Returns Y = Aᵀ⋅J, with J being `this` Jacobian matrix. + @pre blocks_are_dense() is true. */ + SapConstraintJacobian LeftMultiplyByTranspose( + const Eigen::Ref>& A) const; + private: // Blocks for each block. Up to two entries only. std::vector> clique_jacobians_; diff --git a/multibody/contact_solvers/sap/sap_distance_constraint.cc b/multibody/contact_solvers/sap/sap_distance_constraint.cc new file mode 100644 index 000000000000..a2e203db20ea --- /dev/null +++ b/multibody/contact_solvers/sap/sap_distance_constraint.cc @@ -0,0 +1,93 @@ +#include "drake/multibody/contact_solvers/sap/sap_distance_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 +SapDistanceConstraint::ComplianceParameters::ComplianceParameters( + T stiffness, T damping) + : stiffness_(std::move(stiffness)), damping_(std::move(damping)) { + DRAKE_DEMAND(stiffness_ > 0.0); + DRAKE_DEMAND(damping_ >= 0.0); +} + +template +SapDistanceConstraint::SapDistanceConstraint(Kinematics kinematics, + ComplianceParameters parameters) + : SapHolonomicConstraint( + MakeSapHolonomicConstraintKinematics(kinematics), + MakeSapHolonomicConstraintParameters(parameters), + {kinematics.objectA(), kinematics.objectB()}), + kinematics_(std::move(kinematics)), + parameters_(std::move(parameters)) {} + +template +typename SapHolonomicConstraint::Parameters +SapDistanceConstraint::MakeSapHolonomicConstraintParameters( + const ComplianceParameters& p) { + // "Near-rigid" regime parameter, see [Castro et al., 2022]. + // TODO(amcastro-tri): consider exposing this parameter. + constexpr double kBeta = 0.1; + + // Distance constraints do not have impulse limits, they are bi-lateral + // constraints. Each distance constraint introduces a single constraint + // equation. + constexpr double kInf = std::numeric_limits::infinity(); + return typename SapHolonomicConstraint::Parameters{ + Vector1(-kInf), Vector1(kInf), Vector1(p.stiffness()), + Vector1(p.damping() / p.stiffness()), kBeta}; +} + +template +typename SapHolonomicConstraint::Kinematics +SapDistanceConstraint::MakeSapHolonomicConstraintKinematics( + const Kinematics& kinematics) { + Vector1 g(kinematics.distance() - + kinematics.length()); // Constraint function. + Vector1 b = Vector1::Zero(); // Bias term. + + // The constraint Jacobian is the projection of the relative velocity between + // P and Q on p_hat. That is ḋ = p̂⋅v_PQ_W = p̂⋅(J_WBq - J_WAp)⋅v. + // Therefore the distance constraint Jacobian is Jdist = p̂⋅J. + SapConstraintJacobian Jdist = + kinematics.jacobian().LeftMultiplyByTranspose(kinematics.p_hat_W()); + + return typename SapHolonomicConstraint::Kinematics( + std::move(g), std::move(Jdist), std::move(b)); +} + +template +void SapDistanceConstraint::DoAccumulateSpatialImpulses( + int i, const Eigen::Ref>& gamma, + SpatialForce* F) const { + if (i == 0) { + // Object A. + const Vector3 f_Ap_W = -gamma(0) * kinematics_.p_hat_W(); + const Vector3 t_A_W = kinematics_.p_AP_W().cross(f_Ap_W); + const SpatialForce F_Ao_W(t_A_W, f_Ap_W); + *F += F_Ao_W; + } else { + // Object B. + const Vector3 f_Bq_W = gamma(0) * kinematics_.p_hat_W(); + const Vector3 t_B_W = kinematics_.p_BQ_W().cross(f_Bq_W); + const SpatialForce F_Bo_W(t_B_W, f_Bq_W); + *F += F_Bo_W; + return; + } +} + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::contact_solvers::internal::SapDistanceConstraint) diff --git a/multibody/contact_solvers/sap/sap_distance_constraint.h b/multibody/contact_solvers/sap/sap_distance_constraint.h new file mode 100644 index 000000000000..bf485b7d4825 --- /dev/null +++ b/multibody/contact_solvers/sap/sap_distance_constraint.h @@ -0,0 +1,209 @@ +#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) distance constraint between two points. With + finite compliance, this constraint can be used to model a linear spring between + two points. + + To be more precise, consider a point P on an object A and point Q on an object + B. With d the distance between points P and Q and ḋ its rate of change, this + SAP constraint models the constraint impulse as γ ∈ ℝ: + γ = −k⋅(d−ℓ) − c⋅ḋ + where ℓ is the "free length" of the constraint, and k and c are the stiffness + and damping coefficients respectively. + + With p̂ the unit vector from P to Q, the force vector on B applied at Q is: + f_Bq = γ⋅p̂ + and the reaction force on A applied at P is: + f_Ap = -γ⋅p̂ + + @tparam_nonsymbolic_scalar */ +template +class SapDistanceConstraint final : public SapHolonomicConstraint { + public: + /* We do not allow copy, move, or assignment generally to avoid slicing. */ + //@{ + SapDistanceConstraint& operator=(const SapDistanceConstraint&) = delete; + SapDistanceConstraint(SapDistanceConstraint&&) = delete; + SapDistanceConstraint& operator=(SapDistanceConstraint&&) = delete; + //@} + + /* ComplianceParameters that define the constraint's compliance. */ + class ComplianceParameters { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ComplianceParameters); + + /* Stiffness k and damping c. */ + ComplianceParameters(T stiffness, T damping); + + const T& stiffness() const { return stiffness_; } + + const T& damping() const { return damping_; } + + private: + T stiffness_; + T damping_; + }; + + /* Class to store the kinematics of the the constraint in its current + configuration, when it gets constructed. */ + class Kinematics { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Kinematics); + + /* @param[in] objectA + Index of the physical object A on which point P attaches. + @param[in] p_WP_W Position of point P in the world frame. + @param[in] p_AP_W Position of point P in A, expressed in the world frame. + @param[in] objectB + Index of the physical object B on which point Q attaches. + @param[in] p_WQ_W Position of point Q in the world frame. + @param[in] p_BQ_W Position of point Q in B, expressed in the world frame. + @param[in] length The constraint's length. + @param[in] J_ApBq_W Jacobian for the relative velocity v_ApBq_W. */ + Kinematics(int objectA, Vector3 p_WP_W, Vector3 p_AP_W, int objectB, + Vector3 p_WQ_W, Vector3 p_BQ_W, T length, + SapConstraintJacobian J_ApBq_W) + : objectA_(objectA), + p_WP_(std::move(p_WP_W)), + p_AP_W_(std::move(p_AP_W)), + objectB_(objectB), + p_WQ_(std::move(p_WQ_W)), + p_BQ_W_(std::move(p_BQ_W)), + length_(std::move(length)), + J_(std::move(J_ApBq_W)) { + // Thus far only dense Jacobian blocks are supported, i.e. rigid body + // applications in mind. + DRAKE_THROW_UNLESS(J_.blocks_are_dense()); + + const Vector3 p_PQ_W = p_WQ_ - p_WP_; + distance_ = p_PQ_W.norm(); + // Verify the distance did not become ridiculously small. We use the + // constraint's fixed length as a reference. + constexpr double kMinimumDistance = 1.0e-7; + constexpr double kRelativeDistance = 1.0e-2; + if (distance_ < kMinimumDistance + kRelativeDistance * length_) { + throw std::logic_error( + fmt::format("The distance is {}. This is nonphysically small when " + "compared to the free length of the constraint, {}. ", + distance_, length_)); + } + p_hat_W_ = p_PQ_W / distance_; + } + + int objectA() const { return objectA_; } + const Vector3& p_WP() const { return p_WP_; } + const Vector3& p_AP_W() const { return p_AP_W_; } + int objectB() const { return objectB_; } + const Vector3& p_WQ() const { return p_WQ_; } + const Vector3& p_BQ_W() const { return p_BQ_W_; } + const T& length() const { return length_; } + const T& distance() const { return distance_; } + const SapConstraintJacobian& jacobian() const { return J_; } + const Vector3& p_hat_W() const { return p_hat_W_; } + + private: + /* Index to a physical object A. */ + int objectA_; + + /* Position of point P in the world. */ + Vector3 p_WP_; + + /* Position of point P relative to A. */ + Vector3 p_AP_W_; + + /* Index to a physical object B. */ + int objectB_; + + /* Position of point Q in the world. */ + Vector3 p_WQ_; + + /* Position of point Q relative to B. */ + Vector3 p_BQ_W_; + + /* Fixed length of the constraint. */ + T length_; + + /* Jacobian that defines the velocity v_ApBq_W of point P relative to point + Q, expressed in the world frame. That is, v_ApBq_W = J⋅v. */ + SapConstraintJacobian J_; + + /* Distance between point P and Q. */ + T distance_; + + /* Versor pointing from point P to point Q, expressed in the world frame W. + */ + Vector3 p_hat_W_; + }; + + /* Constructs a distance constraint given its kinematics in a particular + configuration and the set of parameters that define the constraint. */ + SapDistanceConstraint(Kinematics kinematics, ComplianceParameters parameters); + + /* The constraint's length. */ + const T& length() const { return kinematics_.length(); } + + const ComplianceParameters& compliance_parameters() const { + return parameters_; + } + + private: + /* Private copy construction is enabled to use in the implementation of + DoClone(). */ + SapDistanceConstraint(const SapDistanceConstraint&) = default; + + // no-op for this constraint. + void DoAccumulateGeneralizedImpulses(int, const Eigen::Ref>&, + EigenPtr>) const final {} + + /* Accumulates generalized forces applied by this constraint on the i-th + object. + @param[in] i Object index. As defined at construction i = 0 corresponds to + object A and i = 1 corresponds to object B. + @param[in] gamma A vector of size 1, with the (scalar) constraint impulse. + @param[out] F On output, this method accumulates the total spatial impulse + applied by this constraint on the i-th object. + @pre 0 ≤ i < 2. + @pre gamma is a vector of size one. + @pre F is not nullptr. */ + void DoAccumulateSpatialImpulses(int i, + const Eigen::Ref>& gamma, + SpatialForce* F) const final; + + /* Helper used at construction. Given parameters `p` for a + SapDistanceConstraint, this method makes the parameters needed by the base + class SapHolonomicConstraint. */ + static typename SapHolonomicConstraint::Parameters + MakeSapHolonomicConstraintParameters(const ComplianceParameters& p); + + /* 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 SapDistanceConstraint(*this)); + } + + Kinematics kinematics_; + ComplianceParameters parameters_; +}; + +} // namespace internal +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/sap/sap_holonomic_constraint.cc b/multibody/contact_solvers/sap/sap_holonomic_constraint.cc index 9a5745da0ce6..6177dce6bd3b 100644 --- a/multibody/contact_solvers/sap/sap_holonomic_constraint.cc +++ b/multibody/contact_solvers/sap/sap_holonomic_constraint.cc @@ -33,6 +33,15 @@ SapHolonomicConstraint::Parameters::Parameters( DRAKE_DEMAND((relaxation_times.array() >= 0).all()); } +template +SapHolonomicConstraint::SapHolonomicConstraint(Kinematics kinematics, + Parameters parameters, + std::vector objects) + : SapConstraint(std::move(kinematics.J), std::move(objects)), + g_(std::move(kinematics.g)), + bias_(std::move(kinematics.b)), + parameters_(std::move(parameters)) {} + template SapHolonomicConstraint::SapHolonomicConstraint(VectorX g, SapConstraintJacobian J, diff --git a/multibody/contact_solvers/sap/sap_holonomic_constraint.h b/multibody/contact_solvers/sap/sap_holonomic_constraint.h index 25ef34809cf2..9c58dc90d114 100644 --- a/multibody/contact_solvers/sap/sap_holonomic_constraint.h +++ b/multibody/contact_solvers/sap/sap_holonomic_constraint.h @@ -2,6 +2,7 @@ #include #include +#include #include "drake/common/drake_copyable.h" #include "drake/common/eigen_types.h" @@ -120,7 +121,7 @@ class SapHolonomicConstraintData { @tparam_nonsymbolic_scalar */ template -class SapHolonomicConstraint final : public SapConstraint { +class SapHolonomicConstraint : 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 @@ -181,6 +182,31 @@ class SapHolonomicConstraint final : public SapConstraint { double beta_{0.1}; }; + /* Struct to store the kinematic values for the constraint function g, its + Jacobian J and bias b. See this class's documentation for details. */ + struct Kinematics { + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Kinematics); + + /* Constraint kinematics values of the constraint function g, Jacobian J and + bias b. Refer to SapHolonomicConstraint for details. + @throws std::exception if sizes g_in.size(), b_in.size() and J_in.rows() + are not all equal. */ + Kinematics(VectorX g_in, SapConstraintJacobian J_in, VectorX b_in) + : g(std::move(g_in)), J(std::move(J_in)), b(std::move(b_in)) { + DRAKE_THROW_UNLESS(g.size() == J.rows()); + DRAKE_THROW_UNLESS(b.size() == g.size()); + } + + VectorX g; // Constraint function g. + SapConstraintJacobian J; // Constraint Jacobian J. + VectorX b; // Bias term. + }; + + /* Constructor for a holonomic constraint given its kinematics and + parameters. */ + SapHolonomicConstraint(Kinematics kinematics, Parameters parameters, + std::vector objects); + /* Constructs a holonomic constraint with zero bias term. @param[in] g The value of the constraint function. @param[in] J The Jacobian w.r.t. to the clique's generalized velocities. @@ -211,11 +237,12 @@ class SapHolonomicConstraint final : public SapConstraint { /* Returns the holonomic constraint bias b. */ const VectorX& bias() const { return bias_; } - private: - /* Private copy construction is enabled to use in the implementation of - DoClone(). */ + protected: + /* Protected copy construction is enabled to use in the implementation of + DoClone() here and for specific derived holonomic constraints. */ SapHolonomicConstraint(const SapHolonomicConstraint&) = default; + private: /* Implementations of SapConstraint NVI functions. */ std::unique_ptr DoMakeData( const T& time_step, @@ -227,7 +254,7 @@ class SapHolonomicConstraint final : public SapConstraint { EigenPtr> gamma) const final; void DoCalcCostHessian(const AbstractValue& abstract_data, MatrixX* G) const final; - std::unique_ptr> DoClone() const final { + std::unique_ptr> DoClone() const override { return std::unique_ptr>( new SapHolonomicConstraint(*this)); } diff --git a/multibody/contact_solvers/sap/test/sap_distance_constraint_test.cc b/multibody/contact_solvers/sap/test/sap_distance_constraint_test.cc new file mode 100644 index 000000000000..48ee01522f13 --- /dev/null +++ b/multibody/contact_solvers/sap/test/sap_distance_constraint_test.cc @@ -0,0 +1,260 @@ +#include "drake/multibody/contact_solvers/sap/sap_distance_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/math/rotation_matrix.h" +#include "drake/multibody/contact_solvers/sap/validate_constraint_gradients.h" + +using drake::math::RotationMatrix; +using Eigen::Matrix3d; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace internal { +namespace { + +// These Jacobian matrices have arbitrary values for testing. We specify the +// size of the matrix in the name, e.g. J32 is of size 3x2. +// clang-format off +const MatrixXd J32 = + (MatrixXd(3, 2) << 2, 1, + 1, 2, + 1, 2).finished(); + +const MatrixXd J34 = + (MatrixXd(3, 4) << 7, 1, 2, 3, + 1, 8, 4, 5, + 2, 4, 9, 6).finished(); +// clang-format on + +template +typename SapDistanceConstraint::ComplianceParameters +MakeArbitraryParameters() { + const T stiffness = 1.0e5; + const T damping = 1.0e3; + return typename SapDistanceConstraint::ComplianceParameters{stiffness, + damping}; +} + +template +typename SapDistanceConstraint::Kinematics MakeArbitraryKinematics( + int num_cliques) { + const int objectA = 12; + Vector3 p_WP(1., 2., 3.); + Vector3 p_AP_W(4., 5., 6.); + const int objectB = 5; + Vector3 p_WQ(7., 8., 9.); + Vector3 p_BQ_W(10., 11., 12.); + const int clique0 = 3; + const int clique1 = 12; + const T length = 0.35; + auto J_PQ_W = (num_cliques == 1) + ? SapConstraintJacobian(clique0, J32) + : SapConstraintJacobian(clique0, J32, clique1, J34); + return typename SapDistanceConstraint::Kinematics{ + objectA, p_WP, p_AP_W, objectB, p_WQ, p_BQ_W, length, J_PQ_W}; +} + +GTEST_TEST(SapDistanceConstraint, SingleCliqueConstraint) { + const int num_cliques = 1; + const SapDistanceConstraint::ComplianceParameters parameters = + MakeArbitraryParameters(); + const SapDistanceConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapDistanceConstraint c(kinematics, parameters); + + const Vector3d p = kinematics.p_WQ() - kinematics.p_WP(); + const Vector3d p_hat = p.normalized(); + + EXPECT_EQ(c.num_objects(), 2); + EXPECT_EQ(c.num_constraint_equations(), 1); + EXPECT_EQ(c.num_cliques(), 1); + EXPECT_EQ(c.first_clique(), kinematics.jacobian().clique(0)); + EXPECT_THROW(c.second_clique(), std::exception); + EXPECT_EQ(c.first_clique_jacobian().MakeDenseMatrix(), + p_hat.transpose() * J32); + EXPECT_THROW(c.second_clique_jacobian(), std::exception); + EXPECT_EQ(c.length(), kinematics.length()); + EXPECT_EQ(c.compliance_parameters().stiffness(), parameters.stiffness()); + EXPECT_EQ(c.compliance_parameters().damping(), parameters.damping()); +} + +GTEST_TEST(SapDistanceConstraint, TwoCliquesConstraint) { + const int num_cliques = 2; + const SapDistanceConstraint::ComplianceParameters parameters = + MakeArbitraryParameters(); + const SapDistanceConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapDistanceConstraint c(kinematics, parameters); + + const Vector3d p = kinematics.p_WQ() - kinematics.p_WP(); + const Vector3d p_hat = p.normalized(); + + EXPECT_EQ(c.num_objects(), 2); + EXPECT_EQ(c.num_constraint_equations(), 1); + EXPECT_EQ(c.num_cliques(), 2); + EXPECT_EQ(c.first_clique(), kinematics.jacobian().clique(0)); + EXPECT_EQ(c.second_clique(), kinematics.jacobian().clique(1)); + EXPECT_EQ(c.first_clique_jacobian().MakeDenseMatrix(), + p_hat.transpose() * J32); + EXPECT_EQ(c.second_clique_jacobian().MakeDenseMatrix(), + p_hat.transpose() * J34); + EXPECT_EQ(c.length(), kinematics.length()); + EXPECT_EQ(c.compliance_parameters().stiffness(), parameters.stiffness()); + EXPECT_EQ(c.compliance_parameters().damping(), parameters.damping()); +} + +// This method validates analytical gradients implemented by +// SapDistanceConstraint using automatic differentiation. +void ValidateProjection( + const SapDistanceConstraint::ComplianceParameters& p, + const Vector1d& vc) { + SapDistanceConstraint::ComplianceParameters p_ad(p.stiffness(), + p.damping()); + + // Arbitrary kinematic values. + const int num_cliques = 1; + const SapDistanceConstraint::Kinematics kin_ad = + MakeArbitraryKinematics(num_cliques); + + // Instantiate constraint on AutoDiffXd for automatic differentiation. + SapDistanceConstraint c(kin_ad, p_ad); + + // Verify cost gradients using AutoDiffXd. + ValidateConstraintGradients(c, vc); +} + +GTEST_TEST(SapDistanceConstraint, Gradients) { + // An arbitrary set of parameters. + SapDistanceConstraint::ComplianceParameters p(1.0e3, 1.0e2); + + // Arbitrary set of vc values. + { + const Vector1d vc(-1.4); + ValidateProjection(p, vc); + } + { + const Vector1d vc(2.3); + ValidateProjection(p, vc); + } + { + const Vector1d vc(6.2); + ValidateProjection(p, vc); + } +} + +GTEST_TEST(SapDistanceConstraint, SingleCliqueConstraintClone) { + const int num_cliques = 1; + const SapDistanceConstraint::ComplianceParameters parameters = + MakeArbitraryParameters(); + const SapDistanceConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapDistanceConstraint c(kinematics, parameters); + + const Vector3d p = kinematics.p_WQ() - kinematics.p_WP(); + const Vector3d p_hat = p.normalized(); + + // 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(), 2); + EXPECT_EQ(clone->num_constraint_equations(), 1); + EXPECT_EQ(clone->num_cliques(), 1); + EXPECT_EQ(clone->first_clique(), kinematics.jacobian().clique(0)); + EXPECT_THROW(clone->second_clique(), std::exception); + EXPECT_EQ(clone->first_clique_jacobian().MakeDenseMatrix(), + p_hat.transpose() * J32); + EXPECT_THROW(clone->second_clique_jacobian(), std::exception); + EXPECT_EQ(clone->length(), kinematics.length()); + EXPECT_EQ(clone->compliance_parameters().stiffness(), parameters.stiffness()); + EXPECT_EQ(clone->compliance_parameters().damping(), parameters.damping()); +} + +GTEST_TEST(SapDistanceConstraint, TwoCliquesConstraintClone) { + const int num_cliques = 2; + const SapDistanceConstraint::ComplianceParameters parameters = + MakeArbitraryParameters(); + const SapDistanceConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapDistanceConstraint c(kinematics, parameters); + + const Vector3d p = kinematics.p_WQ() - kinematics.p_WP(); + const Vector3d p_hat = p.normalized(); + + auto clone = dynamic_pointer_cast>(c.Clone()); + ASSERT_NE(clone, nullptr); + EXPECT_EQ(clone->num_objects(), 2); + EXPECT_EQ(clone->num_constraint_equations(), 1); + EXPECT_EQ(clone->num_cliques(), 2); + EXPECT_EQ(clone->first_clique(), kinematics.jacobian().clique(0)); + EXPECT_EQ(clone->second_clique(), kinematics.jacobian().clique(1)); + EXPECT_EQ(clone->first_clique_jacobian().MakeDenseMatrix(), + p_hat.transpose() * J32); + EXPECT_EQ(clone->second_clique_jacobian().MakeDenseMatrix(), + p_hat.transpose() * J34); + EXPECT_EQ(clone->length(), kinematics.length()); + EXPECT_EQ(clone->compliance_parameters().stiffness(), parameters.stiffness()); + EXPECT_EQ(clone->compliance_parameters().damping(), parameters.damping()); +} + +GTEST_TEST(SapDistanceConstraint, AccumulateSpatialImpulses) { + // Make a distance constraint with an arbitrary kinematics state and set of + // parameters, irrelevant for this test but needed at construction. + const int num_cliques = 1; + const SapDistanceConstraint::ComplianceParameters parameters = + MakeArbitraryParameters(); + const SapDistanceConstraint::Kinematics kinematics = + MakeArbitraryKinematics(num_cliques); + SapDistanceConstraint c(kinematics, parameters); + + // Vector from P to Q, and its normalized versor. + const Vector3d p_PQ_W = kinematics.p_WQ() - kinematics.p_WP(); + const Vector3d p_hat_W = p_PQ_W.normalized(); + + EXPECT_EQ(c.num_objects(), 2); + EXPECT_EQ(c.object(0), kinematics.objectA()); + EXPECT_EQ(c.object(1), kinematics.objectB()); + + // Arbitrary value of the impulse. + const Vector1d gamma(1.2345); + + // Expected spatial impulse on B. + const Vector3d f_B_W = gamma(0) * p_hat_W; + const Vector3d t_Bo_W = kinematics.p_BQ_W().cross(f_B_W); + const SpatialForce F_Bo_W(t_Bo_W, f_B_W); + + // Expected spatial impulse on A. + const Vector3d f_A_W = -f_B_W; + const Vector3d t_Ao_W = kinematics.p_AP_W().cross(f_A_W); + const SpatialForce F_Ao_W(t_Ao_W, f_A_W); + + const SpatialForce F0(Vector3d(1., 2., 3), Vector3d(4., 5., 6)); + SpatialForce Faccumulated = F0; // Initialize to non-zero value. + SpatialForce F_Bo_W_expected = F0 + F_Bo_W; + c.AccumulateSpatialImpulses(1, gamma, &Faccumulated); + EXPECT_TRUE(CompareMatrices( + Faccumulated.get_coeffs(), F_Bo_W_expected.get_coeffs(), + std::numeric_limits::epsilon(), MatrixCompareType::relative)); + + Faccumulated = F0; // Initialize to non-zero value. + SpatialForce F_Ao_W_expected = F0 + F_Ao_W; + c.AccumulateSpatialImpulses(0, gamma, &Faccumulated); + EXPECT_TRUE(CompareMatrices( + Faccumulated.get_coeffs(), F_Ao_W_expected.get_coeffs(), + 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 67290aaf6197..f711f1105591 100644 --- a/multibody/plant/sap_driver.cc +++ b/multibody/plant/sap_driver.cc @@ -12,6 +12,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_distance_constraint.h" #include "drake/multibody/contact_solvers/sap/sap_friction_cone_constraint.h" #include "drake/multibody/contact_solvers/sap/sap_holonomic_constraint.h" #include "drake/multibody/contact_solvers/sap/sap_limit_constraint.h" @@ -31,6 +32,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::SapDistanceConstraint; using drake::multibody::contact_solvers::internal::SapFrictionConeConstraint; using drake::multibody::contact_solvers::internal::SapHolonomicConstraint; using drake::multibody::contact_solvers::internal::SapLimitConstraint; @@ -400,17 +402,10 @@ void SapDriver::AddDistanceConstraints(const systems::Context& context, SapContactProblem* problem) const { DRAKE_DEMAND(problem != nullptr); - // Distance constraints do not have impulse limits, they are bi-lateral - // constraints. Each distance constraint introduces a single constraint - // equation. - constexpr double kInfinity = std::numeric_limits::infinity(); - const Vector1 gamma_lower(-kInfinity); - const Vector1 gamma_upper(kInfinity); - const int nv = plant().num_velocities(); Matrix3X Jv_WAp_W(3, nv); Matrix3X Jv_WBq_W(3, nv); - MatrixX Jdistance = MatrixX::Zero(1, nv); + Matrix3X Jv_ApBq_W(3, nv); const Frame& frame_W = plant().world_frame(); @@ -418,87 +413,70 @@ void SapDriver::AddDistanceConstraints(const systems::Context& context, unused(id); const Body& body_A = plant().get_body(spec.body_A); const Body& body_B = plant().get_body(spec.body_B); + DRAKE_DEMAND(body_A.index() != body_B.index()); const math::RigidTransform& X_WA = plant().EvalBodyPoseInWorld(context, body_A); const math::RigidTransform& X_WB = plant().EvalBodyPoseInWorld(context, body_B); - const Vector3& p_WP = X_WA * spec.p_AP.template cast(); - const Vector3& p_WQ = X_WB * spec.p_BQ.template cast(); - - // Distance as the norm of p_PQ_W = p_WQ - p_WP - const Vector3 p_PQ_W = p_WQ - p_WP; - const T d0 = p_PQ_W.norm(); - // Verify the distance did not become ridiculously small. We use the - // user-specified distance as a reference. - constexpr double kMinimumDistance = 1.0e-7; - constexpr double kRelativeDistance = 1.0e-2; - if (d0 < kMinimumDistance + kRelativeDistance * spec.distance) { - const std::string msg = fmt::format( - "The distance between bodies '{}' and '{}' is: {}. This is " - "nonphysically small when compared to the free length of the " - "constraint, {}. ", - body_A.name(), body_B.name(), d0, spec.distance); - throw std::logic_error(msg); - } - const Vector3 p_hat_W = p_PQ_W / d0; - - DRAKE_DEMAND(body_A.index() != body_B.index()); + const Vector3 p_WP = X_WA * spec.p_AP.template cast(); + const Vector3 p_AP_W = X_WA.rotation() * spec.p_AP.template cast(); + const Vector3 p_WQ = X_WB * spec.p_BQ.template cast(); + const Vector3 p_BQ_W = X_WB.rotation() * spec.p_BQ.template cast(); - // Dense Jacobian. - // d(distance)/dt = Jdistance * v. + // Jacobian for the velocity of point Q (on body B) relative to point P (on + // body B). manager().internal_tree().CalcJacobianTranslationalVelocity( context, JacobianWrtVariable::kV, body_A.body_frame(), frame_W, p_WP, frame_W, frame_W, &Jv_WAp_W); manager().internal_tree().CalcJacobianTranslationalVelocity( context, JacobianWrtVariable::kV, body_B.body_frame(), frame_W, p_WQ, frame_W, frame_W, &Jv_WBq_W); - Jdistance = p_hat_W.transpose() * (Jv_WBq_W - Jv_WAp_W); - - const T dissipation_time_scale = spec.damping / spec.stiffness; - - // TODO(amcastro-tri): consider exposing this parameter. - const double beta = 0.1; - const typename SapHolonomicConstraint::Parameters parameters{ - gamma_lower, gamma_upper, Vector1(spec.stiffness), - Vector1(dissipation_time_scale), beta}; - - const TreeIndex treeA_index = - tree_topology().body_to_tree_index(spec.body_A); - const TreeIndex treeB_index = - tree_topology().body_to_tree_index(spec.body_B); + Jv_ApBq_W = Jv_WBq_W - Jv_WAp_W; + + // Jacobian for the relative velocity v_PQ_W, as required by + // SapDistanceConstraint. + auto make_constraint_jacobian = [this, &Jv_ApBq_W](BodyIndex bodyA, + BodyIndex bodyB) { + const TreeIndex treeA_index = tree_topology().body_to_tree_index(bodyA); + const TreeIndex treeB_index = tree_topology().body_to_tree_index(bodyB); + // Sanity check at least one body is not the world. + DRAKE_DEMAND(treeA_index.is_valid() || treeB_index.is_valid()); + + // Both bodies A and B belong to the same tree or one of them is the + // world. + const bool single_tree = !treeA_index.is_valid() || + !treeB_index.is_valid() || + treeA_index == treeB_index; + + if (single_tree) { + const TreeIndex tree_index = + treeA_index.is_valid() ? treeA_index : treeB_index; + MatrixX Jtree = Jv_ApBq_W.middleCols( + tree_topology().tree_velocities_start(tree_index), + tree_topology().num_tree_velocities(tree_index)); + return SapConstraintJacobian(tree_index, std::move(Jtree)); + } else { + MatrixX JA = Jv_ApBq_W.middleCols( + tree_topology().tree_velocities_start(treeA_index), + tree_topology().num_tree_velocities(treeA_index)); + MatrixX JB = Jv_ApBq_W.middleCols( + tree_topology().tree_velocities_start(treeB_index), + tree_topology().num_tree_velocities(treeB_index)); + return SapConstraintJacobian(treeA_index, std::move(JA), treeB_index, + std::move(JB)); + } + }; - // Sanity check at least one body is not the world. - DRAKE_DEMAND(treeA_index.is_valid() || treeB_index.is_valid()); + const typename SapDistanceConstraint::Kinematics kinematics( + spec.body_A, p_WP, p_AP_W, spec.body_B, p_WQ, p_BQ_W, spec.distance, + make_constraint_jacobian(spec.body_A, spec.body_B)); - // Both bodies A and B belong to the same tree or one of them is the world. - const bool single_tree = !treeA_index.is_valid() || - !treeB_index.is_valid() || - treeA_index == treeB_index; + const typename SapDistanceConstraint::ComplianceParameters parameters( + spec.stiffness, spec.damping); - // Constraint function at current time step. - const Vector1 g0(d0 - spec.distance); - if (single_tree) { - const TreeIndex tree_index = - treeA_index.is_valid() ? treeA_index : treeB_index; - MatrixX Jtree = Jdistance.middleCols( - tree_topology().tree_velocities_start(tree_index), - tree_topology().num_tree_velocities(tree_index)); - SapConstraintJacobian J(tree_index, std::move(Jtree)); - problem->AddConstraint(std::make_unique>( - g0, std::move(J), parameters)); - } else { - MatrixX JA = Jdistance.middleCols( - tree_topology().tree_velocities_start(treeA_index), - tree_topology().num_tree_velocities(treeA_index)); - MatrixX JB = Jdistance.middleCols( - tree_topology().tree_velocities_start(treeB_index), - tree_topology().num_tree_velocities(treeB_index)); - SapConstraintJacobian J(treeA_index, std::move(JA), treeB_index, - std::move(JB)); - problem->AddConstraint(std::make_unique>( - g0, std::move(J), parameters)); - } + problem->AddConstraint(std::make_unique>( + std::move(kinematics), std::move(parameters))); } }