[drake in C++]: Declare an abstract input port in a controller to get contact result from multibody plant

Question

Recently, I am trying to construct a "BaseContoller" to control the motion of "Atlas" in "Drake".

To get the contact result of the multibody plant in the controller, I tried to declare an abstract input port in the controller leafsystem, which is to be connected to the contact results output port of the multibodyplant.

Then, in the Calc method of torque output port, I tried to get contact result from the abstract input port.

All the code can be compiled successfully, but during the runtime "Segmentation fault (core dumped)" occured. I wonder if I have missed something.

// Atlas.cpp

int do_main(){
  
  systems::DiagramBuilder<double> builder;
  auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(
      &builder,
      std::make_unique<MultibodyPlant<double>>(
          FLAGS_mbp_discrete_update_period));

  const std::string full_name = 
          FindResourceOrThrow("drake/../../../../home/hampton/Atlas/urdf/atlas.urdf");
  multibody::Parser(&plant).AddModelFromFile(full_name);
 
  // register the ground as visual geometry and collision geometry；
  ...

  plant.Finalize();
  plant.set_penetration_allowance(FLAGS_penetration_allowance);
  plant.set_stiction_tolerance(FLAGS_stiction_tolerance);

  // connect the controller;
  auto controller = builder.AddSystem<systems::controllers::BaseController<double>>(plant);
  builder.Connect(controller->get_output_port_control(),
                                      plant.get_applied_generalized_force_input_port());
  builder.Connect(plant.get_state_output_port(),
                                      controller->get_input_port_estimated_state());
  builder.Connect(plant.get_contact_results_output_port(),
                                      controller->get_input_port_contact_result());

  auto constant_zero_torque = builder.AddSystem<systems::ConstantVectorSource<double>>(
                Eigen::VectorXd::Zero(plant.num_actuators()));
  builder.Connect(constant_zero_torque->get_output_port(),
                                    plant.get_actuation_input_port());


ConnectContactResultsToDrakeVisualizer(&builder, plant, scene_graph);
geometry::DrakeVisualizerd::AddToBuilder(&builder, scene_graph);

auto diagram = builder.Build();


std::unique_ptr<systems::Context<double>> diagram_context =
         diagram->CreateDefaultContext();

// Do the initialization of context;
...

// Set the simulator;
auto simulator =
      MakeSimulatorFromGflags(*diagram, std::move(diagram_context));
  simulator->AdvanceTo(FLAGS_simulation_time);
  simulator->set_publish_every_time_step(true);

  return 0;
}

//BaseController.h

template <typename T>
class BaseController : public LeafSystem<T>{

public:

    DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BaseController);

    BaseController(
            const multibody::MultibodyPlant<T>& palnt);
    ~BaseController();

    const InputPort<T>& get_input_port_estimated_state() const {
        return this->System<T>::get_input_port(input_port_index_state_);
    }

    const OutputPort<T>& get_output_port_control() const{
        return this->System<T>::get_output_port(output_port_index_control_);
    }

    const InputPort<T>& get_input_port_contact_result() const {
        return this->System<T>::get_input_port(input_port_index_contact_result_);
    }
    
private:

   // Output Calc Function;
    void CalcOutputTorque(const Context<T>& context, 
                                                        BasicVector<T>* force) const;
    
   ...

    const multibody::MultibodyPlant<T>*  const multibody_plant_;

    InputPortIndex input_port_index_state_{0};
    OutputPortIndex output_port_index_control_{0};
    InputPortIndex input_port_index_contact_result_{0};

    const int q_dim_{0};
    const int v_dim_{0};

    drake::systems::CacheIndex multibody_plant_context_cache_index_;

};

// BaseController.cpp

template <typename T>
BaseController<T>::BaseController(
    const multibody::MultibodyPlant<T>& plant)
    :  multibody_plant_(&plant),
       q_dim_(plant.num_positions()),
       v_dim_(plant.num_velocities())  
{
    DRAKE_DEMAND(plant.is_finalized());

    input_port_index_state_ = 
        this->DeclareVectorInputPort(
            "Estimated State", 
            q_dim_ + v_dim_).get_index();

    output_port_index_control_ = 
        this->DeclareVectorOutputPort(
            "Control Torque", 
            v_dim_,
            &BaseController<T>::CalcOutputTorque).get_index();

    input_port_index_contact_result_ = 
        this->systems::LeafSystem<T>::DeclareAbstractInputPort(
            "Contact Result",
            Value<multibody::ContactResults<T>>()).get_index();
            // *AbstractValue::Make(multibody::ContactResults<T>())).get_index();
    
    auto multibody_plant_context = multibody_plant_-> 
                    System<double>::CreateDefaultContext();

    
    multibody_plant_context_cache_index_ =
      this->SystemBase::DeclareCacheEntry(
              "multibody_plant_context_cache",
              *multibody_plant_context,
              &BaseController<T>::SetMultibodyContext,
              {this->input_port_ticket(
                  get_input_port_estimated_state().get_index())})
          .cache_index();
}

...

template <typename T>
void BaseController<T>::CalcOutputTorque(
    const Context<T>& context,
    BasicVector<T>* force) const{
   ...
    const auto& contact_results =
        get_input_port_contact_result().template Eval<multibody::ContactResults<T>>(context);
    
    // const AbstractValue* abstract_value = 
    //             this->EvalAbstractInput(context, input_port_index_contact_result_);
    // const multibody::ContactResults<T>& contact_result_ = 
    //             abstract_value->get_value<multibody::ContactResults<T>>();
  
}

template class BaseController<double>;

Answer 1

I took your code snippets and created a buildable/runnable version of it on my side. I don't have the specific atlas URDF that you reference in your -- I used one of "ours" instead ("drake/examples/atlas/urdf/atlas_convex_hull.urdf"). The good news is it produced a segfault. (I don't know if it's the same segfault, but it produced one.)

When I rebuilt in debug and ran it through the debugger, I found the segfault came when computing the mass matrix (specifically, drake::multibody::internal::MultibodyTree<double>::CalcMassMatrix at multibody/tree/multibody_tree.cc:1513. That invocation is being triggered by BaseController::CalcOutputTorque().

Now, I don't know where your segfault is coming from. So, before investigating that particular segfault with my particular URDF, I'd recommend you do the same and report on where the segfault is happening.

-------- EDIT ------------------

Your code is pointing toward a defect in Drake. You've introduced an algbraic loop. The inputs of the plant depend on the output of the controller, but the input of the controller depends on the output of the plant. This scenario should be detected when you build the diagram, but it isn't. That's something for us to address.

The way to solve this is to put a system in between them where the loop is "broken". This has been discussed here and here. This second solution proposes a low-pass filter to serve as the break. You can also simply use a zero-order hold on the controller output (modeling the controllers real-world frequency).

Answer 2

Now, I figure out that getting the contact results from "lcm-system" can avoid this problem. When simulating the atlas motion, the contact results are published to lcm-system in the "CONTACT_RESULTS" channel with type "lcmt_contact_results_for_viz". So, besides getting from output port of plant, we can also get it from lcm system.

First，add a LcmInterfaceSystem and LcmSubscriberSystem which subscribes "lcmt_contact_results_for_viz" type message of "CONTACT_RESULTS" channel from lcm-system, and connect it to the contact result input port of the controller leafsystem.

// Atlas.cpp
...
#include "drake/systems/lcm/lcm_subscriber_system.h"
#include "drake/systems/lcm/lcm_interface_system.h"
...


auto lcm = builder.AddSystem<systems::lcm::LcmInterfaceSystem>();
auto contact_sub = builder.AddSystem(
    systems::lcm::LcmSubscriberSystem::Make<lcmt_contact_results_for_viz>("CONTACT_RESULTS", lcm));
builder.Connect(contact_sub->get_output_port(),
    IDC->get_input_port_contact_result());

Then, declare the abstract input port in the controller, getting "lcmt_contact_results_for_viz" abstract value.

// Controller.h
#include "drake/lcm/drake_lcm.h"
#include "drake/lcm/drake_interface_.h"
#include "drake_lcmtypes/drake/lcmt_contact_results_for_rviz.hpp"
#icnlude "drake_lcmtypes/drake/lcmt_point_pair_contact_info_for_viz.hpp"
...
// Controller.cpp

input_port_index_contact_result_ = 
  this->DeclareAbstractInputPort("Contact Results", 
  Value<lcmt_contact_results_for_viz>()).get_value();

Finally, decode the message from abstract input port.

const AbstractValue* abstract_value = 
     this->EvalAbstractInput(context, input_port_index_contact_result_);
const auto& contact_result = 
     abstract_value->get_value<lcmt_contact_results_for_viz>();
// Operate the message...