Is there a way to get collision results in the context of the plant?-CodePudding

I'm a bit unfamiliar with Drake and I've been trying to write a function to return the body indices involved in collisions in my MultibodyPlant. I'm having some trouble getting the correct collision results. I'm currently trying to get the contact results from the plant using a LeafSystem:

class ContactResultsReader : public drake::systems::LeafSystem<double> {
public:

    ContactResultsReader() : drake::systems::LeafSystem<double>() {
        contact_results_input_port = &DeclareAbstractInputPort("contact_results_input_port", *drake::AbstractValue::Make<drake::multibody::ContactResults<double>>());
    };

    drake::systems::InputPort<double> & get_contact_results_input_port() {
        return *contact_results_input_port;
    }

private:
    drake::systems::InputPort<double> *contact_results_input_port = nullptr;
};

connecting the contact results output port into this

builder->Connect(plant->get_contact_results_output_port(), contact_results_reader->get_contact_results_input_port());

and reading the contact results from the input port in the LeafSystem:

const drake::systems::InputPortIndex contact_results_input_port_index = contact_results_reader->get_contact_results_input_port().get_index();
    const drake::AbstractValue* abstract_value = 
        contact_results_reader->EvalAbstractInput(*contact_results_reader_context, contact_results_input_port_index);
    const drake::multibody::ContactResults<double> &contact_results = abstract_value->get_value<drake::multibody::ContactResults<double>>();

For some reason, I'm not reading a collision between two bodies that I load into the plant directly on top of each other. Is there something wrong with this method?

I've also tried getting the QueryObject from the plant and using query_object.ComputePointPairPenetration();, but that returns the bodies themselves and I would like to get the body indices. Thank you!

CodePudding user response：

From some application code (e.g., not within your own custom LeafSystem), it would look like this:

Say we have a const drake::multibody::MultibodyPlant<double>& plant reference along with a const drake::systems::Context<double>& plant_context matching context reference. To access its contact results would be like so:

const drake::multibody::ContactResults<double>& contact_results =
  plant.get_contact_results_output_port().
    Eval<drake::multibody::ContactResults<double>>(context);
BodyIndex a = contact_results.point_pair_contact_info(...).bodyA_index();
BodyIndex b = contact_results.point_pair_contact_info(...).bodyB_index();

If you need to do it from within a LeafSystem function, then the code would be a mix of what you posted above, and this answer.

Something like

const drake::multibody::ContactResults<double>& contact_results =
  this->get_input_port(0).
    Eval<drake::multibody::ContactResults<double>>(context);
BodyIndex a = contact_results.point_pair_contact_info(...).bodyA_index();
BodyIndex b = contact_results.point_pair_contact_info(...).bodyB_index();