From 3b26e7aa362c7b27aad0cc30d093e8bab2e2681c Mon Sep 17 00:00:00 2001 From: Joseph Masterjohn Date: Wed, 22 Jul 2026 11:15:36 -0400 Subject: [PATCH 1/2] [CENIC] Support distance constraints Adds support for distance constraints in IcfSolver and thus CENIC. Co-Authored-By: Claude Opus 4.8 (1M context) --- .../multibody_contact_solvers_icf.h | 1 + multibody/contact_solvers/icf/BUILD.bazel | 27 + .../icf/distance_constraints_pool.cc | 127 ++++ .../icf/distance_constraints_pool.h | 107 +++ multibody/contact_solvers/icf/eigen_pool.cc | 6 + multibody/contact_solvers/icf/eigen_pool.h | 1 + .../icf/holonomic_constraints_data_pool.cc | 2 + .../icf/holonomic_constraints_data_pool.h | 5 +- .../icf/holonomic_constraints_pool.cc | 5 + multibody/contact_solvers/icf/icf_builder.cc | 106 ++- multibody/contact_solvers/icf/icf_builder.h | 8 + multibody/contact_solvers/icf/icf_data.cc | 13 +- multibody/contact_solvers/icf/icf_data.h | 17 +- multibody/contact_solvers/icf/icf_model.cc | 40 +- multibody/contact_solvers/icf/icf_model.h | 21 +- .../distance_constraint_init_and_sim_test.cc | 155 ++++ .../test/distance_constraints_pool_test.cc | 683 ++++++++++++++++++ .../icf/test/icf_builder_test.cc | 42 +- .../contact_solvers/icf/test/icf_data_test.cc | 17 +- .../test_utilities/icf_model_test_helpers.cc | 39 +- .../test_utilities/icf_model_test_helpers.h | 7 + 21 files changed, 1395 insertions(+), 34 deletions(-) create mode 100644 multibody/contact_solvers/icf/distance_constraints_pool.cc create mode 100644 multibody/contact_solvers/icf/distance_constraints_pool.h create mode 100644 multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc create mode 100644 multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc diff --git a/bindings/generated_docstrings/multibody_contact_solvers_icf.h b/bindings/generated_docstrings/multibody_contact_solvers_icf.h index cfdf19937493..d176a5e64d3e 100644 --- a/bindings/generated_docstrings/multibody_contact_solvers_icf.h +++ b/bindings/generated_docstrings/multibody_contact_solvers_icf.h @@ -16,6 +16,7 @@ // #include "drake/multibody/contact_solvers/icf/ball_constraints_pool.h" // #include "drake/multibody/contact_solvers/icf/coupler_constraints_data_pool.h" // #include "drake/multibody/contact_solvers/icf/coupler_constraints_pool.h" +// #include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h" // #include "drake/multibody/contact_solvers/icf/eigen_pool.h" // #include "drake/multibody/contact_solvers/icf/gain_constraints_data_pool.h" // #include "drake/multibody/contact_solvers/icf/gain_constraints_pool.h" diff --git a/multibody/contact_solvers/icf/BUILD.bazel b/multibody/contact_solvers/icf/BUILD.bazel index 73cbb6374818..807011cb1f08 100644 --- a/multibody/contact_solvers/icf/BUILD.bazel +++ b/multibody/contact_solvers/icf/BUILD.bazel @@ -183,6 +183,7 @@ drake_cc_library( srcs = [ "ball_constraints_pool.cc", "coupler_constraints_pool.cc", + "distance_constraints_pool.cc", "gain_constraints_pool.cc", "holonomic_constraints_pool.cc", "icf_model.cc", @@ -194,6 +195,7 @@ drake_cc_library( "abstract_constraints_pool.h", "ball_constraints_pool.h", "coupler_constraints_pool.h", + "distance_constraints_pool.h", "gain_constraints_pool.h", "holonomic_constraints_pool.h", "icf_model.h", @@ -266,6 +268,31 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "distance_constraint_init_and_sim_test", + deps = [ + "//multibody/cenic:cenic_integrator", + "//multibody/parsing", + "//multibody/plant", + "//multibody/plant:multibody_plant_config_functions", + "//systems/analysis:simulator", + "//systems/framework:diagram_builder", + ], +) + +drake_cc_googletest( + name = "distance_constraints_pool_test", + deps = [ + ":icf_data", + ":icf_model", + ":icf_search_direction_data", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:limit_malloc", + "//math:gradient", + "//multibody/contact_solvers/icf/test_utilities:icf_model_test_helpers", + ], +) + drake_cc_googletest( name = "eigen_pool_test", deps = [ diff --git a/multibody/contact_solvers/icf/distance_constraints_pool.cc b/multibody/contact_solvers/icf/distance_constraints_pool.cc new file mode 100644 index 000000000000..c00132a4a870 --- /dev/null +++ b/multibody/contact_solvers/icf/distance_constraints_pool.cc @@ -0,0 +1,127 @@ +#include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h" + +#include "drake/math/cross_product.h" +#include "drake/multibody/contact_solvers/icf/icf_model.h" + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace icf { +namespace internal { + +using math::VectorToSkewSymmetric; + +template +void DistanceConstraintsPool::Set(int index, int bodyA, int bodyB, + const Vector3& p_AP_W, + const Vector3& p_BQ_W, + const Vector3& p_hat_W, const T& g0, + const T& stiffness, const T& damping) { + p_AP_W_[index] = p_AP_W; + p_BQ_W_[index] = p_BQ_W; + p_hat_W_[index] = p_hat_W; + // Constraint function g₀ = d₀ − ℓ ∈ ℝ. + this->SetCommon(index, bodyA, bodyB, Vector1(g0), stiffness, damping); +} + +template +Vector1 DistanceConstraintsPool::CalcConstraintVelocity( + int k, const Vector6& V_WB, const Vector6* V_WA) const { + // vc = ḋ = p̂ᵀ⋅(v_W_Bq − v_W_Ap), the rate of change of distance. + const Vector3& p_hat_W = p_hat_W_[k]; + const Vector3& w_WB = V_WB.template head<3>(); + const Vector3& v_WBo = V_WB.template tail<3>(); + const Vector3 v_W_Bq = v_WBo + w_WB.cross(p_BQ_W_[k]); + + T vc = p_hat_W.dot(v_W_Bq); + if (V_WA != nullptr) { + const Vector3& w_WA = V_WA->template head<3>(); + const Vector3& v_WAo = V_WA->template tail<3>(); + const Vector3 v_W_Ap = v_WAo + w_WA.cross(p_AP_W_[k]); + vc -= p_hat_W.dot(v_W_Ap); + } + return Vector1(vc); +} + +template +void DistanceConstraintsPool::CalcSpatialImpulses( + int k, const Vector1& gamma, Vector6* Gamma_Bo, + Vector6* Gamma_Ao) const { + // The scalar impulse γ acts as γ⋅p̂ along the line PQ, applied + // at Q on B and −γ⋅p̂ at P on A. Shift each to the body origin. + const Vector3& p_hat_W = p_hat_W_[k]; + const Vector3 f_B = gamma(0) * p_hat_W; + const Vector6 spatial_gamma_Bq = + (Vector6() << Vector3::Zero(), f_B).finished(); + *Gamma_Bo = ShiftSpatialImpulse(spatial_gamma_Bq, p_BQ_W_[k]); + if (Gamma_Ao != nullptr) { + const Vector6 minus_spatial_gamma_Ap = + (Vector6() << Vector3::Zero(), Vector3(-f_B)).finished(); + *Gamma_Ao = ShiftSpatialImpulse(minus_spatial_gamma_Ap, p_AP_W_[k]); + } +} + +template +void DistanceConstraintsPool::CalcHessianBlocks(int k, const T& R_inv, + Matrix6* G_Bp, + Matrix6* G_Ap, + Matrix6* G_cross) const { + // G = diag(0, Gt) with Gt = R⁻¹⋅p̂⋅p̂ᵀ. + const Vector3& p_hat_W = p_hat_W_[k]; + const Matrix3 Gt = R_inv * (p_hat_W * p_hat_W.transpose()); + const Matrix3 px_B = VectorToSkewSymmetric(p_BQ_W_[k]); + // Compute G_Bp = Φ(p_BoBm)ᵀ⋅G⋅Φ(p_BoBm) where Φ(p) = [𝕀₃, 0; -pₓ, 𝕀₃] and + // G = diag(0, Gt). + G_Bp->template topLeftCorner<3, 3>() = -px_B * Gt * px_B; + G_Bp->template topRightCorner<3, 3>() = px_B * Gt; + G_Bp->template bottomLeftCorner<3, 3>() = -Gt * px_B; + G_Bp->template bottomRightCorner<3, 3>() = Gt; + + if (G_Ap != nullptr) { + const Matrix3 px_A = VectorToSkewSymmetric(p_AP_W_[k]); + // G_Ap = Φ(p_AoAm)ᵀ⋅G⋅Φ(p_AoAm). + G_Ap->template topLeftCorner<3, 3>() = -px_A * Gt * px_A; + G_Ap->template topRightCorner<3, 3>() = px_A * Gt; + G_Ap->template bottomLeftCorner<3, 3>() = -Gt * px_A; + G_Ap->template bottomRightCorner<3, 3>() = Gt; + + // G_cross = −Φ(p_BQ)ᵀ⋅G⋅Φ(p_AP). + G_cross->template topLeftCorner<3, 3>() = px_B * Gt * px_A; + G_cross->template topRightCorner<3, 3>() = -px_B * Gt; + G_cross->template bottomLeftCorner<3, 3>() = Gt * px_A; + G_cross->template bottomRightCorner<3, 3>() = -Gt; + } +} + +template +void DistanceConstraintsPool::ResizeGeometry(int num_constraints) { + p_AP_W_.Resize(num_constraints, 3, 1); + p_BQ_W_.Resize(num_constraints, 3, 1); + p_hat_W_.Resize(num_constraints, 3, 1); +} + +template +void DistanceConstraintsPool::ReduceGeometryInto( + DistanceConstraintsPool* reduced, int k, bool flip) const { + // g₀ = d − ℓ is symmetric under swapping P and Q, but the unit direction p̂ + // (from P to Q) negates and the anchor points swap. + if (flip) { + reduced->p_AP_W_.Add(3, 1) = p_BQ_W_[k]; + reduced->p_BQ_W_.Add(3, 1) = p_AP_W_[k]; + reduced->p_hat_W_.Add(3, 1) = -p_hat_W_[k]; + } else { + reduced->p_AP_W_.Add(3, 1) = p_AP_W_[k]; + reduced->p_BQ_W_.Add(3, 1) = p_BQ_W_[k]; + reduced->p_hat_W_.Add(3, 1) = p_hat_W_[k]; + } +} + +} // namespace internal +} // namespace icf +} // namespace contact_solvers +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::contact_solvers::icf::internal:: + DistanceConstraintsPool); diff --git a/multibody/contact_solvers/icf/distance_constraints_pool.h b/multibody/contact_solvers/icf/distance_constraints_pool.h new file mode 100644 index 000000000000..955614f0260a --- /dev/null +++ b/multibody/contact_solvers/icf/distance_constraints_pool.h @@ -0,0 +1,107 @@ +#pragma once + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/multibody/contact_solvers/icf/abstract_constraints_pool.h" +#include "drake/multibody/contact_solvers/icf/eigen_pool.h" +#include "drake/multibody/contact_solvers/icf/holonomic_constraints_data_pool.h" +#include "drake/multibody/contact_solvers/icf/holonomic_constraints_pool.h" +#include "drake/multibody/contact_solvers/icf/icf_data.h" + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace icf { +namespace internal { + +// Forward declaration to break circular dependencies. +template +class IcfModel; + +/* A pool of distance constraints between pairs of bodies. + +Each distance constraint connects distinct bodies A and B, constraining the +Euclidean distance d between a point P on A and a point Q on B to a free length +ℓ. This is a single (scalar) holonomic constraint equation: + g = d − ℓ = 0 ∈ ℝ. +Adapted from the SAP distance constraint (see sap_distance_constraint.h), +this is a compliant (spring-damper) constraint: with p̂ the unit vector from P +to Q, stiffness k and damping c, the scalar impulse is + γ = −k⋅(d − ℓ) − c⋅ḋ ∈ ℝ +applied as γ⋅p̂ on B at Q and −γ⋅p̂ on A at P. + +Unlike other holonomic constraints (e.g. weld, ball), the distance constraint +exposes stiffness and damping parameters to the user, allowing the modeling of +a linear spring-damper between two points. If the stiffness is set to +∞, the +constraint uses the near-rigid approximation to approximate a rigid distance +constraint. + +@tparam_nonsymbolic_scalar */ +template +class DistanceConstraintsPool + : public HolonomicConstraintsPool> { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DistanceConstraintsPool); + + explicit DistanceConstraintsPool(const IcfModel* parent_model) + : HolonomicConstraintsPool>( + parent_model) {} + + /* The scalar constraint function g₀ = d − ℓ is symmetric under swapping the + points P and Q, so (unlike the weld/ball) it does not negate on an A/B flip + during model reduction. See HolonomicConstraintsPool::kFlipNegatesG0(). */ + static constexpr bool FlipNegatesG0() { return false; } + + /* Sets the k-th distance constraint. + @param index The index of the constraint within the pool. + @param bodyA The index of body A. May be anchored. + @param bodyB The index of body B. Must not be anchored. + @param p_AP_W Position of constraint point P in body A, expressed in world. + @param p_BQ_W Position of constraint point Q in body B, expressed in world. + @param p_hat_W Unit vector from P to Q, expressed in world. + @param g0 The constraint function at q₀, g₀ = d₀ − ℓ. + @param stiffness The constraint stiffness k in N/m (may be +∞ for near-rigid). + @param damping The constraint damping c in N⋅s/m. + @pre indices in range, bodyA ≠ bodyB, bodyB not anchored, stiffness > 0, + damping ≥ 0. */ + void Set(int index, int bodyA, int bodyB, const Vector3& p_AP_W, + const Vector3& p_BQ_W, const Vector3& p_hat_W, const T& g0, + const T& stiffness, const T& damping); + + /* Hooks required by HolonomicConstraintsPool (CRTP). */ + Vector1 CalcConstraintVelocity(int k, const Vector6& V_WB, + const Vector6* V_WA) const; + void CalcSpatialImpulses(int k, const Vector1& gamma, Vector6* Gamma_Bo, + Vector6* Gamma_Ao) const; + void CalcHessianBlocks(int k, const T& R_inv, Matrix6* G_Bp, + Matrix6* G_Ap, Matrix6* G_cross) const; + const DistanceConstraintsDataPool& GetDataPool( + const IcfData& data) const { + return data.distance_constraints_data(); + } + void ResizeGeometry(int num_constraints); + void ReduceGeometryInto(DistanceConstraintsPool* reduced, int k, + bool flip) const; + + /* Testing-only access. */ + const EigenPool>& p_AP_W() const { return p_AP_W_; } + const EigenPool>& p_BQ_W() const { return p_BQ_W_; } + const EigenPool>& p_hat_W() const { return p_hat_W_; } + + private: + EigenPool> p_AP_W_; // Position of P in A, expressed in W. + EigenPool> p_BQ_W_; // Position of Q in B, expressed in W. + EigenPool> p_hat_W_; // Unit vector P→Q, expressed in W. +}; +static_assert(IsAbstractConstraintsPool); +static_assert(IsHolonomicConstraintsDerived); + +} // namespace internal +} // namespace icf +} // namespace contact_solvers +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::contact_solvers::icf::internal:: + DistanceConstraintsPool); diff --git a/multibody/contact_solvers/icf/eigen_pool.cc b/multibody/contact_solvers/icf/eigen_pool.cc index d72f66dda8c0..dbc7a2dbd2ec 100644 --- a/multibody/contact_solvers/icf/eigen_pool.cc +++ b/multibody/contact_solvers/icf/eigen_pool.cc @@ -168,6 +168,12 @@ template class EigenPoolDynamicSizeStorage>; template class EigenPool>; template class EigenPoolDynamicSizeStorage>; +// Vector1 +template class EigenPool>; +template class EigenPoolFixedSizeStorage>; +template class EigenPool>; +template class EigenPoolFixedSizeStorage>; + // Vector3 template class EigenPool>; template class EigenPoolFixedSizeStorage>; diff --git a/multibody/contact_solvers/icf/eigen_pool.h b/multibody/contact_solvers/icf/eigen_pool.h index b07db315d9e2..1800d77653c3 100644 --- a/multibody/contact_solvers/icf/eigen_pool.h +++ b/multibody/contact_solvers/icf/eigen_pool.h @@ -118,6 +118,7 @@ scalars and/or sharing the allocation across multiple pools. - Matrix6 - Matrix6X - MatrixX +- Vector1 - Vector3 - Vector6 - VectorX diff --git a/multibody/contact_solvers/icf/holonomic_constraints_data_pool.cc b/multibody/contact_solvers/icf/holonomic_constraints_data_pool.cc index c0fa880893c2..3c6370fecff5 100644 --- a/multibody/contact_solvers/icf/holonomic_constraints_data_pool.cc +++ b/multibody/contact_solvers/icf/holonomic_constraints_data_pool.cc @@ -21,6 +21,8 @@ template class HolonomicConstraintsDataPool; template class HolonomicConstraintsDataPool; template class HolonomicConstraintsDataPool; template class HolonomicConstraintsDataPool; +template class HolonomicConstraintsDataPool; +template class HolonomicConstraintsDataPool; } // namespace internal } // namespace icf diff --git a/multibody/contact_solvers/icf/holonomic_constraints_data_pool.h b/multibody/contact_solvers/icf/holonomic_constraints_data_pool.h index cc2bbaa879a3..8b0148195d11 100644 --- a/multibody/contact_solvers/icf/holonomic_constraints_data_pool.h +++ b/multibody/contact_solvers/icf/holonomic_constraints_data_pool.h @@ -50,11 +50,14 @@ class HolonomicConstraintsDataPool { }; /* Named data pools for the concrete holonomic constraints. Each is just the -generic pool at the constraint's equation count (weld: 6, ball: 3). */ +generic pool at the constraint's equation count (weld: 6, ball: 3, +distance: 1). */ template using WeldConstraintsDataPool = HolonomicConstraintsDataPool; template using BallConstraintsDataPool = HolonomicConstraintsDataPool; +template +using DistanceConstraintsDataPool = HolonomicConstraintsDataPool; } // namespace internal } // namespace icf diff --git a/multibody/contact_solvers/icf/holonomic_constraints_pool.cc b/multibody/contact_solvers/icf/holonomic_constraints_pool.cc index 3921f2eb0b04..c2b7e3f2e1b5 100644 --- a/multibody/contact_solvers/icf/holonomic_constraints_pool.cc +++ b/multibody/contact_solvers/icf/holonomic_constraints_pool.cc @@ -7,6 +7,7 @@ #include "drake/common/autodiff.h" #include "drake/common/extract_double.h" #include "drake/multibody/contact_solvers/icf/ball_constraints_pool.h" +#include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h" #include "drake/multibody/contact_solvers/icf/icf_model.h" #include "drake/multibody/contact_solvers/icf/weld_constraints_pool.h" @@ -356,6 +357,10 @@ template class HolonomicConstraintsPool>; template class HolonomicConstraintsPool>; +template class HolonomicConstraintsPool>; +template class HolonomicConstraintsPool>; } // namespace internal } // namespace icf diff --git a/multibody/contact_solvers/icf/icf_builder.cc b/multibody/contact_solvers/icf/icf_builder.cc index 5b3c97e86b86..dd62322d0b5e 100644 --- a/multibody/contact_solvers/icf/icf_builder.cc +++ b/multibody/contact_solvers/icf/icf_builder.cc @@ -7,6 +7,7 @@ #include #include +#include "drake/common/extract_double.h" #include "drake/geometry/scene_graph_config.h" #include "drake/geometry/scene_graph_inspector.h" #include "drake/math/rotation_matrix.h" @@ -15,6 +16,7 @@ #include "drake/multibody/topology/graph.h" using drake::multibody::CalcContactFrictionFromSurfaceProperties; +using drake::multibody::DistanceConstraintParams; using drake::multibody::internal::BallConstraintSpec; using drake::multibody::internal::CouplerConstraintSpec; using drake::multibody::internal::GetCombinedHuntCrossleyDissipation; @@ -241,6 +243,10 @@ void IcfBuilder::UpdateModel( AllocateBallConstraints(model); SetBallConstraints(context, model); + // Distance constraints + AllocateDistanceConstraints(model); + SetDistanceConstraints(context, model); + // Limit constraints AllocateLimitConstraints(model); SetLimitConstraints(context, model); @@ -306,13 +312,14 @@ void IcfBuilder::ValidatePlant() { // Revisit this condition as constraints are implemented. See issues #23759, // #23760, #23762, #23763. if (plant_.num_constraints() - plant_.num_ball_constraints() - - plant_.num_coupler_constraints() - plant_.num_weld_constraints() > + plant_.num_coupler_constraints() - plant_.num_distance_constraints() - + plant_.num_weld_constraints() > 0) { throw std::logic_error(fmt::format( "The CENIC integrator does not yet support some constraints, but " - "they are present in the given MultibodyPlant: {} distance " - "constraint(s), {} tendon constraint(s)", - plant_.num_distance_constraints(), plant_.num_tendon_constraints())); + "they are present in the given MultibodyPlant: {} tendon " + "constraint(s)", + plant_.num_tendon_constraints())); } } @@ -585,6 +592,97 @@ void IcfBuilder::SetBallConstraints(const systems::Context& context, } } +template +void IcfBuilder::AllocateDistanceConstraints(IcfModel* model) const { + DRAKE_ASSERT(model != nullptr); + DistanceConstraintsPool& distance_constraints = + model->distance_constraints_pool(); + distance_constraints.Resize(plant_.num_distance_constraints()); +} + +template +void IcfBuilder::SetDistanceConstraints(const systems::Context& context, + IcfModel* model) const { + DRAKE_ASSERT(model != nullptr); + using drake::math::RigidTransform; + + // Distance constraint parameters are context-dependent (runtime-mutable), + // unlike ball/weld specs; read the current values from the context. + const std::map& params_map = + plant_.GetDistanceConstraintParams(context); + + DistanceConstraintsPool& distance_constraints = + model->distance_constraints_pool(); + + int index = 0; + for (const auto& [id, params] : params_map) { + const RigidBody& body_A = plant_.get_body(params.bodyA()); + const RigidBody& body_B = plant_.get_body(params.bodyB()); + + // By convention in the ICF distance constraint pool, body B must not be + // anchored. If body A is anchored, that's fine. + const bool A_anchored = plant_.IsAnchored(body_A); + const bool B_anchored = plant_.IsAnchored(body_B); + + // TODO(sherm1): Move this exception up to the plant level so + // that it fails as fast as possible. Currently, the earliest this can + // happen is in MbP::Finalize() after the topology has been finalized. + if (A_anchored && B_anchored) { + const std::string msg = fmt::format( + "Creating a distance constraint between bodies '{}' and '{}' where " + "both are welded to the world is not allowed.", + body_A.name(), body_B.name()); + throw std::logic_error(msg); + } + + // If B is anchored but A is not, swap roles so that the "B" body in the + // pool is always the dynamic one. + const RigidBody& pool_bodyA = B_anchored ? body_B : body_A; + const RigidBody& pool_bodyB = B_anchored ? body_A : body_B; + const Vector3& p_AP_spec = + B_anchored ? params.p_BQ() : params.p_AP(); + const Vector3& p_BQ_spec = + B_anchored ? params.p_AP() : params.p_BQ(); + + const RigidTransform& X_WA = pool_bodyA.EvalPoseInWorld(context); + const RigidTransform& X_WB = pool_bodyB.EvalPoseInWorld(context); + + // Constraint point positions in world. + const Vector3 p_WP = X_WA * p_AP_spec.template cast(); + const Vector3 p_WQ = X_WB * p_BQ_spec.template cast(); + + // Positions of P in A and Q in B, expressed in world. + const Vector3 p_AP_W = X_WA.rotation() * p_AP_spec.template cast(); + const Vector3 p_BQ_W = X_WB.rotation() * p_BQ_spec.template cast(); + + // Current distance d₀ and unit direction p̂ from P to Q. Guard against a + // nonphysically small distance (the constraint is singular there; use a + // ball constraint for coincident points), mirroring SapDistanceConstraint. + const Vector3 p_PQ_W = p_WQ - p_WP; + const T d0 = p_PQ_W.norm(); + const double length = params.distance(); // The free length ℓ. + constexpr double kMinimumDistance = 1.0e-7; + constexpr double kRelativeDistance = 1.0e-2; + if (ExtractDoubleOrThrow(d0) < + kMinimumDistance + kRelativeDistance * length) { + throw std::logic_error(fmt::format( + "The distance between the two points of a distance constraint " + "between bodies '{}' and '{}' is {}, which is nonphysically small " + "compared to the constraint's free length, {}.", + body_A.name(), body_B.name(), ExtractDoubleOrThrow(d0), length)); + } + const Vector3 p_hat_W = p_PQ_W / d0; + + // Constraint function g₀ = d₀ − ℓ. + const T g0 = d0 - length; + + distance_constraints.Set(index, pool_bodyA.index(), pool_bodyB.index(), + p_AP_W, p_BQ_W, p_hat_W, g0, T(params.stiffness()), + T(params.damping())); + ++index; + } +} + template void IcfBuilder::AllocateLimitConstraints(IcfModel* model) const { DRAKE_ASSERT(model != nullptr); diff --git a/multibody/contact_solvers/icf/icf_builder.h b/multibody/contact_solvers/icf/icf_builder.h index fb42b3891e05..e548009259a0 100644 --- a/multibody/contact_solvers/icf/icf_builder.h +++ b/multibody/contact_solvers/icf/icf_builder.h @@ -131,6 +131,14 @@ class IcfBuilder { void SetBallConstraints(const systems::Context& context, IcfModel* model) const; + /* Resizes the model to accommodate distance constraints. */ + void AllocateDistanceConstraints(IcfModel* model) const; + + /* Sets distance constraints in the model. + @pre AllocateDistanceConstraints() has already been called. */ + void SetDistanceConstraints(const systems::Context& context, + IcfModel* model) const; + /* Resizes the model to accommodate limit constraints. */ void AllocateLimitConstraints(IcfModel* model) const; diff --git a/multibody/contact_solvers/icf/icf_data.cc b/multibody/contact_solvers/icf/icf_data.cc index 7f93b002837d..9b0d93a698d9 100644 --- a/multibody/contact_solvers/icf/icf_data.cc +++ b/multibody/contact_solvers/icf/icf_data.cc @@ -11,8 +11,8 @@ namespace internal { template void IcfData::Scratch::Resize(int num_bodies, int num_velocities, int max_clique_size, int num_ball_constraints, - int num_couplers, int num_welds, - std::span gain_sizes, + int num_couplers, int num_distance_constraints, + int num_welds, std::span gain_sizes, std::span limit_sizes, std::span patch_sizes) { Av_minus_r.Resize(1, num_velocities, 1); @@ -26,6 +26,7 @@ void IcfData::Scratch::Resize(int num_bodies, int num_velocities, ball_constraints_data.Resize(num_ball_constraints); coupler_constraints_data.Resize(num_couplers); + distance_constraints_data.Resize(num_distance_constraints); gain_constraints_data.Resize(gain_sizes); limit_constraints_data.Resize(limit_sizes); patch_constraints_data.Resize(patch_sizes); @@ -47,7 +48,8 @@ IcfData::~IcfData() = default; template void IcfData::Resize(int num_bodies, int num_velocities, int max_clique_size, int num_ball_constraints, int num_couplers, - int num_welds, std::span gain_sizes, + int num_distance_constraints, int num_welds, + std::span gain_sizes, std::span limit_sizes, std::span patch_sizes) { v_.resize(num_velocities); @@ -56,13 +58,14 @@ void IcfData::Resize(int num_bodies, int num_velocities, int max_clique_size, gradient_.resize(num_velocities); ball_constraints_data_.Resize(num_ball_constraints); coupler_constraints_data_.Resize(num_couplers); + distance_constraints_data_.Resize(num_distance_constraints); gain_constraints_data_.Resize(gain_sizes); limit_constraints_data_.Resize(limit_sizes); patch_constraints_data_.Resize(patch_sizes); weld_constraints_data_.Resize(num_welds); scratch_.Resize(num_bodies, num_velocities, max_clique_size, - num_ball_constraints, num_couplers, num_welds, gain_sizes, - limit_sizes, patch_sizes); + num_ball_constraints, num_couplers, num_distance_constraints, + num_welds, gain_sizes, limit_sizes, patch_sizes); } template diff --git a/multibody/contact_solvers/icf/icf_data.h b/multibody/contact_solvers/icf/icf_data.h index 01f126d5c29a..517f39d5e2c3 100644 --- a/multibody/contact_solvers/icf/icf_data.h +++ b/multibody/contact_solvers/icf/icf_data.h @@ -48,7 +48,8 @@ class IcfData { struct Scratch { /* Resizes the scratch space, allocating memory as needed. */ void Resize(int num_bodies, int num_velocities, int max_clique_size, - int num_ball_constraints, int num_couplers, int num_welds, + int num_ball_constraints, int num_couplers, + int num_distance_constraints, int num_welds, std::span gain_sizes, std::span limit_sizes, std::span patch_sizes); @@ -78,6 +79,7 @@ class IcfData { // Scratch data pools for CalcCostAlongLine. BallConstraintsDataPool ball_constraints_data; CouplerConstraintsDataPool coupler_constraints_data; + DistanceConstraintsDataPool distance_constraints_data; GainConstraintsDataPool gain_constraints_data; LimitConstraintsDataPool limit_constraints_data; PatchConstraintsDataPool patch_constraints_data; @@ -111,6 +113,7 @@ class IcfData { @param max_clique_size Maximum number of velocities in any clique. @param num_ball_constraints Number of ball constraints. @param num_couplers Number of coupler constraints. + @param num_distance_constraints Number of distance constraints. @param num_welds Number of weld constraints. @param gain_sizes Number of velocities for each gain constraint. @param limit_sizes Number of velocities for each limit constraint. @@ -120,7 +123,8 @@ class IcfData { // constraint types. Consider switching to a parameter struct which would // let us use named fields at the call sites. void Resize(int num_bodies, int num_velocities, int max_clique_size, - int num_ball_constraints, int num_couplers, int num_welds, + int num_ball_constraints, int num_couplers, + int num_distance_constraints, int num_welds, std::span gain_sizes, std::span limit_sizes, std::span patch_sizes); @@ -177,6 +181,14 @@ class IcfData { return coupler_constraints_data_; } + /* Returns the data pool for distance constraints. */ + const DistanceConstraintsDataPool& distance_constraints_data() const { + return distance_constraints_data_; + } + DistanceConstraintsDataPool& mutable_distance_constraints_data() { + return distance_constraints_data_; + } + /* Returns the data pool for external gain (e.g., actuation) constraints. */ const GainConstraintsDataPool& gain_constraints_data() const { return gain_constraints_data_; @@ -224,6 +236,7 @@ class IcfData { // Type-specific constraint pools. BallConstraintsDataPool ball_constraints_data_; CouplerConstraintsDataPool coupler_constraints_data_; + DistanceConstraintsDataPool distance_constraints_data_; GainConstraintsDataPool gain_constraints_data_; LimitConstraintsDataPool limit_constraints_data_; PatchConstraintsDataPool patch_constraints_data_; diff --git a/multibody/contact_solvers/icf/icf_model.cc b/multibody/contact_solvers/icf/icf_model.cc index 7448e5ac0125..b13cf275bad6 100644 --- a/multibody/contact_solvers/icf/icf_model.cc +++ b/multibody/contact_solvers/icf/icf_model.cc @@ -22,6 +22,7 @@ IcfModel::IcfModel() : params_{std::make_unique>()}, ball_constraints_pool_(this), coupler_constraints_pool_(this), + distance_constraints_pool_(this), gain_constraints_pool_(this), limit_constraints_pool_(this), patch_constraints_pool_(this), @@ -100,6 +101,7 @@ void IcfModel::ResizeData(IcfData* data) const { data->Resize(num_bodies_, num_velocities_, max_clique_size_, ball_constraints_pool_.num_constraints(), coupler_constraints_pool_.num_constraints(), + distance_constraints_pool_.num_constraints(), weld_constraints_pool_.num_constraints(), gain_constraints_pool_.constraint_sizes(), limit_constraints_pool_.constraint_sizes(), @@ -122,6 +124,8 @@ void IcfModel::CalcData(const VectorX& v, IcfData* data) const { ball_constraints_pool_.CalcData(V_WB, &data->mutable_ball_constraints_data()); coupler_constraints_pool_.CalcData(v, &data->mutable_coupler_constraints_data()); + distance_constraints_pool_.CalcData( + V_WB, &data->mutable_distance_constraints_data()); gain_constraints_pool_.CalcData(v, &data->mutable_gain_constraints_data()); limit_constraints_pool_.CalcData(v, &data->mutable_limit_constraints_data()); patch_constraints_pool_.CalcData(V_WB, @@ -132,6 +136,7 @@ void IcfModel::CalcData(const VectorX& v, IcfData* data) const { VectorX& gradient = data->mutable_gradient(); ball_constraints_pool_.AccumulateGradient(*data, &gradient); coupler_constraints_pool_.AccumulateGradient(*data, &gradient); + distance_constraints_pool_.AccumulateGradient(*data, &gradient); gain_constraints_pool_.AccumulateGradient(*data, &gradient); limit_constraints_pool_.AccumulateGradient(*data, &gradient); patch_constraints_pool_.AccumulateGradient(*data, &gradient); @@ -140,6 +145,7 @@ void IcfModel::CalcData(const VectorX& v, IcfData* data) const { // Accumulate cost contributions from constraints. data->set_cost(data->momentum_cost() + data->ball_constraints_data().cost() + data->coupler_constraints_data().cost() + + data->distance_constraints_data().cost() + data->gain_constraints_data().cost() + data->limit_constraints_data().cost() + data->patch_constraints_data().cost() + @@ -170,6 +176,7 @@ void IcfModel::UpdateHessian( // Add constraints' contributions. ball_constraints_pool_.AccumulateHessian(data, hessian); coupler_constraints_pool_.AccumulateHessian(data, hessian); + distance_constraints_pool_.AccumulateHessian(data, hessian); gain_constraints_pool_.AccumulateHessian(data, hessian); limit_constraints_pool_.AccumulateHessian(data, hessian); patch_constraints_pool_.AccumulateHessian(data, hessian); @@ -300,6 +307,24 @@ T IcfModel::CalcCostAlongLine( *d2cost_dalpha2 += constraint_d2cost; } + // Add distance constraints contributions: + { + T constraint_dcost, constraint_d2cost; + + EigenPool>& V_WB_alpha = data.scratch().V_WB_alpha; + // N.B. V_WB_alpha was already computed above for patch constraints. + + distance_constraints_pool_.CalcData( + V_WB_alpha, &data.scratch().distance_constraints_data); + distance_constraints_pool_.CalcCostAlongLine( + data.scratch().distance_constraints_data, search_direction.U, + &constraint_dcost, &constraint_d2cost); + + cost += data.scratch().distance_constraints_data.cost(); + *dcost_dalpha += constraint_dcost; + *d2cost_dalpha2 += constraint_d2cost; + } + // Add weld constraints contributions: { T constraint_dcost, constraint_d2cost; @@ -342,12 +367,14 @@ void IcfModel::SetSparsityPattern() { // Build off-diagonal entries in the sparsity pattern. ball_constraints_pool_.CalcSparsityPattern(&sparsity); + distance_constraints_pool_.CalcSparsityPattern(&sparsity); patch_constraints_pool_.CalcSparsityPattern(&sparsity); weld_constraints_pool_.CalcSparsityPattern(&sparsity); - // Precompute the iteration-invariant ball and weld Hessian blocks now that - // all constraint data and model parameters are finalized. + // Precompute the iteration-invariant ball, distance, and weld Hessian blocks + // now that all constraint data and model parameters are finalized. ball_constraints_pool_.PrecomputeHessianBlocks(); + distance_constraints_pool_.PrecomputeHessianBlocks(); weld_constraints_pool_.PrecomputeHessianBlocks(); // TODO(#23912): This line allocates. @@ -376,10 +403,11 @@ void IcfModel::UpdateTimeStep(const T& time_step) { params_->time_step = time_step; - // Ball and weld constraint regularization R depends on the time step. - // Recompute Hessian blocks whenever dt changes so - // AccumulateHessian() uses up-to-date values. + // Ball, distance, and weld constraint regularization R depends on the time + // step. Recompute Hessian blocks whenever dt changes so AccumulateHessian() + // uses up-to-date values. ball_constraints_pool_.PrecomputeHessianBlocks(); + distance_constraints_pool_.PrecomputeHessianBlocks(); weld_constraints_pool_.PrecomputeHessianBlocks(); } @@ -482,6 +510,8 @@ void IcfModel::ReduceInto(IcfModel* reduced_model, &reduced_model->ball_constraints_pool()); coupler_constraints_pool().ReduceInto( *mapping, &reduced_model->coupler_constraints_pool()); + distance_constraints_pool().ReduceInto( + *mapping, &reduced_model->distance_constraints_pool()); gain_constraints_pool().ReduceInto(*mapping, &reduced_model->gain_constraints_pool()); limit_constraints_pool().ReduceInto(*mapping, diff --git a/multibody/contact_solvers/icf/icf_model.h b/multibody/contact_solvers/icf/icf_model.h index 3d48feee982d..639845340a72 100644 --- a/multibody/contact_solvers/icf/icf_model.h +++ b/multibody/contact_solvers/icf/icf_model.h @@ -12,6 +12,7 @@ #include "drake/multibody/contact_solvers/block_sparse_lower_triangular_or_symmetric_matrix.h" #include "drake/multibody/contact_solvers/icf/ball_constraints_pool.h" #include "drake/multibody/contact_solvers/icf/coupler_constraints_pool.h" +#include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h" #include "drake/multibody/contact_solvers/icf/eigen_pool.h" #include "drake/multibody/contact_solvers/icf/gain_constraints_pool.h" #include "drake/multibody/contact_solvers/icf/icf_data.h" @@ -164,8 +165,9 @@ class IcfModel { /* Returns the total number of constraints of any type in the problem. */ int num_constraints() const { return num_ball_constraints() + num_coupler_constraints() + - num_gain_constraints() + num_limit_constraints() + - num_patch_constraints() + num_weld_constraints(); + num_distance_constraints() + num_gain_constraints() + + num_limit_constraints() + num_patch_constraints() + + num_weld_constraints(); } /* Provides const access to the pool of all ball constraints. */ @@ -188,6 +190,16 @@ class IcfModel { return coupler_constraints_pool_; } + /* Provides const access to the pool of all distance constraints. */ + const DistanceConstraintsPool& distance_constraints_pool() const { + return distance_constraints_pool_; + } + + /* Provides mutable access to the pool of all distance constraints. */ + DistanceConstraintsPool& distance_constraints_pool() { + return distance_constraints_pool_; + } + /* Provides const access to the pool of all gain (e.g., actuation) constraints. */ const GainConstraintsPool& gain_constraints_pool() const { @@ -238,6 +250,10 @@ class IcfModel { return coupler_constraints_pool_.num_constraints(); } + int num_distance_constraints() const { + return distance_constraints_pool_.num_constraints(); + } + int num_gain_constraints() const { return gain_constraints_pool_.num_constraints(); } @@ -510,6 +526,7 @@ class IcfModel { // Fixed set of constraints. BallConstraintsPool ball_constraints_pool_; CouplerConstraintsPool coupler_constraints_pool_; + DistanceConstraintsPool distance_constraints_pool_; GainConstraintsPool gain_constraints_pool_; LimitConstraintsPool limit_constraints_pool_; PatchConstraintsPool patch_constraints_pool_; diff --git a/multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc b/multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc new file mode 100644 index 000000000000..1e0930d4d12c --- /dev/null +++ b/multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc @@ -0,0 +1,155 @@ +/* Tests that CENIC can resolve a distance constraint with a large initial +error and then maintain it over time under gravity, for both a rigid distance +constraint and a compliant (spring-damper) one. + +Setup: + - box1: a cube welded to the world at z=1.0 (no joint in MJCF), so point P at + its bottom-face center is fixed at world (0, 0, 0.85). + - box2: a cube, free body (6 DOF), connected to box1 via AddDistanceConstraint + with point Q at box2's center of mass. The constraint holds ‖p_WQ − p_WP‖ + at the free length ℓ (rigid) or as a spring-damper (compliant). box2 starts + severely displaced (a large initial distance error), so the ICF constraint + correction required is large, which exercises the near-rigid small-time-step + regime. Because Q is at box2's center of mass, gravity produces no moment + and box2 hangs straight below P. +*/ + +#include +#include +#include + +#include + +#include "drake/math/rigid_transform.h" +#include "drake/multibody/cenic/cenic_integrator.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/plant/multibody_plant_config_functions.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace icf { +namespace { + +using Eigen::Vector3d; +using math::RigidTransformd; +using multibody::CenicIntegrator; +using multibody::MultibodyPlant; +using multibody::MultibodyPlantConfig; +using multibody::Parser; +using systems::DiagramBuilder; +using systems::Simulator; + +// MJCF model defining two boxes and a floor. +// +// box1: half-extents 0.15m, welded to the world (no joint) at z=1.0. +// box2: half-extents 0.10m, free floating body (freejoint). The distance +// constraint will be added programmatically below. +// floor: half-extents 1m x 1m x 0.05m, for visual context. +constexpr char kMjcf[] = R"""( + + + + + + + + + + + + + + + + + + +)"""; + +// Point P on box1 (bottom-face center) → world (0, 0, 0.85), and point Q at +// box2's center of mass (its body origin). +constexpr double kFreeLength = 0.3; +const Vector3d kPWorld(0.0, 0.0, 0.85); + +// Builds the diagram/plant with a distance constraint (rigid iff stiffness is +// infinite), starts box2 with a large initial distance error, simulates with +// CENIC, and returns the final distance ‖p_WQ − p_WP‖. +double SimulateAndGetFinalDistance(double stiffness, double damping) { + DiagramBuilder builder; + + // Continuous-time plant is required for the CENIC integrator. + MultibodyPlantConfig plant_config; + plant_config.time_step = 0.0; + auto [plant, scene_graph] = AddMultibodyPlant(plant_config, &builder); + + Parser(&plant).AddModelsFromString(kMjcf, "xml"); + + const auto& box1 = plant.GetBodyByName("box1"); + const auto& box2 = plant.GetBodyByName("box2"); + + const Vector3d p_box1_P(0.0, 0.0, -0.15); // Bottom face of box1. + const Vector3d p_box2_Q(0.0, 0.0, 0.0); // box2 center of mass. + plant.AddDistanceConstraint(box1, p_box1_P, box2, p_box2_Q, kFreeLength, + stiffness, damping); + + plant.Finalize(); + auto diagram = builder.Build(); + + // Start box2 well below its constrained position: the constrained center is + // at distance ~kFreeLength below P (i.e. z ≈ 0.55). Start at z = 0.05, so the + // initial distance is 0.8 — a large error CENIC must resolve. + auto context = diagram->CreateDefaultContext(); + auto& plant_context = plant.GetMyMutableContextFromRoot(context.get()); + plant.SetFloatingBaseBodyPoseInWorldFrame( + &plant_context, box2, RigidTransformd(Vector3d(0.0, 0.0, 0.05))); + + auto simulator = + std::make_unique>(*diagram, std::move(context)); + auto& integrator = simulator->reset_integrator>(); + integrator.set_maximum_step_size(0.1); + integrator.set_fixed_step_mode(false); // Use error control. + integrator.set_target_accuracy(1e-3); + + simulator->Initialize(); + simulator->AdvanceTo(1.0); + + const auto& final_plant_context = + plant.GetMyContextFromRoot(simulator->get_context()); + const RigidTransformd& X_WB2 = + plant.EvalBodyPoseInWorld(final_plant_context, box2); + const Vector3d p_WQ = X_WB2 * p_box2_Q; + return (p_WQ - kPWorld).norm(); +} + +// A rigid distance constraint should drive the distance to the free length. +GTEST_TEST(DistanceConstraintSimulation, RigidLargeInitialError) { + const double distance = SimulateAndGetFinalDistance( + /*stiffness=*/std::numeric_limits::infinity(), /*damping=*/0.0); + // The near-rigid regularization allows a tiny stretch under gravity; 1e-3 is + // comfortably tight (< 1 mm) while robust to that residual compliance. + EXPECT_NEAR(distance, kFreeLength, 1e-3); +} + +// A compliant distance constraint behaves as a spring-damper. At rest under +// gravity the spring stretches so that k⋅(d − ℓ) = m⋅g, i.e. d = ℓ + m⋅g/k. +GTEST_TEST(DistanceConstraintSimulation, CompliantSpring) { + const double kStiffness = 200.0; // N/m. + const double kDamping = 20.0; // N⋅s/m (settles within the sim horizon). + const double kMass = 1.0; // box2 mass, from the MJCF. + const double kGravity = 9.81; // Default plant gravity magnitude. + + const double distance = SimulateAndGetFinalDistance(kStiffness, kDamping); + const double expected_distance = kFreeLength + kMass * kGravity / kStiffness; + EXPECT_NEAR(distance, expected_distance, 2e-3); +} + +} // namespace +} // namespace icf +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc b/multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc new file mode 100644 index 000000000000..bfb735de9f84 --- /dev/null +++ b/multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc @@ -0,0 +1,683 @@ +#include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/limit_malloc.h" +#include "drake/math/autodiff_gradient.h" +#include "drake/math/cross_product.h" +#include "drake/multibody/contact_solvers/icf/icf_data.h" +#include "drake/multibody/contact_solvers/icf/icf_model.h" +#include "drake/multibody/contact_solvers/icf/icf_search_direction_data.h" +#include "drake/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.h" + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace drake { +namespace multibody { +namespace contact_solvers { +namespace icf { +namespace internal { +namespace { + +constexpr double kEpsilon = std::numeric_limits::epsilon(); + +/* Checks that model.CalcData does not incur any heap allocations on a problem +with distance constraints. */ +GTEST_TEST(DistanceConstraintsPool, LimitMallocOnCalcData) { + IcfModel model; + MakeUnconstrainedModel(&model); + AddDistanceConstraints(&model); + model.SetSparsityPattern(); + EXPECT_EQ(model.num_cliques(), 3); + EXPECT_EQ(model.num_velocities(), 18); + EXPECT_EQ(model.num_constraints(), 2); + EXPECT_EQ(model.num_distance_constraints(), 2); + + IcfData data; + model.ResizeData(&data); + EXPECT_EQ(data.distance_constraints_data().num_constraints(), 2); + + const int nv = model.num_velocities(); + const VectorXd v = VectorXd::LinSpaced(nv, -10.0, 10.0); + + // Computing data should not cause any new allocations. + { + drake::test::LimitMalloc guard; + model.CalcData(v, &data); + } +} + +/* Checks that pool.ReduceInto does not incur any heap allocations on a +problem with distance constraints. */ +GTEST_TEST(DistanceConstraintsPool, LimitMallocOnReduceInto) { + IcfModel model; + MakeUnconstrainedModel(&model); + AddDistanceConstraints(&model); + + IcfModel reduced_model; + ReducedMapping mapping; + + // Do a not-smaller reduction to allocate memory in the reduced model. + MakeModelReducible(&model, {}); + model.ReduceInto(&reduced_model, &mapping); + + // Given prior allocation of a big enough model, the constraint pool + // reduction does not allocate. + { + drake::test::LimitMalloc guard; + model.distance_constraints_pool().ReduceInto( + mapping, &reduced_model.distance_constraints_pool()); + } +} + +/* Verifies that distance constraints produce correct data. Uses a rigid and a +compliant constraint (see AddDistanceConstraints), exercising both R branches. +*/ +GTEST_TEST(DistanceConstraintsPool, Data) { + IcfModel model; + MakeUnconstrainedModel(&model); + model.SetSparsityPattern(); + EXPECT_EQ(model.num_cliques(), 3); + EXPECT_EQ(model.num_velocities(), 18); + EXPECT_EQ(model.num_constraints(), 0); + + IcfData data; + model.ResizeData(&data); + EXPECT_EQ(data.num_velocities(), model.num_velocities()); + EXPECT_EQ(data.distance_constraints_data().num_constraints(), 0); + + // At this point there should be no distance constraints. + EXPECT_EQ(model.num_distance_constraints(), 0); + EXPECT_EQ(model.num_constraints(), 0); + + // Add distance constraints. + AddDistanceConstraints(&model); + EXPECT_EQ(model.num_distance_constraints(), 2); + EXPECT_EQ(model.num_constraints(), 2); + + // Re-set sparsity since distance constraints introduce cross-clique coupling. + model.SetSparsityPattern(); + + // Resize data to include distance constraints data. + model.ResizeData(&data); + EXPECT_EQ(data.distance_constraints_data().num_constraints(), 2); + + const int nv = model.num_velocities(); + VectorXd v_value = VectorXd::LinSpaced(nv, -10, 10.0); + VectorX v(nv); + math::InitializeAutoDiff(v_value, &v); + model.CalcData(v, &data); + + const DistanceConstraintsDataPool& distance_data = + data.distance_constraints_data(); + EXPECT_EQ(distance_data.num_constraints(), 2); + + // The distance constraints should add positive cost (non-zero error). + EXPECT_GT(distance_data.cost().value(), 0.0); + + // Impulses should be finite and non-zero scalars. + for (int k = 0; k < 2; ++k) { + const AutoDiffXd& gamma = distance_data.gamma(k)(0); + EXPECT_TRUE(std::isfinite(gamma.value())); + EXPECT_GT(std::abs(gamma.value()), 0.0); + } + + // The total cost should include the distance contribution. + EXPECT_GT(data.cost().value(), data.momentum_cost().value()); + + // Verify accumulated total cost and gradients via AutoDiff. + const VectorXd total_cost_derivatives = data.cost().derivatives(); + const VectorXd total_gradient_value = math::ExtractValue(data.gradient()); + EXPECT_TRUE(CompareMatrices(total_gradient_value, total_cost_derivatives, + 2 * kEpsilon, MatrixCompareType::relative)); + + // Verify contributions to Hessian. The autodiff derivatives of the gradient + // give the exact Hessian, so the difference from the analytically computed + // Hessian is pure floating-point round-off, which scales with the magnitude + // of the (large) intermediate products rather than with each individual + // entry. We therefore use a scale-aware absolute tolerance. + auto distance_hessian = model.MakeHessian(data); + MatrixXd distance_hessian_value = + math::ExtractValue(distance_hessian->MakeDenseMatrix()); + MatrixXd distance_gradient_derivatives = + math::ExtractGradient(data.gradient()); + const double hessian_scale = distance_hessian_value.cwiseAbs().maxCoeff(); + EXPECT_TRUE(CompareMatrices( + distance_hessian_value, distance_gradient_derivatives, + 100 * kEpsilon * hessian_scale, MatrixCompareType::absolute)); + + // The cross-clique distance (body 2, clique 1 to body 3, clique 2) should + // produce non-zero off-diagonal blocks in the Hessian. + const double off_diag_norm = distance_hessian_value.block<6, 6>(6, 12).norm(); + EXPECT_GT(off_diag_norm, 0.0); + + // Check CalcCostAlongLine for distance constraints. + const VectorX w = VectorX::LinSpaced( + nv, 0.1, -0.2); // Arbitrary search direction. + IcfSearchDirectionData search_data; + + // Set data with constant value of v. + VectorX v_constant = + VectorX::LinSpaced(nv, -10, 10.0); + model.CalcData(v_constant, &data); + model.CalcSearchDirectionData(data, w, &search_data); + + const AutoDiffXd alpha = { + 0.35 /* arbitrary value */, + VectorXd::Ones(1) /* This is the independent variable */}; + AutoDiffXd dcost, d2cost; + const AutoDiffXd cost = + model.CalcCostAlongLine(alpha, data, search_data, &dcost, &d2cost); + + const double scale = std::abs(dcost.value()); + EXPECT_NEAR(dcost.value(), cost.derivatives()[0], scale * kEpsilon); + EXPECT_NEAR(d2cost.value(), dcost.derivatives()[0], scale * kEpsilon); +} + +/* Sets up a simple model with 3 bodies in separate cliques, suitable for +testing distance constraints. Body 0 is the world (anchored), bodies 1-3 are +dynamic with 6 DOFs each. */ +template +void MakeModelForDistance(IcfModel* model, double time_step = 0.01) { + const int nv = 18; + + std::unique_ptr> params = model->ReleaseParameters(); + ASSERT_TRUE(params != nullptr); + + params->time_step = time_step; + params->v0 = VectorX::LinSpaced(nv, -1.0, 1.0); + + // Sparse mass matrix with three cliques of size 6. + const Matrix6 A1 = 0.3 * Matrix6::Identity(); + const Matrix6 A2 = 2.3 * Matrix6::Identity(); + const Matrix6 A3 = 1.5 * Matrix6::Identity(); + + MatrixX& M0 = params->M0; + M0 = MatrixX::Identity(nv, nv); + M0.template block<6, 6>(0, 0) = A1; + M0.template block<6, 6>(6, 6) = A2; + M0.template block<6, 6>(12, 12) = A3; + + params->D0 = VectorX::Constant(nv, 0.1); + params->k0 = VectorX::LinSpaced(nv, -1.0, 1.0); + + params->clique_sizes = {6, 6, 6}; + + // Body 0 = world (anchored), body 1 = floating, body 2 = floating, + // body 3 = non-floating (uses non-identity Jacobian). + params->body_is_floating = {0, 1, 1, 0}; + params->body_mass = {1.0e20, 0.3, 2.3, 1.5}; + + // Body-to-clique mapping. World is anchored (clique = -1). + params->body_to_clique = {-1, 0, 1, 2}; + + const Matrix6 J_WB3 = VectorX::LinSpaced(36, -1.0, 1.0).reshaped(6, 6); + + params->J_WB.Resize(4, 6, 6); + params->J_WB[0] = Matrix6::Identity(); // World (ignored). + params->J_WB[1] = Matrix6::Identity(); // Floating body. + params->J_WB[2] = Matrix6::Identity(); // Floating body. + params->J_WB[3] = J_WB3; + + // No joint locking. + auto& reduction = params->reduction; + reduction.unlocked_dofs = {0, 1, 2, 3, 4, 5, // BR + 6, 7, 8, 9, 10, 11, // + 12, 13, 14, 15, 16, 17}; + reduction.per_clique_unlocked_dofs = { + {0, 1, 2, 3, 4, 5}, + {0, 1, 2, 3, 4, 5}, + {0, 1, 2, 3, 4, 5}, + }; + + model->ResetParameters(std::move(params)); +} + +/* Verifies basic construction and accessors. */ +GTEST_TEST(DistanceConstraintsPool, BasicConstruction) { + IcfModel model; + MakeModelForDistance(&model); + + DistanceConstraintsPool& distances = + model.distance_constraints_pool(); + EXPECT_EQ(distances.num_constraints(), 0); + + distances.Resize(2); + EXPECT_EQ(distances.num_constraints(), 2); +} + +/* Verifies that a RIGID distance constraint correctly computes impulse, cost, +gradient, and Hessian for a simple case: a distance constraint between a +floating body (body 1) and the world (body 0, anchored). Expected values are +computed by hand from the distance constraint formulation. */ +GTEST_TEST(DistanceConstraintsPool, DistanceToWorld) { + IcfModel model; + MakeModelForDistance(&model); + + DistanceConstraintsPool& distances = + model.distance_constraints_pool(); + distances.Resize(1); + + const Vector3 p_AP_W(0.1, 0.0, 0.0); + const Vector3 p_BQ_W(0.0, 0.1, 0.0); + const Vector3 p_hat_W(1.0, 0.0, 0.0); // Unit direction. + const double g0 = 0.05; // Distance error d₀ − ℓ. + const double kInf = std::numeric_limits::infinity(); + + // Body 0 (anchored) = A, body 1 (floating, clique 0) = B. Rigid (k = ∞). + distances.Set(0, /*bodyA=*/0, /*bodyB=*/1, p_AP_W, p_BQ_W, p_hat_W, g0, + /*stiffness=*/kInf, /*damping=*/0.0); + + model.SetSparsityPattern(); + + IcfData data; + model.ResizeData(&data); + + const VectorXd& v0 = model.v0(); + model.CalcData(v0, &data); + + // --- Hand calculation of expected distance constraint values --- + const double dt = model.time_step(); + const double mass_B = model.body_mass(1); + + // Constraint velocity vc = p̂ᵀ⋅v_W_Bq, with v_W_Bq the velocity of point Q on + // the floating body 1. Body A (world) is anchored so it contributes nothing. + const Vector3 w_WB = v0.head<3>(); + const Vector3 v_WBo = v0.segment<3>(3); + const Vector3 v_W_Bq = v_WBo + w_WB.cross(p_BQ_W); + const double vc = p_hat_W.dot(v_W_Bq); + + // Near-rigid regularization (rigid ⇒ near-rigid branch chosen). + constexpr double kBeta = IcfModel::kBeta; + const double taud = kBeta * dt / M_PI; + const double dt_plus_taud = dt + taud; + const double r_scale = + (kBeta * kBeta * dt * dt) / (4.0 * M_PI * M_PI * dt * dt_plus_taud); + const double w = 1.0 / mass_B; // Body A anchored. + const double R = r_scale * w; + const double v_hat = -g0 / dt_plus_taud; + + const double expected_gamma = (v_hat - vc) / R; + const double expected_cost = 0.5 * (v_hat - vc) * expected_gamma; + + // Expected momentum cost for v = v0. + MatrixXd A_mat = model.M0(); + A_mat.diagonal() += dt * model.D0(); + const VectorXd Av0 = A_mat * v0; + const VectorXd r = Av0 - dt * model.k0(); + const double expected_momentum_cost = v0.dot(0.5 * Av0 - r); + const double expected_total_cost = expected_momentum_cost + expected_cost; + + // --- Verify computed values match hand calculation --- + // The rigid constraint must select the near-rigid regularization. + EXPECT_NEAR(distances.R()[0], R, 4 * kEpsilon * R); + EXPECT_NEAR(distances.v_hat()[0](0), v_hat, 4 * kEpsilon * std::abs(v_hat)); + + const double gamma = data.distance_constraints_data().gamma(0)(0); + EXPECT_NEAR(gamma, expected_gamma, 4 * kEpsilon * std::abs(expected_gamma)); + + const double distance_cost = data.distance_constraints_data().cost(); + EXPECT_NEAR(distance_cost, expected_cost, + 4 * kEpsilon * std::abs(expected_cost)); + + EXPECT_NEAR(data.momentum_cost(), expected_momentum_cost, + 4 * kEpsilon * std::abs(expected_momentum_cost)); + EXPECT_NEAR(data.cost(), expected_total_cost, + 4 * kEpsilon * std::abs(expected_total_cost)); + + // --- Hand calculation of the (rank-1) Hessian block for body B (clique 0) + // --- + // + // For a distance-to-world constraint with floating body B, the contribution + // to the (c_B, c_B) block is H_BB = Φ(p_BQ)ᵀ⋅diag(0, Gt)⋅Φ(p_BQ), where the + // rank-1 translational regularization Gt = R⁻¹⋅p̂⋅p̂ᵀ. Body B is floating so + // J_WB = I₆, giving H_BB directly. + const double R_inv = 1.0 / R; + const Eigen::Matrix3d Gt = R_inv * p_hat_W * p_hat_W.transpose(); + const Eigen::Matrix3d px = math::VectorToSkewSymmetric(p_BQ_W); + + Matrix6 expected_H_BB; + expected_H_BB.topLeftCorner<3, 3>() = -px * Gt * px; + expected_H_BB.topRightCorner<3, 3>() = px * Gt; + expected_H_BB.bottomLeftCorner<3, 3>() = -Gt * px; + expected_H_BB.bottomRightCorner<3, 3>() = Gt; + + const Matrix6 A_clique0 = A_mat.block<6, 6>(0, 0); + const Matrix6 expected_hessian_block = A_clique0 + expected_H_BB; + + const MatrixXd hessian = model.MakeHessian(data)->MakeDenseMatrix(); + EXPECT_TRUE(hessian.allFinite()); + + const Matrix6 hessian_block_B = hessian.block<6, 6>(0, 0); + EXPECT_TRUE(CompareMatrices(hessian_block_B, expected_hessian_block, + 4 * kEpsilon, MatrixCompareType::relative)); +} + +/* Verifies the compliant (finite stiffness/damping) branch: the regularization +must be R = 1/(δt⋅(δt + τ)⋅k) with τ = c/k, and the impulse/cost must match the +hand-computed spring-damper values. */ +GTEST_TEST(DistanceConstraintsPool, CompliantSpring) { + IcfModel model; + MakeModelForDistance(&model); + + DistanceConstraintsPool& distances = + model.distance_constraints_pool(); + distances.Resize(1); + + const Vector3 p_AP_W(0.1, 0.0, 0.0); + const Vector3 p_BQ_W(0.0, 0.1, 0.0); + const Vector3 p_hat_W(1.0, 0.0, 0.0); + const double g0 = 0.05; + // Soft spring so the compliant branch (not the near-rigid floor) is selected. + const double stiffness = 100.0; + const double damping = 1.0; + + distances.Set(0, /*bodyA=*/0, /*bodyB=*/1, p_AP_W, p_BQ_W, p_hat_W, g0, + stiffness, damping); + + model.SetSparsityPattern(); + + IcfData data; + model.ResizeData(&data); + const VectorXd& v0 = model.v0(); + model.CalcData(v0, &data); + + // --- Hand calculation --- + const double dt = model.time_step(); + const double tau = damping / stiffness; + const double R_compliant = 1.0 / (dt * (dt + tau) * stiffness); + + // Confirm the compliant branch was selected (softer than the near-rigid + // floor). + constexpr double kBeta = IcfModel::kBeta; + const double taud = kBeta * dt / M_PI; + const double r_scale = + (kBeta * kBeta * dt * dt) / (4.0 * M_PI * M_PI * dt * (dt + taud)); + const double R_near_rigid = r_scale * (1.0 / model.body_mass(1)); + ASSERT_GT(R_compliant, R_near_rigid); + + EXPECT_NEAR(distances.R()[0], R_compliant, 4 * kEpsilon * R_compliant); + + const double v_hat = -g0 / (dt + tau); + EXPECT_NEAR(distances.v_hat()[0](0), v_hat, 4 * kEpsilon * std::abs(v_hat)); + + // Constraint velocity and expected impulse/cost. + const Vector3 w_WB = v0.head<3>(); + const Vector3 v_WBo = v0.segment<3>(3); + const Vector3 v_W_Bq = v_WBo + w_WB.cross(p_BQ_W); + const double vc = p_hat_W.dot(v_W_Bq); + const double expected_gamma = (v_hat - vc) / R_compliant; + const double expected_cost = 0.5 * (v_hat - vc) * expected_gamma; + + const double gamma = data.distance_constraints_data().gamma(0)(0); + EXPECT_NEAR(gamma, expected_gamma, 4 * kEpsilon * std::abs(expected_gamma)); + EXPECT_NEAR(data.distance_constraints_data().cost(), expected_cost, + 4 * kEpsilon * std::abs(expected_cost)); +} + +/* Verifies the distance constraint between two dynamic bodies in different +cliques (cross-clique case). Parameterized to test both clique orderings. */ +class CrossCliqueDistanceTest : public testing::TestWithParam {}; + +TEST_P(CrossCliqueDistanceTest, CrossCliqueDistance) { + const bool reverse_bodies = GetParam(); + + IcfModel model; + MakeModelForDistance(&model); + + DistanceConstraintsPool& distances = + model.distance_constraints_pool(); + distances.Resize(1); + + // Distance between body 1 (clique 0) and body 2 (clique 1). + const Vector3 p_AP_W(0.1, 0.2, 0.3); + const Vector3 p_BQ_W(0.0, 0.1, 0.0); + const Vector3 p_hat_W = Vector3(0.2, -0.1, 0.3).normalized(); + const double g0 = 0.02; + + if (reverse_bodies) { + // Body 2 = A (clique 1), Body 1 = B (clique 0), i.e. c_A > c_B. + distances.Set(0, /*bodyA=*/2, /*bodyB=*/1, p_AP_W, p_BQ_W, p_hat_W, g0, + /*stiffness=*/1000.0, /*damping=*/5.0); + } else { + // Body 1 = A (clique 0), Body 2 = B (clique 1), i.e. c_B > c_A. + distances.Set(0, /*bodyA=*/1, /*bodyB=*/2, p_AP_W, p_BQ_W, p_hat_W, g0, + /*stiffness=*/1000.0, /*damping=*/5.0); + } + + model.SetSparsityPattern(); + + IcfData data; + model.ResizeData(&data); + const VectorXd v = model.v0(); + model.CalcData(v, &data); + + // Verify cost and impulse. + EXPECT_GT(data.distance_constraints_data().cost(), 0.0); + const double gamma = data.distance_constraints_data().gamma(0)(0); + EXPECT_TRUE(std::isfinite(gamma)); + EXPECT_GT(std::abs(gamma), 0.0); + + // Build Hessian and verify it has off-diagonal blocks. + auto hessian = model.MakeHessian(data); + const MatrixXd H_dense = hessian->MakeDenseMatrix(); + EXPECT_TRUE(H_dense.allFinite()); + + // The off-diagonal block between clique 0 and clique 1 should be non-zero. + const double off_diag_norm = H_dense.block<6, 6>(0, 6).norm(); + EXPECT_GT(off_diag_norm, 0.0); +} + +INSTANTIATE_TEST_SUITE_P( + DistanceConstraintsPool, CrossCliqueDistanceTest, testing::Bool(), + [](const testing::TestParamInfo& test_param_info) { + return test_param_info.param ? "CliqueAGreater" : "CliqueBGreater"; + }); + +/* Verifies that CalcCostAlongLine produces consistent derivatives using +AutoDiff. */ +GTEST_TEST(DistanceConstraintsPool, CalcCostAlongLine) { + IcfModel model; + MakeModelForDistance(&model); + + DistanceConstraintsPool& distances = + model.distance_constraints_pool(); + distances.Resize(1); + + const Vector3 p_AP_W(0.1, 0.0, 0.0); + const Vector3 p_BQ_W(0.0, 0.1, 0.0); + const Vector3 p_hat_W(1.0, 0.0, 0.0); + + // Use a compliant spring so this test also covers the compliant branch. + distances.Set(0, 0, 1, p_AP_W, p_BQ_W, p_hat_W, AutoDiffXd(0.05), + AutoDiffXd(100.0), AutoDiffXd(1.0)); + + model.SetSparsityPattern(); + + const int nv = model.num_velocities(); + IcfData data, scratch; + model.ResizeData(&data); + model.ResizeData(&scratch); + + // Compute data at the base point v = v0. + const VectorX v = VectorXd::LinSpaced(nv, -1.0, 1.0); + model.CalcData(v, &data); + + // Arbitrary search direction. + const VectorX w = VectorX::LinSpaced(nv, 0.1, 0.5); + + // Precompute search direction data. + IcfSearchDirectionData search_data; + model.CalcSearchDirectionData(data, w, &search_data); + + // Verify at several α values. + for (double alpha_value : {-0.45, 0.0, 0.15, 0.34, 0.93, 1.32}) { + const AutoDiffXd alpha = {alpha_value, VectorXd::Ones(1)}; + + const VectorX v_alpha = v + alpha * w; + model.CalcData(v_alpha, &scratch); + const double cost_expected = scratch.cost().value(); + const double dcost_expected = scratch.cost().derivatives()[0]; + const VectorXd w_times_H = math::ExtractGradient(scratch.gradient()); + const double d2cost_expected = w_times_H.dot(math::ExtractValue(w)); + + AutoDiffXd dcost, d2cost; + const AutoDiffXd cost = + model.CalcCostAlongLine(alpha, data, search_data, &dcost, &d2cost); + // Scale-aware tolerance with an absolute floor: the analytic and autodiff + // paths agree to a few ULP, but a scalar distance constraint's derivatives + // can be small, so a pure relative tolerance would be too tight. + EXPECT_NEAR(cost.value(), cost_expected, + 100 * kEpsilon * (std::abs(cost_expected) + 1.0)); + EXPECT_NEAR(dcost.value(), dcost_expected, + 100 * kEpsilon * (std::abs(dcost_expected) + 1.0)); + EXPECT_NEAR(d2cost.value(), d2cost_expected, + 100 * kEpsilon * (std::abs(d2cost_expected) + 1.0)); + } +} + +/* Verifies gradient consistency using AutoDiff. */ +GTEST_TEST(DistanceConstraintsPool, GradientConsistency) { + IcfModel model; + MakeModelForDistance(&model); + + DistanceConstraintsPool& distances = + model.distance_constraints_pool(); + distances.Resize(1); + + const Vector3 p_AP_W(0.1, 0.0, 0.0); + const Vector3 p_BQ_W(0.0, 0.1, 0.0); + const Vector3 p_hat_W(1.0, 0.0, 0.0); + + distances.Set(0, 0, 1, p_AP_W, p_BQ_W, p_hat_W, AutoDiffXd(0.05), + AutoDiffXd(100.0), AutoDiffXd(1.0)); + + model.SetSparsityPattern(); + + const int nv = model.num_velocities(); + IcfData data; + model.ResizeData(&data); + + const VectorXd v_values = math::ExtractValue(model.v0()); + VectorX v(nv); + math::InitializeAutoDiff(v_values, &v); + + model.CalcData(v, &data); + + const VectorXd cost_derivatives = data.cost().derivatives(); + const VectorXd gradient_value = math::ExtractValue(data.gradient()); + + EXPECT_TRUE(CompareMatrices(gradient_value, cost_derivatives, 100 * kEpsilon, + MatrixCompareType::relative)); +} + +/* Verifies that reducing the distance constraint pool produces correct data. */ +GTEST_TEST(DistanceConstraintsPool, Reduce) { + IcfModel model; + MakeUnconstrainedModel(&model); + AddDistanceConstraints(&model); + + IcfData data; + model.ResizeData(&data); + const int nv = model.num_velocities(); + const VectorXd v = VectorXd::LinSpaced(nv, -10, 10.0); + model.CalcData(v, &data); + + auto check_reduced = [&](const std::vector& locked_dofs) { + SCOPED_TRACE(fmt::format("locked_dofs [{}]", fmt::join(locked_dofs, ", "))); + MakeModelReducible(&model, locked_dofs); + IcfModel reduced_model; + ReducedMapping mapping; + model.ReduceInto(&reduced_model, &mapping); + + // Check the data transmitted by pool.ReduceInto(). + const auto& full_pool = model.distance_constraints_pool(); + const auto& reduced_pool = reduced_model.distance_constraints_pool(); + + int r_k{0}; // Reduced constraints cursor. + for (int k = 0; k < full_pool.num_constraints(); ++k) { + SCOPED_TRACE( + fmt::format("full constraint {} vs. reduced constraint {}", k, r_k)); + const auto& [a, b] = full_pool.body_pairs()[k]; + const int clique_b = model.params().body_to_clique[b]; + const int clique_a = model.params().body_to_clique[a]; + const bool have_b = mapping.clique_subsequence.participates(clique_b); + const bool have_a = + clique_a >= 0 && mapping.clique_subsequence.participates(clique_a); + if (!(have_a || have_b)) { + continue; + } + const bool is_flipped = have_a && !have_b; + const auto& [r_a, r_b] = reduced_pool.body_pairs()[r_k]; + SCOPED_TRACE(fmt::format("flipped? {} have a? {} have b? {}", is_flipped, + have_a, have_b)); + if (is_flipped) { + EXPECT_EQ(r_a, b); + EXPECT_EQ(r_b, a); + EXPECT_EQ(reduced_pool.p_AP_W()[r_k], full_pool.p_BQ_W()[k]); + EXPECT_EQ(reduced_pool.p_BQ_W()[r_k], full_pool.p_AP_W()[k]); + // The unit direction p̂ (from P to Q) negates when P and Q swap; the + // scalar constraint function g₀ = d − ℓ is symmetric and unchanged. + EXPECT_EQ(reduced_pool.p_hat_W()[r_k], -full_pool.p_hat_W()[k]); + EXPECT_EQ(reduced_pool.g0()[r_k], full_pool.g0()[k]); + } else { + EXPECT_EQ(r_a, a); + EXPECT_EQ(r_b, b); + EXPECT_EQ(reduced_pool.p_AP_W()[r_k], full_pool.p_AP_W()[k]); + EXPECT_EQ(reduced_pool.p_BQ_W()[r_k], full_pool.p_BQ_W()[k]); + EXPECT_EQ(reduced_pool.p_hat_W()[r_k], full_pool.p_hat_W()[k]); + EXPECT_EQ(reduced_pool.g0()[r_k], full_pool.g0()[k]); + } + // Compliance parameters carry over unchanged regardless of flipping. + EXPECT_EQ(reduced_pool.stiffness()[r_k], full_pool.stiffness()[k]); + EXPECT_EQ(reduced_pool.damping()[r_k], full_pool.damping()[k]); + ++r_k; + } + EXPECT_EQ(ssize(reduced_pool.R()), r_k); + EXPECT_EQ(reduced_pool.hessian_blocks_size(), r_k); + EXPECT_EQ(reduced_pool.num_constraints(), r_k); + }; + + // Reduce by none; essentially, just copy. + const std::vector none_locked; + check_reduced(none_locked); + + // Lock some arbitrary dofs. + const std::vector arbitrary_locked = {0, 17}; + check_reduced(arbitrary_locked); + + // Lock clique 0. + const std::vector clique0_locked = {0, 1, 2, 3, 4, 5}; + check_reduced(clique0_locked); + + // Lock clique 1. + const std::vector clique1_locked = {6, 7, 8, 9, 10, 11}; + check_reduced(clique1_locked); + + // Lock clique 2. + const std::vector clique2_locked = {12, 13, 14, 15, 16, 17}; + check_reduced(clique2_locked); + + // Lock everything. + std::vector all_locked(model.num_velocities()); + std::iota(all_locked.begin(), all_locked.end(), 0); + check_reduced(all_locked); +} + +} // namespace +} // namespace internal +} // namespace icf +} // namespace contact_solvers +} // namespace multibody +} // namespace drake diff --git a/multibody/contact_solvers/icf/test/icf_builder_test.cc b/multibody/contact_solvers/icf/test/icf_builder_test.cc index 506b7a0d10d8..045bcf4362dd 100644 --- a/multibody/contact_solvers/icf/test/icf_builder_test.cc +++ b/multibody/contact_solvers/icf/test/icf_builder_test.cc @@ -384,21 +384,47 @@ GTEST_TEST(IcfBuilder, NoBallBetweenAnchoredBodies) { "welded to the world.*not allowed.*"); } -GTEST_TEST(IcfBuilder, DistanceConstraintUnsupported) { +GTEST_TEST(IcfBuilder, DistanceConstraint) { systems::DiagramBuilder diagram_builder; multibody::MultibodyPlantConfig plant_config{.time_step = 0.0}; + MultibodyPlant& plant = multibody::AddMultibodyPlant(plant_config, &diagram_builder); - Parser(&plant, "Pendulum").AddModelsFromString(kRobotXml, "xml"); - - plant.AddDistanceConstraint(plant.get_body(BodyIndex(0)), Vector3d::Zero(), - plant.get_body(BodyIndex(1)), Vector3d::Zero(), - 0.01); + Parser(&plant, "Pendulum1").AddModelsFromString(kRobotXml, "xml"); + Parser(&plant, "Pendulum2").AddModelsFromString(kRobotXml, "xml"); + // A rigid (default, infinite-stiffness) distance constraint and a compliant + // (finite stiffness/damping) one, between body 1 and body 2. The attachment + // points are offset so the two constrained points are not coincident. + plant.AddDistanceConstraint(plant.get_body(BodyIndex(1)), + Vector3d(0.5, 0.0, 0.0), + plant.get_body(BodyIndex(2)), Vector3d::Zero(), + /*distance=*/0.1); + plant.AddDistanceConstraint( + plant.get_body(BodyIndex(1)), Vector3d(0.0, 0.5, 0.0), + plant.get_body(BodyIndex(2)), Vector3d::Zero(), /*distance=*/0.1, + /*stiffness=*/1000.0, /*damping=*/5.0); plant.Finalize(); - DRAKE_EXPECT_THROWS_MESSAGE(IcfBuilder(&plant), - ".*not.*support.*1 distance constraint\\(s\\).*"); + auto diagram = diagram_builder.Build(); + auto diagram_context = diagram->CreateDefaultContext(); + const auto& plant_context = plant.GetMyContextFromRoot(*diagram_context); + + const double time_step = 0.01; + IcfBuilder builder(&plant); + IcfModel model; + builder.UpdateModel(plant_context, time_step, nullptr, nullptr, &model); + EXPECT_EQ(model.num_cliques(), 2); + EXPECT_EQ(model.num_velocities(), plant.num_velocities()); + ASSERT_EQ(model.num_distance_constraints(), 2); + + // Check the distance constraints produced. Both connect body 1 to body 2. + const auto& pool = model.distance_constraints_pool(); + EXPECT_EQ(pool.num_constraints(), 2); + for (int k = 0; k < 2; ++k) { + EXPECT_EQ(pool.body_pairs()[k].first, 1); + EXPECT_EQ(pool.body_pairs()[k].second, 2); + } } GTEST_TEST(IcfBuilder, TendonConstraintUnsupported) { diff --git a/multibody/contact_solvers/icf/test/icf_data_test.cc b/multibody/contact_solvers/icf/test/icf_data_test.cc index e22dc2abcb77..3bc2f3455b98 100644 --- a/multibody/contact_solvers/icf/test/icf_data_test.cc +++ b/multibody/contact_solvers/icf/test/icf_data_test.cc @@ -40,13 +40,15 @@ GTEST_TEST(IcfData, ResizeAndAccessors) { const int max_clique_size = 6; const int num_ball_constraints = 4; const int num_couplers = 2; + const int num_distance_constraints = 5; const int num_welds = 3; const std::vector gain_sizes = {3, 2}; const std::vector limit_sizes = {5, 4, 3}; const std::vector patch_sizes = {8, 6, 4, 2}; data.Resize(num_bodies, num_velocities, max_clique_size, num_ball_constraints, - num_couplers, num_welds, gain_sizes, limit_sizes, patch_sizes); + num_couplers, num_distance_constraints, num_welds, gain_sizes, + limit_sizes, patch_sizes); // Main data elements EXPECT_EQ(data.num_velocities(), num_velocities); @@ -68,6 +70,8 @@ GTEST_TEST(IcfData, ResizeAndAccessors) { num_ball_constraints); EXPECT_EQ(data.scratch().coupler_constraints_data.num_constraints(), num_couplers); + EXPECT_EQ(data.scratch().distance_constraints_data.num_constraints(), + num_distance_constraints); EXPECT_EQ(data.scratch().gain_constraints_data.num_constraints(), ssize(gain_sizes)); EXPECT_EQ(data.scratch().limit_constraints_data.num_constraints(), @@ -98,17 +102,20 @@ GTEST_TEST(IcfData, LimitMallocOnResize) { const int max_clique_size = 7; const int num_ball_constraints = 2; const int num_couplers = 2; + const int num_distance_constraints = 3; const int num_welds = 1; const std::vector gain_sizes = {3}; const std::vector limit_sizes = {4}; const std::vector patch_sizes = {5}; data.Resize(num_bodies, num_velocities, max_clique_size, num_ball_constraints, - num_couplers, num_welds, gain_sizes, limit_sizes, patch_sizes); + num_couplers, num_distance_constraints, num_welds, gain_sizes, + limit_sizes, patch_sizes); // Clearing pools changes size but shouldn't change capacity. EXPECT_EQ(data.scratch().V_WB_alpha.size(), num_bodies); - data.scratch().Resize(0, 0, 0, 0, 0, 0, gain_sizes, limit_sizes, patch_sizes); + data.scratch().Resize(0, 0, 0, 0, 0, 0, 0, gain_sizes, limit_sizes, + patch_sizes); EXPECT_EQ(data.scratch().V_WB_alpha.size(), 0); VectorX v = VectorX::LinSpaced(num_velocities, 1.0, 11.0); @@ -117,8 +124,8 @@ GTEST_TEST(IcfData, LimitMallocOnResize) { // cause any new allocations. drake::test::LimitMalloc guard; data.Resize(num_bodies, num_velocities, max_clique_size, - num_ball_constraints, num_couplers, num_welds, gain_sizes, - limit_sizes, patch_sizes); + num_ball_constraints, num_couplers, num_distance_constraints, + num_welds, gain_sizes, limit_sizes, patch_sizes); data.set_v(v); } EXPECT_EQ(data.scratch().V_WB_alpha.size(), num_bodies); diff --git a/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.cc b/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.cc index 594cf6c89c7b..362ffc0f4be1 100644 --- a/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.cc +++ b/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.cc @@ -1,5 +1,6 @@ #include "drake/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.h" +#include #include #include @@ -399,10 +400,44 @@ void AddBallConstraints(IcfModel* model) { } } +template +void AddDistanceConstraints(IcfModel* model) { + DRAKE_DEMAND(model != nullptr); + constexpr double kInf = std::numeric_limits::infinity(); + + DistanceConstraintsPool& distance_constraints = + model->distance_constraints_pool(); + distance_constraints.Resize(2); + + // Distance 0: world (body 0, anchored) to body 1. Rigid (infinite stiffness). + { + const Vector3 p_AP_W(0.1, 0.0, 0.0); + const Vector3 p_BQ_W(0.0, 0.0, 0.0); + const Vector3 p_hat_W(1.0, 0.0, 0.0); + const T g0(0.05); // Current distance minus free length. + distance_constraints.Set(0, /*bodyA=*/0, /*bodyB=*/1, p_AP_W, p_BQ_W, + p_hat_W, g0, /*stiffness=*/T(kInf), + /*damping=*/T(0.0)); + } + + // Distance 1: body 2 to body 3 (cross-clique in multi-clique case). + // Compliant (finite stiffness and damping) to exercise the spring path. + { + const Vector3 p_AP_W(0.1, 0.2, 0.3); + const Vector3 p_BQ_W(0.0, 0.1, 0.0); + const Vector3 p_hat_W = Vector3(0.2, -0.1, 0.3).normalized(); + const T g0(0.02); + distance_constraints.Set(1, /*bodyA=*/2, /*bodyB=*/3, p_AP_W, p_BQ_W, + p_hat_W, g0, /*stiffness=*/T(1000.0), + /*damping=*/T(5.0)); + } +} + DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( (&MakeUnconstrainedModel, &MakeModelReducible, &AddBallConstraints, - &AddCouplerConstraint, &AddGainConstraints, &AddLimitConstraints, - &AddPatchConstraints, &AddWeldConstraints)); + &AddCouplerConstraint, &AddDistanceConstraints, + &AddGainConstraints, &AddLimitConstraints, &AddPatchConstraints, + &AddWeldConstraints)); } // namespace internal } // namespace icf diff --git a/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.h b/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.h index 1a22d5d0bf89..16bb3400b590 100644 --- a/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.h +++ b/multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.h @@ -69,6 +69,13 @@ one between the world (body 0) and body 1, and one between body 2 and body 3 template void AddBallConstraints(IcfModel* model); +/* Adds distance constraints to the given model. Two distance constraints are +added: a rigid one between the world (body 0) and body 1, and a compliant +(finite stiffness/damping) one between body 2 and body 3 (cross-clique in the +multi-clique case). */ +template +void AddDistanceConstraints(IcfModel* model); + } // namespace internal } // namespace icf } // namespace contact_solvers From a84da9e7fcd5afdb8191da8fa0f1286c81f9888e Mon Sep 17 00:00:00 2001 From: Joseph Masterjohn Date: Wed, 29 Jul 2026 13:22:55 -0400 Subject: [PATCH 2/2] address reviews --- .../icf/distance_constraints_pool.cc | 17 ++++---- .../icf/distance_constraints_pool.h | 2 +- multibody/contact_solvers/icf/icf_builder.cc | 29 +++++++------- .../test/distance_constraints_pool_test.cc | 6 +-- .../icf/test/icf_builder_test.cc | 39 +++++++++++++++++++ .../sap/sap_distance_constraint.h | 2 +- 6 files changed, 67 insertions(+), 28 deletions(-) diff --git a/multibody/contact_solvers/icf/distance_constraints_pool.cc b/multibody/contact_solvers/icf/distance_constraints_pool.cc index c00132a4a870..14a2ebad6170 100644 --- a/multibody/contact_solvers/icf/distance_constraints_pool.cc +++ b/multibody/contact_solvers/icf/distance_constraints_pool.cc @@ -27,18 +27,19 @@ void DistanceConstraintsPool::Set(int index, int bodyA, int bodyB, template Vector1 DistanceConstraintsPool::CalcConstraintVelocity( int k, const Vector6& V_WB, const Vector6* V_WA) const { - // vc = ḋ = p̂ᵀ⋅(v_W_Bq − v_W_Ap), the rate of change of distance. + // With d = ‖p_WBq − p_WAp‖ the distance between P and Q, the constraint + // velocity is vc = ḋ = p̂ᵀ⋅(v_WBq − v_WAp), the rate of change of distance. const Vector3& p_hat_W = p_hat_W_[k]; const Vector3& w_WB = V_WB.template head<3>(); const Vector3& v_WBo = V_WB.template tail<3>(); - const Vector3 v_W_Bq = v_WBo + w_WB.cross(p_BQ_W_[k]); + const Vector3 v_WBq = v_WBo + w_WB.cross(p_BQ_W_[k]); - T vc = p_hat_W.dot(v_W_Bq); + T vc = p_hat_W.dot(v_WBq); if (V_WA != nullptr) { const Vector3& w_WA = V_WA->template head<3>(); const Vector3& v_WAo = V_WA->template tail<3>(); - const Vector3 v_W_Ap = v_WAo + w_WA.cross(p_AP_W_[k]); - vc -= p_hat_W.dot(v_W_Ap); + const Vector3 v_WAp = v_WAo + w_WA.cross(p_AP_W_[k]); + vc -= p_hat_W.dot(v_WAp); } return Vector1(vc); } @@ -50,13 +51,13 @@ void DistanceConstraintsPool::CalcSpatialImpulses( // The scalar impulse γ acts as γ⋅p̂ along the line PQ, applied // at Q on B and −γ⋅p̂ at P on A. Shift each to the body origin. const Vector3& p_hat_W = p_hat_W_[k]; - const Vector3 f_B = gamma(0) * p_hat_W; + const Vector3 j_B = gamma(0) * p_hat_W; // Impulse on B at Q, along p̂. const Vector6 spatial_gamma_Bq = - (Vector6() << Vector3::Zero(), f_B).finished(); + (Vector6() << Vector3::Zero(), j_B).finished(); *Gamma_Bo = ShiftSpatialImpulse(spatial_gamma_Bq, p_BQ_W_[k]); if (Gamma_Ao != nullptr) { const Vector6 minus_spatial_gamma_Ap = - (Vector6() << Vector3::Zero(), Vector3(-f_B)).finished(); + (Vector6() << Vector3::Zero(), Vector3(-j_B)).finished(); *Gamma_Ao = ShiftSpatialImpulse(minus_spatial_gamma_Ap, p_AP_W_[k]); } } diff --git a/multibody/contact_solvers/icf/distance_constraints_pool.h b/multibody/contact_solvers/icf/distance_constraints_pool.h index 955614f0260a..124db0a631dd 100644 --- a/multibody/contact_solvers/icf/distance_constraints_pool.h +++ b/multibody/contact_solvers/icf/distance_constraints_pool.h @@ -27,7 +27,7 @@ Euclidean distance d between a point P on A and a point Q on B to a free length Adapted from the SAP distance constraint (see sap_distance_constraint.h), this is a compliant (spring-damper) constraint: with p̂ the unit vector from P to Q, stiffness k and damping c, the scalar impulse is - γ = −k⋅(d − ℓ) − c⋅ḋ ∈ ℝ + γ = δt⋅(−k⋅(d−ℓ) − c⋅ḋ) ∈ ℝ applied as γ⋅p̂ on B at Q and −γ⋅p̂ on A at P. Unlike other holonomic constraints (e.g. weld, ball), the distance constraint diff --git a/multibody/contact_solvers/icf/icf_builder.cc b/multibody/contact_solvers/icf/icf_builder.cc index dd62322d0b5e..04abfca7ad3f 100644 --- a/multibody/contact_solvers/icf/icf_builder.cc +++ b/multibody/contact_solvers/icf/icf_builder.cc @@ -655,23 +655,22 @@ void IcfBuilder::SetDistanceConstraints(const systems::Context& context, const Vector3 p_AP_W = X_WA.rotation() * p_AP_spec.template cast(); const Vector3 p_BQ_W = X_WB.rotation() * p_BQ_spec.template cast(); - // Current distance d₀ and unit direction p̂ from P to Q. Guard against a - // nonphysically small distance (the constraint is singular there; use a - // ball constraint for coincident points), mirroring SapDistanceConstraint. + // Current distance d₀ = ‖p_WQ − p_WP‖ and unit direction p̂ from P to Q. const Vector3 p_PQ_W = p_WQ - p_WP; const T d0 = p_PQ_W.norm(); - const double length = params.distance(); // The free length ℓ. - constexpr double kMinimumDistance = 1.0e-7; - constexpr double kRelativeDistance = 1.0e-2; - if (ExtractDoubleOrThrow(d0) < - kMinimumDistance + kRelativeDistance * length) { - throw std::logic_error(fmt::format( - "The distance between the two points of a distance constraint " - "between bodies '{}' and '{}' is {}, which is nonphysically small " - "compared to the constraint's free length, {}.", - body_A.name(), body_B.name(), ExtractDoubleOrThrow(d0), length)); - } - const Vector3 p_hat_W = p_PQ_W / d0; + const double length = params.distance(); // The free length ℓ > 0. + + // When P and Q are (nearly) coincident, p̂ is undefined. The free length ℓ + // is strictly positive (enforced by DistanceConstraintParams), so there + // g₀ = d₀ − ℓ < 0 and the constraint pushes the points apart; any unit + // vector serves to seed the direction for the first step, after which + // d₀ > 0 makes p̂ well defined. This lets CENIC resolve an initial + // condition with coincident points rather than rejecting it. The threshold + // is purely a divide-by-zero guard for the normalization below. + constexpr double kMinimumDistance = 1.0e-14; + const Vector3 p_hat_W = ExtractDoubleOrThrow(d0) < kMinimumDistance + ? Vector3(Vector3::UnitX()) + : Vector3(p_PQ_W / d0); // Constraint function g₀ = d₀ − ℓ. const T g0 = d0 - length; diff --git a/multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc b/multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc index bfb735de9f84..9e0d7c94f7fd 100644 --- a/multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc +++ b/multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc @@ -58,7 +58,7 @@ GTEST_TEST(DistanceConstraintsPool, LimitMallocOnCalcData) { } } -/* Checks that pool.ReduceInto does not incur any heap allocations on a +/* Checks that pool.ReduceInto() does not incur any heap allocations on a problem with distance constraints. */ GTEST_TEST(DistanceConstraintsPool, LimitMallocOnReduceInto) { IcfModel model; @@ -82,8 +82,8 @@ GTEST_TEST(DistanceConstraintsPool, LimitMallocOnReduceInto) { } /* Verifies that distance constraints produce correct data. Uses a rigid and a -compliant constraint (see AddDistanceConstraints), exercising both R branches. -*/ +compliant constraint (see AddDistanceConstraints), exercising both the +near-rigid and compliant regularization (R) branches. */ GTEST_TEST(DistanceConstraintsPool, Data) { IcfModel model; MakeUnconstrainedModel(&model); diff --git a/multibody/contact_solvers/icf/test/icf_builder_test.cc b/multibody/contact_solvers/icf/test/icf_builder_test.cc index 045bcf4362dd..4b48f7ace843 100644 --- a/multibody/contact_solvers/icf/test/icf_builder_test.cc +++ b/multibody/contact_solvers/icf/test/icf_builder_test.cc @@ -427,6 +427,45 @@ GTEST_TEST(IcfBuilder, DistanceConstraint) { } } +// A distance constraint whose two attachment points start coincident (d₀ = 0) +// must still assemble: the constraint direction p̂ is undefined there, so +// IcfBuilder seeds it with an arbitrary unit vector for the first step (after +// which d₀ > 0 makes p̂ well defined). Bodies 1 and 2 share an origin in the +// default configuration, so points at each body's origin map to the same world +// location, giving d₀ = 0. +GTEST_TEST(IcfBuilder, DistanceConstraintCoincidentPoints) { + systems::DiagramBuilder diagram_builder; + multibody::MultibodyPlantConfig plant_config{.time_step = 0.0}; + + MultibodyPlant& plant = + multibody::AddMultibodyPlant(plant_config, &diagram_builder); + + Parser(&plant, "Pendulum1").AddModelsFromString(kRobotXml, "xml"); + Parser(&plant, "Pendulum2").AddModelsFromString(kRobotXml, "xml"); + const double kFreeLength = 0.1; + plant.AddDistanceConstraint(plant.get_body(BodyIndex(1)), Vector3d::Zero(), + plant.get_body(BodyIndex(2)), Vector3d::Zero(), + kFreeLength); + plant.Finalize(); + + auto diagram = diagram_builder.Build(); + auto diagram_context = diagram->CreateDefaultContext(); + const auto& plant_context = plant.GetMyContextFromRoot(*diagram_context); + + const double time_step = 0.01; + IcfBuilder builder(&plant); + IcfModel model; + // Assembly must not throw despite the coincident (singular) configuration. + EXPECT_NO_THROW( + builder.UpdateModel(plant_context, time_step, nullptr, nullptr, &model)); + ASSERT_EQ(model.num_distance_constraints(), 1); + + // The seeded direction is a valid unit vector, and g₀ = d₀ − ℓ = −ℓ (d₀ = 0). + const auto& pool = model.distance_constraints_pool(); + EXPECT_NEAR(pool.p_hat_W()[0].norm(), 1.0, 1e-14); + EXPECT_NEAR(pool.g0()[0](0), -kFreeLength, 1e-14); +} + GTEST_TEST(IcfBuilder, TendonConstraintUnsupported) { systems::DiagramBuilder diagram_builder; multibody::MultibodyPlantConfig plant_config{.time_step = 0.0}; diff --git a/multibody/contact_solvers/sap/sap_distance_constraint.h b/multibody/contact_solvers/sap/sap_distance_constraint.h index f1ffd99d0a7f..f4eb1b11762d 100644 --- a/multibody/contact_solvers/sap/sap_distance_constraint.h +++ b/multibody/contact_solvers/sap/sap_distance_constraint.h @@ -22,7 +22,7 @@ class SapDistanceConstraint; 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⋅ḋ + γ = δt⋅(−k⋅(d−ℓ) − c⋅ḋ) where ℓ is the "free length" of the constraint, and k and c are the stiffness and damping coefficients respectively.