在 Drake 中建模 force/torque 传感器的问题

Question

我在将 get_reaction_forces_output_port() 连接到我的控制器时遇到以下错误消息。

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

这是我的代码片段：

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

Answer 1

你得到的是一个代数环。 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.

是阅读此内容的好地方

在 Drake 中，代数环通常（总是？）意味着错误的建模决策。这只意味着两件事之一：

其中一个系统在不应该有馈通的情况下有馈通。例如，控制器通常在状态下工作，因此它们不是馈通。
馈通是不可避免的，因为你是认真的。这在 Drake 中意味着您不应该有两个系统，而应该有一个包含整个代数模型的系统。

在实践中，事情可能会变得棘手。在你的情况下，你似乎想使用反作用力在你的控制器中采取行动。现在，您的工厂模型是离散的。回想一下，这意味着我们没有 ODE，而是一个描述给定当前状态的下一个状态的代数方程（虽然很复杂）。当您使用驱动的代数模型 u 作为反作用力的函数添加反馈时，您所做的就是提出一个更大的代数方程组。

现在，想想你在真实机器人中遇到的问题。您可能会做的是在时间 t1 测量反作用力，在 t2 应用控制动作！= t1（因为从传感器读取并计算您的控制动作确实需要时间）。因此，您的控制动作不可避免地存在滞后。这可以用 ZOH（零阶保持）建模，在 Drake 中是 systems::ZeroOrderHold。如果将该块放置在控制器之前或之后，就会破坏馈通（希望在阅读上面的参考资料后清楚原因？）。

另一个“技巧”虽然是从实际经验中激发出来的，但它是使用低通滤波器来消除快速变化的力。在设备和控制器之间添加一个低通滤波器来过滤反作用力也会破坏环路。在 Drake 中，您可以添加一个 systems::FirstOrderLowPassFilter.

再说一次，这些不仅是技巧，而且如果您仔细想想，这确实是正确的做法。

希望这对您有所帮助。

在 Drake 中建模 force/torque 传感器的问题

The issue with modelling the force/torque sensor in Drake

drake