Skip to content

Commit

Permalink
Updates SAP's reference year (#17513)
Browse files Browse the repository at this point in the history
  • Loading branch information
amcastro-tri authored Jul 13, 2022
1 parent 7b860b9 commit 88630a8
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 8 deletions.
8 changes: 4 additions & 4 deletions multibody/contact_solvers/sap/sap_model.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<T>& inv_sqrt_dynamics_matrix() const;

/* Const access to the bundle for this model. */
Expand Down Expand Up @@ -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<T>& EvalImpulses(const systems::Context<T>& context) const {
return EvalImpulsesCache(context).gamma;
Expand Down Expand Up @@ -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<MatrixX<T>>& EvalConstraintsHessian(
const systems::Context<T>& context) const {
Expand Down Expand Up @@ -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.
Expand Down
9 changes: 5 additions & 4 deletions multibody/contact_solvers/sap/test/sap_solver_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ class SapSolverTester {

constexpr double kEps = std::numeric_limits<double>::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":
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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() <
Expand Down

0 comments on commit 88630a8

Please sign in to comment.