在 PyDrake 中可视化 AutoDiff MultibodyPlant

问题描述

我正在尝试使用基本的 DrakeVisualizer 在 Drake 中构建一个简单的多体植物系统。但是，对于我的用例，我还希望能够通过物理模拟自动跟踪导数，因此我使用 autodiffXd 版本的系统：

timestep = 1e-3
builder = DiagramBuilder_[autodiffXd]()

plant = MultibodyPlant(timestep)
scene_graph = SceneGraph_[autodiffXd]()

brick_file = FindResourceOrThrow("drake/examples/manipulation_station/models/061_foam_brick.sdf")
parser = Parser(plant)
brick = parser.AddModelFromFile(brick_file,model_name="brick")

plant.Finalize()
plant_ad = plant.ToautodiffXd()
plant_ad.RegisterasSourceForSceneGraph(scene_graph)

scene_graph.AddRenderer("renderer",MakeRenderEngineVtk(RenderEngineVtkParams()))
DrakeVisualizer.AddToBuilder(builder,scene_graph)

builder.AddSystem(plant_ad)
builder.AddSystem(scene_graph)
builder.Connect(plant_ad.get_geometry_poses_output_port(),scene_graph.get_source_pose_port(plant_ad.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),plant_ad.get_geometry_query_input_port())

diagram = builder.Build()
context = diagram.CreateDefaultContext()
simulator = Simulator_[autodiffXd](diagram,context)
simulator.Advanceto(2.0)

但是，当我运行它时，出现以下错误：

  File "/home/craig/Repos/drake-exps/autodiffExperiment.py",line 102,in auto_phys
    DrakeVisualizer.AddToBuilder(builder,scene_graph)
TypeError: AddToBuilder(): incompatible function arguments. The following argument types are supported:
    1. (builder: pydrake.systems.framework.DiagramBuilder_[float],scene_graph: drake::geometry::SceneGraph<double>,lcm: pydrake.lcm.DrakeLcmInterface = None,params: pydrake.geometry.DrakeVisualizerParams = <pydrake.geometry.DrakeVisualizerParams object at 0x7ff6274e14b0>) -> pydrake.geometry.DrakeVisualizer
    2. (builder: pydrake.systems.framework.DiagramBuilder_[float],query_object_port: pydrake.systems.framework.OutputPort_[float],params: pydrake.geometry.DrakeVisualizerParams = <pydrake.geometry.DrakeVisualizerParams object at 0x7ff627736730>) -> pydrake.geometry.DrakeVisualizer

Invoked with: <pydrake.systems.framework.DiagramBuilder_[autodiffXd] object at 0x7ff65654f8f0>,<pydrake.geometry.SceneGraph_[autodiffXd] object at 0x7ff656562130>

从这个错误来看，DrakeVisualizer 类似乎只接受专门使用浮点标量的系统。所以我被卡住了——要么我可以回到浮动（但失去了我首先追求的 autodiff 可微模拟功能），要么继续使用 autodiffxd 系统（但完全无法想象我的模拟）。

有没有办法让我错过的两者都得到？

解决方法

很抱歉给您带来的痛苦和不便。你的描述和评估都是正确的。大多数可视化机制仅适用于 float，在当前状态下，尝试可视化 AutoDiff 图表将失败。

您有几个选择（都没有吸引力）：

1. 采用您在上面描述的结果之一（无可见性或无衍生）。
2. 提出 Drake 功能请求，以便能够将可视化工具附加到 AutoDiff 图表。

我可以想出一些变态的变通方法（即使不是很清楚也行得通）。因此，如果您迫切需要衍生品和可视化，可以探索它们。但是，最终，功能请求和正式的 Drake 解决方案将是最好的长期解决方案。

======================================

大更新。从 #14569 开始，DrakeVisualizer 类现在基于标量类型（上面列表中的第 2 项）进行了模板化。这有两个含义：

1. 您可以构建一个带有可视化工具的 AutoDiffXd 值图（如您的示例所示），或者
2. 您可以创建一个 double 值图并将其标量转换（即，diagram.ToAutoDiffXd() 为 AutoDiffXd 值图。