在 Drake 中建模力/扭矩传感器的问题

问题描述

我在将 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,-1.2,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()

解决方法

您拥有的是一个代数环。阅读此内容的一个好地方可能是 here，尽管该概念是一个一般性的概念，您可以在有关动态系统的其他参考资料中找到（谷歌搜索代数环了解更多）。另一个阅读here。最后，可能也是首先，我建议阅读 Drake 中错误消息本身提供的链接，this one。

在 Drake 中，拥有代数环通常（总是？）意味着一个糟糕的建模决策。它只意味着两件事之一：

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

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

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

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

同样，这些不仅是技巧，而且如果您仔细考虑一下，它们确实是正确的做法。

希望这会有所帮助。

drake

在 Drake 中建模力/扭矩传感器的问题

问题描述

解决方法

相关问答