Skip to content

Commit

Permalink
ContactResults report deformable contact results (RobotLocomotion#20274)
Browse files Browse the repository at this point in the history 
The contact result computation is also moved from
CompliantContactManager to DiscreteUpdateManager
  • Loading branch information
@xuchenhan-tri
xuchenhan-tri authored Oct 18, 2023
1 parent bdcd24b commit 9d10d31
Show file tree
Hide file tree
Showing 14 changed files with 550 additions and 375 deletions.
244 changes: 0 additions & 244 deletions multibody/plant/compliant_contact_manager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,18 +87,6 @@ void CompliantContactManager<T>::DoDeclareCacheEntries() {
// Therefore if we make it dependent on q_ticket() the Jacobian only
// gets evaluated once at the start of the simulation.

// Cache hydroelastic contact info.
const auto& hydroelastic_contact_info_cache_entry = this->DeclareCacheEntry(
"Hydroelastic contact info.",
systems::ValueProducer(
this, &CompliantContactManager<T>::CalcHydroelasticContactInfo),
// Compliant contact forces due to hydroelastics with Hunt &
// Crosseley are function of the kinematic variables q & v only.
{systems::System<T>::xd_ticket(),
systems::System<T>::all_parameters_ticket()});
cache_indexes_.hydroelastic_contact_info =
hydroelastic_contact_info_cache_entry.cache_index();

// Accelerations due to non-constraint forces.
// We cache non-contact forces, ABA forces and accelerations into an
// AccelerationsDueNonConstraintForcesCache.
Expand Down Expand Up @@ -223,238 +211,6 @@ void CompliantContactManager<T>::DoCalcContactSolverResults(
}
}

template <typename T>
void CompliantContactManager<T>::AppendContactResultsForPointContact(
const drake::systems::Context<T>& context,
ContactResults<T>* contact_results) const {
DRAKE_DEMAND(contact_results != nullptr);

const std::vector<PenetrationAsPointPair<T>>& point_pairs =
plant().EvalPointPairPenetrations(context);
const DiscreteContactData<DiscreteContactPair<T>>& discrete_pairs =
this->EvalDiscreteContactPairs(context);
const DiscreteContactData<ContactPairKinematics<T>>& contact_kinematics =
this->EvalContactKinematics(context);
const contact_solvers::internal::ContactSolverResults<T>& solver_results =
this->EvalContactSolverResults(context);

const VectorX<T>& fn = solver_results.fn;
const VectorX<T>& ft = solver_results.ft;
const VectorX<T>& vt = solver_results.vt;
const VectorX<T>& vn = solver_results.vn;

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);

for (int icontact = 0; icontact < num_point_contacts; ++icontact) {
const auto& discrete_pair = discrete_pairs[icontact];
const auto& point_pair = point_pairs[icontact];

const GeometryId geometryA_id = discrete_pair.id_A;
const GeometryId geometryB_id = discrete_pair.id_B;

const BodyIndex bodyA_index = this->FindBodyByGeometryId(geometryA_id);
const BodyIndex bodyB_index = this->FindBodyByGeometryId(geometryB_id);

const RotationMatrix<T>& R_WC =
contact_kinematics[icontact].configuration.R_WC;

// Contact forces applied on B at contact point C.
const Vector3<T> f_Bc_C(ft(2 * icontact), ft(2 * icontact + 1),
fn(icontact));
const Vector3<T> f_Bc_W = R_WC * f_Bc_C;

// Slip velocity.
const T slip = vt.template segment<2>(2 * icontact).norm();

// Separation velocity in the normal direction.
const T separation_velocity = vn(icontact);

// Add pair info to the contact results.
contact_results->AddContactInfo({bodyA_index, bodyB_index, f_Bc_W,
discrete_pair.p_WC, separation_velocity,
slip, point_pair});
}
}

template <typename T>
void CompliantContactManager<T>::AppendContactResultsForHydroelasticContact(
const drake::systems::Context<T>& context,
ContactResults<T>* contact_results) const {
const std::vector<HydroelasticContactInfo<T>>& contact_info =
this->EvalHydroelasticContactInfo(context);

for (const HydroelasticContactInfo<T>& info : contact_info) {
// Note: caching dependencies guarantee that the lifetime of `info` is
// valid for the lifetime of the contact results.
contact_results->AddContactInfo(&info);
}
}

template <typename T>
void CompliantContactManager<T>::CalcHydroelasticContactInfo(
const systems::Context<T>& context,
std::vector<HydroelasticContactInfo<T>>* contact_info) const {
DRAKE_DEMAND(contact_info != nullptr);

const std::vector<ContactSurface<T>>& all_surfaces =
this->EvalContactSurfaces(context);

// Reserve memory here to keep from repeatedly allocating heap storage in
// the loop below.
// TODO(joemasterjohn): Consider caching this vector and the quadrature
// point data vectors to avoid this dynamic allocation.
contact_info->clear();
contact_info->reserve(all_surfaces.size());

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

const contact_solvers::internal::ContactSolverResults<T>& solver_results =
this->EvalContactSolverResults(context);

const VectorX<T>& fn = solver_results.fn;
const VectorX<T>& ft = solver_results.ft;
const VectorX<T>& vt = solver_results.vt;
const VectorX<T>& vn = solver_results.vn;

// Discrete pairs contain both point and hydro contact force results.
const int num_contacts = discrete_pairs.size();
DRAKE_DEMAND(fn.size() == num_contacts);
DRAKE_DEMAND(ft.size() == 2 * num_contacts);
DRAKE_DEMAND(vn.size() == num_contacts);
DRAKE_DEMAND(vt.size() == 2 * num_contacts);

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,
SpatialForce<T>::Zero());

std::vector<std::vector<HydroelasticQuadraturePointData<T>>> quadrature_data(
num_surfaces);
for (int isurface = 0; isurface < num_surfaces; ++isurface) {
quadrature_data[isurface].reserve(all_surfaces[isurface].num_faces());
}

// 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_point_contacts + num_hydro_contacts; ++icontact) {
const auto& pair = discrete_pairs[icontact];
// Quadrature point Q.
const Vector3<T>& p_WQ = pair.p_WC;
const RotationMatrix<T>& R_WC =
contact_kinematics[icontact].configuration.R_WC;

// Contact forces applied on B at quadrature point Q expressed in the
// contact frame.
const Vector3<T> f_Bq_C(ft(2 * icontact), ft(2 * icontact + 1),
fn(icontact));
// Contact force applied on A at quadrature point Q expressed in the world
// frame.
const Vector3<T> f_Aq_W = -(R_WC * f_Bq_C);

const int surface_index = pair.surface_index.value();
const auto& s = all_surfaces[surface_index];
// Surface's centroid point O.
const Vector3<T>& p_WO = s.is_triangle() ? s.tri_mesh_W().centroid()
: s.poly_mesh_W().centroid();

// Spatial force
const Vector3<T> p_QO_W = p_WO - p_WQ;
const SpatialForce<T> Fq_Ao_W =
SpatialForce<T>(Vector3<T>::Zero(), f_Aq_W).Shift(p_QO_W);
// Accumulate force for the corresponding contact surface.
F_Ao_W_per_surface[surface_index] += Fq_Ao_W;

// Velocity of Aq relative to Bq in the tangent direction.
// N.B. DiscreteUpdateManager<T>::CalcContactKinematics() uses the
// convention of computing J_AcBc_C and thus J_AcBc_C * v = v_AcBc_W (i.e.
// relative velocity of Bc with respect to Ac). Thus we flip the sign here
// for the convention used by HydroelasticQuadratureData.
const Vector3<T> vt_BqAq_C(-vt(2 * icontact), -vt(2 * icontact + 1), 0);
const Vector3<T> vt_BqAq_W = R_WC * vt_BqAq_C;

// Traction vector applied to body A at point Aq (Aq and Bq are coincident)
// expressed in the world frame.
const int face_index = pair.face_index.value();
const Vector3<T> traction_Aq_W = f_Aq_W / s.area(face_index);

quadrature_data[surface_index].emplace_back(p_WQ, face_index, vt_BqAq_W,
traction_Aq_W);
}

const MultibodyTreeTopology& topology = this->internal_tree().get_topology();
const std::vector<std::vector<int>>& per_tree_unlocked_indices =
this->EvalJointLockingCache(context).unlocked_velocity_indices_per_tree;

// Update contact info to include the correct contact forces.
for (int surface_index = 0; surface_index < num_surfaces; ++surface_index) {
BodyIndex bodyA_index = this->geometry_id_to_body_index().at(
all_surfaces[surface_index].id_M());
BodyIndex bodyB_index = this->geometry_id_to_body_index().at(
all_surfaces[surface_index].id_N());

const TreeIndex& treeA_index = topology.body_to_tree_index(bodyA_index);
const TreeIndex& treeB_index = topology.body_to_tree_index(bodyB_index);

// For joint locking, filter out contacts between bodies who belong to
// trees with 0 degrees of freedom. For a contact to remain in
// consideration, at least one of the trees involved has to be valid and
// have a non-zero number of DOFs.
if ((treeA_index.is_valid() &&
per_tree_unlocked_indices[treeA_index].size() != 0) ||
(treeB_index.is_valid() &&
per_tree_unlocked_indices[treeB_index].size() != 0)) {
contact_info->emplace_back(&all_surfaces[surface_index],
F_Ao_W_per_surface[surface_index],
std::move(quadrature_data[surface_index]));
}
}
}

template <typename T>
const std::vector<HydroelasticContactInfo<T>>&
CompliantContactManager<T>::EvalHydroelasticContactInfo(
const systems::Context<T>& context) const {
return plant()
.get_cache_entry(cache_indexes_.hydroelastic_contact_info)
.template Eval<std::vector<HydroelasticContactInfo<T>>>(context);
}

template <typename T>
void CompliantContactManager<T>::DoCalcContactResults(
const drake::systems::Context<T>& context,
ContactResults<T>* contact_results) const {
DRAKE_DEMAND(contact_results != nullptr);
contact_results->Clear();
contact_results->set_plant(&plant());

if (plant().num_collision_geometries() == 0) return;

switch (plant().get_contact_model()) {
case ContactModel::kPoint:
AppendContactResultsForPointContact(context, contact_results);
break;
case ContactModel::kHydroelastic:
AppendContactResultsForHydroelasticContact(context, contact_results);
break;
case ContactModel::kHydroelasticWithFallback:
AppendContactResultsForPointContact(context, contact_results);
AppendContactResultsForHydroelasticContact(context, contact_results);
break;
}
}

// TODO(xuchenhan-tri): Consider a scalar converting constructor to cut down
// repeated code in CloneToDouble() and CloneToAutoDiffXd().
template <typename T>
Expand Down
29 changes: 0 additions & 29 deletions multibody/plant/compliant_contact_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/geometry/scene_graph_inspector.h"
#include "drake/multibody/plant/contact_results.h"
#include "drake/multibody/plant/deformable_driver.h"
#include "drake/multibody/plant/discrete_update_manager.h"
#include "drake/systems/framework/context.h"
Expand Down Expand Up @@ -116,7 +115,6 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
// Struct used to conglomerate the indexes of cache entries declared by the
// manager.
struct CacheIndexes {
systems::CacheIndex hydroelastic_contact_info;
systems::CacheIndex non_constraint_forces_accelerations;
};

Expand Down Expand Up @@ -157,23 +155,10 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
void DoCalcAccelerationKinematicsCache(
const systems::Context<T>&,
multibody::internal::AccelerationKinematicsCache<T>*) const final;
void DoCalcContactResults(const systems::Context<T>&,
ContactResults<T>* contact_results) const final;
void DoCalcDiscreteUpdateMultibodyForces(
const systems::Context<T>& context,
MultibodyForces<T>* forces) const final;

// Computes per-face contact information for the hydroelastic model (slip
// velocity, traction, etc). On return contact_info->size() will equal the
// number of faces discretizing the contact surface.
void CalcHydroelasticContactInfo(
const systems::Context<T>& context,
std::vector<HydroelasticContactInfo<T>>* contact_info) const;

// Eval version of CalcHydroelasticContactInfo() .
const std::vector<HydroelasticContactInfo<T>>& EvalHydroelasticContactInfo(
const systems::Context<T>& context) const;

// Computes non-constraint forces and the accelerations they induce.
void CalcAccelerationsDueToNonConstraintForcesCache(
const systems::Context<T>& context,
Expand All @@ -185,20 +170,6 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
EvalAccelerationsDueToNonConstraintForcesCache(
const systems::Context<T>& context) const;

// Helper method to fill in contact_results with point contact information
// for the given state stored in `context`.
// @param[in,out] contact_results is appended to
void AppendContactResultsForPointContact(
const systems::Context<T>& context,
ContactResults<T>* contact_results) const;

// Helper method to fill in `contact_results` with hydroelastic contact
// information for the given state stored in `context`.
// @param[in,out] contact_results is appended to
void AppendContactResultsForHydroelasticContact(
const systems::Context<T>& context,
ContactResults<T>* contact_results) const;

CacheIndexes cache_indexes_;
// Vector of joint damping coefficients, of size plant().num_velocities().
// This information is extracted during the call to ExtractModelInfo().
Expand Down
Loading

0 comments on commit 9d10d31

Please sign in to comment.