有没有办法在工厂环境中获得碰撞结果？

Question

我对 Drake 有点陌生，我一直在尝试为 return body 索引编写一个函数，该索引涉及我的 MultibodyPlant 中的碰撞。我在获得正确的碰撞结果时遇到了一些麻烦。我目前正在尝试使用 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;
};

将联系人结果输出端口连接到此

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

并从 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>>();

出于某种原因，我没有读到我加载到植物中的两个物体之间的碰撞，这两个物体直接相互重叠。这个方法有问题吗？

我也试过从工厂获取 QueryObject 并使用 query_object.ComputePointPairPenetration();，但是 return 是主体本身，我想获取 body 索引。谢谢！

Answer 1

从某些应用程序代码（例如，不在您自己的自定义 LeafSystem 中）来看，它看起来像这样：

假设我们有一个 const drake::multibody::MultibodyPlant<double>& plant 引用以及一个 const drake::systems::Context<double>& plant_context 匹配的上下文引用。要访问它的联系结果，就像这样：

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

如果您需要从 LeafSystem 函数中执行此操作，那么代码将是您上面发布的内容和此答案的混合。

类似

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

有没有办法在工厂环境中获得碰撞结果？

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

c++

drake