Skip to content

Commit 69d56b1

Browse files
committed
fix: correct force directions
1 parent 2a77c13 commit 69d56b1

File tree

2 files changed

+3
-3
lines changed

2 files changed

+3
-3
lines changed

multibody/geom_geom_collider.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -194,12 +194,12 @@ GeomGeomCollider<T>::CalcForceBasisInWorldFrame(
194194
const auto query_result = GetGeometryQueryResult(context);
195195
if (num_friction_directions < 1) {
196196
// Planar contact: basis is constructed from the contact and planar normals.
197-
return ComputePlanarForceBasis(query_result.signed_distance_pair.nhat_BA_W,
197+
return ComputePlanarForceBasis(-query_result.signed_distance_pair.nhat_BA_W,
198198
planar_normal);
199199
} else {
200200
// 3D contact: build polytope basis and rotate using contact normal.
201201
auto R_WC = drake::math::RotationMatrix<T>::MakeFromOneVector(
202-
query_result.signed_distance_pair.nhat_BA_W, 0);
202+
-query_result.signed_distance_pair.nhat_BA_W, 0);
203203
return ComputePolytopeForceBasis(num_friction_directions) *
204204
R_WC.matrix().transpose();
205205
}

multibody/lcs_factory.cc

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -419,7 +419,7 @@ std::vector<LCSContactDescription> LCSFactory::GetContactDescriptions() {
419419
contact_descriptions.at(i).witness_point_A ==
420420
normal_contact_descriptions.at(normal_index).witness_point_A);
421421
contact_descriptions.at(i).force_basis =
422-
- contact_descriptions.at(i).force_basis +
422+
contact_descriptions.at(i).force_basis +
423423
mu_[normal_index] *
424424
normal_contact_descriptions.at(normal_index).force_basis;
425425
}

0 commit comments

Comments
 (0)