The issue with modelling the force/torque sensor in Drake

I came across the following error message when connecting get_reaction_forces_output_port() to my controller.

RuntimeError: Reported algebraic loop detected in DiagramBuilder:
      InputPort[0] (geometry_query) of System ::plant (MultibodyPlant<double>) is direct-feedthrough to
      OutputPort[15] (reaction_forces) of System ::plant (MultibodyPlant<double>) is connected to
      InputPort[0] (spatial_forces_in) of System ::__main__.MyController@00000000030a35e0 (__main__.MyController) is direct-feedthrough to
      OutputPort[0] (iiwa_torque) of System ::__main__.MyController@00000000030a35e0 (__main__.MyController) is connected to
      InputPort[3] (iiwa14_actuation) of System ::plant (MultibodyPlant<double>) is direct-feedthrough to
      InputPort[0] (geometry_query) of System ::plant (MultibodyPlant<double>)
    A System may have conservatively reported that one of its output ports depends on an input port, making one of the 'is direct-feedthrough to' lines above spurious.  If that is the case, remove the spurious dependency per the Drake API documentation for declaring output ports. https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_leaf_system.html#DeclareLeafOutputPort_feedthrough

This is my code snippet:

class MyController(LeafSystem):
    def __init__(self, plant):
        LeafSystem.__init__(self)
        self.plant = plant
        self.plant_context = plant.CreateDefaultContext()

        self.DeclareAbstractInputPort("spatial_forces_in", Value[List[SpatialForce]]()) 

        self.DeclareVectorOutputPort("iiwa_torque", BasicVector(7), self.CalcOutput)
        

    def CalcOutput(self, context, output):

        spatial_vec = self.get_input_port(0).Eval(context)
        sensor_joint_index = self.plant.GetJointByName("iiwa_joint_7").index()
        print(spatial_vec[sensor_joint_index].translational())

        tau = np.zeros(7)
        output.SetFromVector(tau)


builder = DiagramBuilder()

plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3)

Parser(plant, scene_graph).AddModelFromFile(
        FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
plant.Finalize()

meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)

controller = builder.AddSystem(MyController(plant))

builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port())
builder.Connect(plant.get_reaction_forces_output_port(), controller.get_input_port(0))

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0])
# plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7))

simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)

meshcat.start_recording()
simulator.AdvanceTo(5.0)
meshcat.stop_recording()
meshcat.publish_recording()

Solution

What you have is an algebraic loop. A good place where to read about this could be here though the concept is a general idea that you could find in other references on dynamical systems (google search algebraic loop for more). Another read here. Finally, and probably first, I recommend reading the link provided by the error message itself in Drake, this one.

In Drake, having an algebraic loop usually (always?) means a bad modeling decision. It'd only mean one of two things:

One of the systems has a feedthrough when it shouldn't. For instance, controllers usually work on state, and therefore they are not feedthrough.
The feedthrough is unavoidable because you really mean what you wrote. What that means in Drake is that you should not have two systems, but one that incorporates the entire algebraic model.

In practice things can get tricky though. In your case, it'd seem you want to use reaction forces to take an action in your controller. Now, your plant model is discrete. Recall this means that we do not have an ODE but an algebraic equation (though complex) describing the next state given the current state. When you add feedback using an algebraic model of actuation u as a function of reaction forces, what you did is to come up with a larger system of algebraic equations.

Now, think about your problem in a real robot. What you probably would do, is to measure reaction forces at time t1, to apply a control action at t2 != t1 (since reading from a sensor and computing your control action do take time). Therefore, there is an unavoidable lag in your control action. This could be modeled with a ZOH (zero order hold) which in Drake is systems::ZeroOrderHold. If you place that block right before or after your controller, you'll break the feedthrough (hopefully clear why after reading the references above?).

Another "trick", though motivated from real experience, would be having a low pass filter to remove quickly changing forces. Adding a low pass filter between plant and the controller to filter the reaction forces will also break the loop. In Drake you can add one with systems::FirstOrderLowPassFilter.

Again, these are not only tricks, but really the proper thing to do if you think about it.

Hopefully this helps.