diff --git a/multibody/contact_solvers/sap/sap_model.h b/multibody/contact_solvers/sap/sap_model.h index 9a3bb4dcff75..20c8b69e43b2 100644 --- a/multibody/contact_solvers/sap/sap_model.h +++ b/multibody/contact_solvers/sap/sap_model.h @@ -172,7 +172,7 @@ class SapModel { // Inverse of the diagonal matrix formed with the square root of the diagonal // entries of the momentum matrix, i.e. diag(A)^{-1/2}, of size // num_velocities(). This matrix is used to compute a scaled momentum - // residual, see [Castro et al. 2021]. + // residual, see [Castro et al. 2022]. const VectorX& inv_sqrt_dynamics_matrix() const; /* Const access to the bundle for this model. */ @@ -215,7 +215,7 @@ class SapModel { } /* Evaluates the contact impulses γ(v) according to the analytical inverse - dynamics, see [Castro et al. 2021]. + dynamics, see [Castro et al. 2022]. @pre `context` is created with a call to MakeContext(). */ const VectorX& EvalImpulses(const systems::Context& context) const { return EvalImpulsesCache(context).gamma; @@ -274,7 +274,7 @@ class SapModel { For the i-th constraint, its Hessian is defined as Gᵢ(v) = d²ℓᵢ/dvcᵢ², where ℓᵢ is the regularizer's cost ℓᵢ = 1/2⋅γᵢᵀ⋅Rᵢ⋅γᵢ and vcᵢ is the constraint's velocity. Gᵢ is an SPD matrix. It turns out that the Hessian of SAP's cost is - H = A + Jᵀ⋅G⋅J, see [Castro et al. 2021]. + H = A + Jᵀ⋅G⋅J, see [Castro et al. 2022]. @pre `context` is created with a call to MakeContext(). */ const std::vector>& EvalConstraintsHessian( const systems::Context& context) const { @@ -379,7 +379,7 @@ class SapModel { // approximation Wᵢᵢ of the block diagonal element corresponding to the i-th // constraint, the scaling is computed as delassus_diagonal[i] = ‖Wᵢᵢ‖ᵣₘₛ = // ‖Wᵢᵢ‖/nᵢ, where nᵢ is the number of equations for the i-th constraint (and - // the size of Wᵢᵢ). See [Castro et al. 2021] for details. + // the size of Wᵢᵢ). See [Castro et al. 2022] for details. // // @param[in] A linear dynamics matrix for each participating clique in the // model. diff --git a/multibody/contact_solvers/sap/test/sap_solver_test.cc b/multibody/contact_solvers/sap/test/sap_solver_test.cc index f8fc020d2ba7..0294025a0620 100644 --- a/multibody/contact_solvers/sap/test/sap_solver_test.cc +++ b/multibody/contact_solvers/sap/test/sap_solver_test.cc @@ -54,7 +54,7 @@ class SapSolverTester { constexpr double kEps = std::numeric_limits::epsilon(); // Suggested value for the dimensionless parameter used in the regularization of -// friction, see [Castro et al. 2021]. +// friction, see [Castro et al. 2022]. constexpr double kDefaultSigma = 1.0e-3; /* Model of a "pizza saver": @@ -315,7 +315,7 @@ class PizzaSaverTest problem.time_step() * Vector4d(0.0, 0.0, problem.mass() * problem.g(), -Mz); - // Maximum expected slip. See Castro et al. 2021 for details (Eq. 31). + // Maximum expected slip. See Castro et al. 2022 for details (Eq. 31). const double max_slip_expected = kDefaultSigma * problem.mu() * problem.time_step() * problem.g(); @@ -595,8 +595,9 @@ TEST_P(PizzaSaverTest, NearRigidStiction) { const Vector4d j_expected(0.0, 0.0, dt * problem.mass() * problem.g(), -dt * Mz); - // Maximum expected slip. See Castro et al. 2021 for details (Eq. 22). - const double max_slip_expected = kDefaultSigma * dt * problem.g(); + // Maximum expected slip. See Castro et al. 2022 for details (Eq. 31). + const double max_slip_expected = + kDefaultSigma * problem.mu() * dt * problem.g(); EXPECT_TRUE(CompareMatrices(result.v.head<3>(), Vector3d::Zero(), 1.0e-9)); EXPECT_TRUE((result.j - j_expected).norm() / j_expected.norm() <