Skip to content

Commit

Permalink
Add DiscreteContactData (RobotLocomotion#20209)
Browse files Browse the repository at this point in the history 
We have three types of contact: point, hydroelastic, and deformable
that share similar contact data like  DiscreteContactPair and
ContactPairKinematics. Currently, we store them all in a std::vector
with an ordering convention that is inconsistent across various classes
that produce and consume these data.

We introduce DiscreteContactData to help enforce ordering conventions
across the board and ensure the correct contact data is retrieved.
  • Loading branch information
@xuchenhan-tri
xuchenhan-tri authored Sep 28, 2023
1 parent 38a51d0 commit d2bcf2d
Show file tree
Hide file tree
Showing 16 changed files with 487 additions and 141 deletions.
27 changes: 27 additions & 0 deletions multibody/plant/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ drake_cc_package_library(
":contact_results",
":coulomb_friction",
":deformable_ids",
":discrete_contact_data",
":discrete_contact_pair",
":externally_applied_spatial_force",
":externally_applied_spatial_force_multiplexer",
Expand Down Expand Up @@ -51,6 +52,15 @@ drake_cc_library(
],
)

drake_cc_library(
name = "discrete_contact_data",
srcs = [],
hdrs = ["discrete_contact_data.h"],
deps = [
"//common:essential",
],
)

drake_cc_library(
name = "discrete_contact_pair",
srcs = [],
Expand Down Expand Up @@ -120,6 +130,7 @@ drake_cc_library(
":contact_results",
":coulomb_friction",
":deformable_ids",
":discrete_contact_data",
":discrete_contact_pair",
":externally_applied_spatial_force",
":hydroelastic_traction",
Expand Down Expand Up @@ -942,6 +953,14 @@ drake_cc_googletest(
],
)

drake_cc_googletest(
name = "deformable_contact_results_test",
deps = [
":plant",
"//systems/framework:diagram_builder",
],
)

drake_cc_googletest(
name = "deformable_driver_test",
deps = [
Expand Down Expand Up @@ -1011,6 +1030,14 @@ drake_cc_googletest(
],
)

drake_cc_googletest(
name = "discrete_contact_data_test",
deps = [
":discrete_contact_data",
"//common/test_utilities:limit_malloc",
],
)

drake_cc_googletest(
name = "floating_body_test",
timeout = "moderate",
Expand Down
160 changes: 80 additions & 80 deletions multibody/plant/compliant_contact_manager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,40 +154,50 @@ void CompliantContactManager<T>::DoDeclareCacheEntries() {
}

template <typename T>
std::vector<ContactPairKinematics<T>>
DiscreteContactData<ContactPairKinematics<T>>
CompliantContactManager<T>::CalcContactKinematics(
const systems::Context<T>& context) const {
const std::vector<DiscreteContactPair<T>>& contact_pairs =
const DiscreteContactData<DiscreteContactPair<T>>& contact_pairs =
this->EvalDiscreteContactPairs(context);
DiscreteContactData<ContactPairKinematics<T>> contact_kinematics;
const int num_contacts = contact_pairs.size();
std::vector<ContactPairKinematics<T>> contact_kinematics;
contact_kinematics.reserve(num_contacts);

// Quick no-op exit.
if (num_contacts == 0) return contact_kinematics;

const geometry::QueryObject<T>& query_object =
this->plant()
.get_geometry_query_input_port()
.template Eval<geometry::QueryObject<T>>(context);
const geometry::SceneGraphInspector<T>& inspector = query_object.inspector();
contact_kinematics.Reserve(contact_pairs.num_point_contacts(),
contact_pairs.num_hydro_contacts(),
contact_pairs.num_deformable_contacts());
AppendContactKinematics(context, contact_pairs.point_contact_data(),
DiscreteContactType::kPoint, &contact_kinematics);
AppendContactKinematics(context, contact_pairs.hydro_contact_data(),
DiscreteContactType::kHydroelastic,
&contact_kinematics);
if constexpr (std::is_same_v<T, double>) {
if (deformable_driver_ != nullptr) {
deformable_driver_->AppendContactKinematics(context, &contact_kinematics);
}
}
return contact_kinematics;
}

template <typename T>
void CompliantContactManager<T>::AppendContactKinematics(
const systems::Context<T>& context,
const std::vector<DiscreteContactPair<T>>& contact_pairs,
DiscreteContactType type,
DiscreteContactData<ContactPairKinematics<T>>* contact_kinematics) const {
// Scratch workspace variables.
const int nv = plant().num_velocities();
Matrix3X<T> Jv_WAc_W(3, nv);
Matrix3X<T> Jv_WBc_W(3, nv);
Matrix3X<T> Jv_AcBc_W(3, nv);

const Frame<T>& frame_W = plant().world_frame();
for (int icontact = 0; icontact < num_contacts; ++icontact) {
for (int icontact = 0; icontact < ssize(contact_pairs); ++icontact) {
const auto& point_pair = contact_pairs[icontact];

const GeometryId geometryA_id = point_pair.id_A;
// All contact pairs involving deformable bodies come after pairs involving
// only rigid bodies. Once we reach a deformable body, we know that there
// are no rigid-only pairs left.
if (inspector.IsDeformableGeometry(geometryA_id)) {
break;
}
const GeometryId geometryB_id = point_pair.id_B;

BodyIndex bodyA_index = this->geometry_id_to_body_index().at(geometryA_id);
Expand Down Expand Up @@ -262,32 +272,39 @@ CompliantContactManager<T>::CalcContactKinematics(
.p_BqC_W = p_BC_W,
.phi = point_pair.phi0,
.R_WC = R_WC};

contact_kinematics.emplace_back(std::move(jacobian_blocks),
std::move(configuration));
}
if constexpr (std::is_same_v<T, double>) {
if (deformable_driver_ != nullptr) {
deformable_driver_->AppendContactKinematics(context, &contact_kinematics);
switch (type) {
case DiscreteContactType::kPoint: {
contact_kinematics->AppendPointData(ContactPairKinematics<T>{
std::move(jacobian_blocks), std::move(configuration)});
break;
}
case DiscreteContactType::kHydroelastic: {
contact_kinematics->AppendHydroData(ContactPairKinematics<T>{
std::move(jacobian_blocks), std::move(configuration)});
break;
}
case DiscreteContactType::kDeformable: {
throw std::logic_error(
"Call DeformableDriver::AppendContactKinematics() to compute "
"contact kinematics for deformable contact instead.");
}
}
}

return contact_kinematics;
}

template <typename T>
const std::vector<ContactPairKinematics<T>>&
const DiscreteContactData<ContactPairKinematics<T>>&
CompliantContactManager<T>::EvalContactKinematics(
const systems::Context<T>& context) const {
return plant()
.get_cache_entry(cache_indexes_.contact_kinematics)
.template Eval<std::vector<ContactPairKinematics<T>>>(context);
.template Eval<DiscreteContactData<ContactPairKinematics<T>>>(context);
}

template <>
void CompliantContactManager<symbolic::Expression>::CalcDiscreteContactPairs(
const drake::systems::Context<symbolic::Expression>&,
std::vector<DiscreteContactPair<symbolic::Expression>>*) const {
DiscreteContactData<DiscreteContactPair<symbolic::Expression>>*) const {
// Currently, the computation of contact pairs is not supported when T =
// symbolic::Expression.
throw std::domain_error(
Expand All @@ -298,11 +315,11 @@ void CompliantContactManager<symbolic::Expression>::CalcDiscreteContactPairs(
template <typename T>
void CompliantContactManager<T>::CalcDiscreteContactPairs(
const systems::Context<T>& context,
std::vector<DiscreteContactPair<T>>* contact_pairs) const {
DiscreteContactData<DiscreteContactPair<T>>* contact_pairs) const {
plant().ValidateContext(context);
DRAKE_DEMAND(contact_pairs != nullptr);

contact_pairs->clear();
contact_pairs->Clear();
if (plant().num_collision_geometries() == 0) return;

const auto contact_model = plant().get_contact_model();
Expand Down Expand Up @@ -335,8 +352,7 @@ void CompliantContactManager<T>::CalcDiscreteContactPairs(
num_quadrature_pairs += s.num_faces();
}
}
const int num_contact_pairs = num_point_pairs + num_quadrature_pairs;
contact_pairs->reserve(num_contact_pairs);
contact_pairs->Reserve(num_point_pairs, num_quadrature_pairs, 0);

if (contact_model == ContactModel::kPoint ||
contact_model == ContactModel::kHydroelasticWithFallback) {
Expand All @@ -356,8 +372,8 @@ void CompliantContactManager<T>::CalcDiscreteContactPairs(
template <typename T>
void CompliantContactManager<T>::AppendDiscreteContactPairsForPointContact(
const systems::Context<T>& context,
std::vector<DiscreteContactPair<T>>* result) const {
std::vector<DiscreteContactPair<T>>& contact_pairs = *result;
DiscreteContactData<DiscreteContactPair<T>>* result) const {
DiscreteContactData<DiscreteContactPair<T>>& contact_pairs = *result;

const geometry::QueryObject<T>& query_object =
this->plant()
Expand Down Expand Up @@ -421,18 +437,19 @@ void CompliantContactManager<T>::AppendDiscreteContactPairsForPointContact(
const T phi0 = -pair.depth;
const T fn0 = k * pair.depth; // Used by TAMSI, ignored by SAP.

contact_pairs.push_back({pair.id_A,
pair.id_B,
p_WC,
pair.nhat_BA_W,
phi0,
fn0,
k,
d,
tau,
mu,
{} /* no surface index */,
{} /* no face index */});
contact_pairs.AppendPointData(
DiscreteContactPair<T>{pair.id_A,
pair.id_B,
p_WC,
pair.nhat_BA_W,
phi0,
fn0,
k,
d,
tau,
mu,
{} /* no surface index */,
{} /* no face index */});
}
}
}
Expand All @@ -441,7 +458,7 @@ template <>
void CompliantContactManager<symbolic::Expression>::
AppendDiscreteContactPairsForHydroelasticContact(
const drake::systems::Context<symbolic::Expression>&,
std::vector<DiscreteContactPair<symbolic::Expression>>*) const {
DiscreteContactData<DiscreteContactPair<symbolic::Expression>>*) const {
throw std::domain_error(
fmt::format("This method doesn't support T = {}.",
NiceTypeName::Get<symbolic::Expression>()));
Expand All @@ -451,8 +468,8 @@ template <typename T>
void CompliantContactManager<T>::
AppendDiscreteContactPairsForHydroelasticContact(
const systems::Context<T>& context,
std::vector<DiscreteContactPair<T>>* result) const {
std::vector<DiscreteContactPair<T>>& contact_pairs = *result;
DiscreteContactData<DiscreteContactPair<T>>* result) const {
DiscreteContactData<DiscreteContactPair<T>>& contact_pairs = *result;

// N.B. For discrete hydro we use a first order quadrature rule. As such,
// the per-face quadrature point is the face's centroid and the weight is 1.
Expand Down Expand Up @@ -598,8 +615,9 @@ void CompliantContactManager<T>::
const T phi0 = -p0 / g;

if (k > 0) {
contact_pairs.push_back({s.id_M(), s.id_N(), p_WQ, nhat_W, phi0,
fn0, k, d, tau, mu, surface_index, face});
contact_pairs.AppendHydroData(DiscreteContactPair<T>{
s.id_M(), s.id_N(), p_WQ, nhat_W, phi0, fn0, k, d, tau, mu,
surface_index, face});
}
}
}
Expand Down Expand Up @@ -674,12 +692,12 @@ void CompliantContactManager<T>::CalcAccelerationsDueToNonConstraintForcesCache(
}

template <typename T>
const std::vector<DiscreteContactPair<T>>&
const DiscreteContactData<DiscreteContactPair<T>>&
CompliantContactManager<T>::EvalDiscreteContactPairs(
const systems::Context<T>& context) const {
return plant()
.get_cache_entry(cache_indexes_.discrete_contact_pairs)
.template Eval<std::vector<DiscreteContactPair<T>>>(context);
.template Eval<DiscreteContactData<DiscreteContactPair<T>>>(context);
}

template <typename T>
Expand Down Expand Up @@ -753,9 +771,9 @@ void CompliantContactManager<T>::AppendContactResultsForPointContact(

const std::vector<PenetrationAsPointPair<T>>& point_pairs =
plant().EvalPointPairPenetrations(context);
const std::vector<internal::DiscreteContactPair<T>>& discrete_pairs =
const DiscreteContactData<DiscreteContactPair<T>>& discrete_pairs =
this->EvalDiscreteContactPairs(context);
const std::vector<ContactPairKinematics<T>>& contact_kinematics =
const DiscreteContactData<ContactPairKinematics<T>>& contact_kinematics =
this->EvalContactKinematics(context);
const contact_solvers::internal::ContactSolverResults<T>& solver_results =
this->EvalContactSolverResults(context);
Expand All @@ -765,26 +783,13 @@ void CompliantContactManager<T>::AppendContactResultsForPointContact(
const VectorX<T>& vt = solver_results.vt;
const VectorX<T>& vn = solver_results.vn;

// N.B. Contact between two fully locked trees are filtered out of discrete
// pairs, therefore for purely point contact:
// num_point_contacts != plant().EvalPointPairPenetrations(context).size()
// We find the index of the first non-point contact, whose value corresponds
// to num_point_contacts.
auto itr = std::find_if(discrete_pairs.begin(), discrete_pairs.end(),
[](const auto& pair) {
return pair.surface_index.has_value();
});
const int num_point_contacts = std::distance(discrete_pairs.begin(), itr);
const int num_point_contacts = discrete_pairs.num_point_contacts();

DRAKE_DEMAND(fn.size() >= num_point_contacts);
DRAKE_DEMAND(ft.size() >= 2 * num_point_contacts);
DRAKE_DEMAND(vn.size() >= num_point_contacts);
DRAKE_DEMAND(vt.size() >= 2 * num_point_contacts);

// The correspondence between `discrete_pairs` and `point_pairs` depends on
// strict ordering of CompliantContactManager::CalcDiscreteContactPairs.
// All point contacts must come first and their order in discrete_pairs
// corresponds to their order in point_pairs.
for (int icontact = 0; icontact < num_point_contacts; ++icontact) {
const auto& discrete_pair = discrete_pairs[icontact];
const auto& point_pair = point_pairs[icontact];
Expand Down Expand Up @@ -846,9 +851,9 @@ void CompliantContactManager<T>::CalcHydroelasticContactInfo(
contact_info->clear();
contact_info->reserve(all_surfaces.size());

const std::vector<internal::DiscreteContactPair<T>>& discrete_pairs =
const DiscreteContactData<DiscreteContactPair<T>>& discrete_pairs =
this->EvalDiscreteContactPairs(context);
const std::vector<ContactPairKinematics<T>>& contact_kinematics =
const DiscreteContactData<ContactPairKinematics<T>>& contact_kinematics =
this->EvalContactKinematics(context);

const contact_solvers::internal::ContactSolverResults<T>& solver_results =
Expand All @@ -866,14 +871,8 @@ void CompliantContactManager<T>::CalcHydroelasticContactInfo(
DRAKE_DEMAND(vn.size() == num_contacts);
DRAKE_DEMAND(vt.size() == 2 * num_contacts);

// If the point contact model is used, hydroelastic contact pairs are appended
// after point contact pairs. The index of the first discrete pair with a
// valid surface index is the first non-point contact pair.
auto itr = std::find_if(discrete_pairs.begin(), discrete_pairs.end(),
[](const auto& pair) {
return pair.surface_index.has_value();
});
const int num_point_contacts = std::distance(discrete_pairs.begin(), itr);
const int num_point_contacts = discrete_pairs.num_point_contacts();
const int num_hydro_contacts = discrete_pairs.num_hydro_contacts();
const int num_surfaces = all_surfaces.size();

std::vector<SpatialForce<T>> F_Ao_W_per_surface(num_surfaces,
Expand All @@ -888,7 +887,8 @@ void CompliantContactManager<T>::CalcHydroelasticContactInfo(
// We only scan discrete pairs corresponding to hydroelastic quadrature
// points. These are appended by CalcDiscreteContactPairs() at the end of the
// point contact forces.
for (int icontact = num_point_contacts; icontact < num_contacts; ++icontact) {
for (int icontact = num_point_contacts;
icontact < num_point_contacts + num_hydro_contacts; ++icontact) {
const auto& pair = discrete_pairs[icontact];
// Quadrature point Q.
const Vector3<T>& p_WQ = pair.p_WC;
Expand Down
Loading

0 comments on commit d2bcf2d

Please sign in to comment.