对 pydrake 中的 object 施加外力

Question

这个问题与

密切相关

我希望能够对 pydrake 中的简单几何图元施加外力。这是为了对身体之间的相互作用进行评估。

我当前的实现：


builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.001))
parser = Parser(plant)
cube_instance = parser.AddModelFromFile('cube.urdf', model_name='cube')

plant.Finalize()

force = builder.AddSystem(ConstantVectorSource(np.zeros(6)))
builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())

diagram = builder.Build()

但是当我运行它时，我得到以下错误：

builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())
RuntimeError: DiagramBuilder::Connect: Cannot mix vector-valued and abstract-valued ports while connecting output port y0 of System drake/systems/ConstantVectorSource@0000000002db5aa0 to input port applied_spatial_force of System drake/multibody/MultibodyPlant@0000000003118680

我倾向于实现一个 LeafSystem，它在工厂上实现 abstract-value 端口。

根据建议更新

使用：AbstractValue 和 ConstantValueSource

value = AbstractValue.Make([np.zeros(6)])
force = builder.AddSystem(ConstantValueSource(value))

ref_vector_externally_applied_spatial_force = plant.get_applied_spatial_force_input_port()
builder.Connect(force.get_output_port(0), ref_vector_externally_applied_spatial_force)

我收到以下错误：

RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource@0000000002533a30 (type pybind11::object) to
input port applied_spatial_force of System drake/multibody/MultibodyPlant@0000000002667760 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)

input-output 端口的类型应该匹配才有意义。预期的类型似乎是 ExternallyAppliedSpatialForce 的向量。

然后我将抽象类型更改如下：

value = AbstractValue.Make(ExternallyAppliedSpatialForce())

RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource@0000000002623980 (type drake::multibody::ExternallyAppliedSpatialForce<double>)
to input port applied_spatial_force of System drake/multibody/MultibodyPlant@00000000027576b0 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)

我越来越近了。但是，我无法发送 ExternallyAppliedSpatialForce 的矢量。如果我尝试将它作为列表发送，我会收到一个投诉，说它无法 pickle object。我没有在 AbstractValue 示例中看到如何创建这样一个 objects.

的向量

如有任何额外帮助，我们将不胜感激。

解决方案是使用类型 VectorExternallyAppliedSpatialForced

稍后发布完整解决方案。

Answer 1

无需实现 LeafSystem — 我们有一个类似于 ConstantVectorSource 的 ConstantValueSource，但用于抽象类型。 https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_constant_value_source.html

你是对的，你需要这个端口的抽象类型：https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#ab2ad1faa7547d440f008cdddd32d85e8

可以在此处找到使用 python 中的抽象值的一些示例：https://github.com/RobotLocomotion/drake/blob/d6133a04/bindings/pydrake/systems/test/value_test.py#L84

Answer 2

下面是一个向刚性物体施加外部静力的工作示例：

sim_time_step = 0.001
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, sim_time_step)
object_instance = Parser(plant).AddModelFromFile('box.urdf')
scene_graph.AddRenderer("renderer", MakeRenderEngineVtk(RenderEngineVtkParams()))
ConnectDrakeVisualizer(builder, scene_graph)

plant.Finalize()

force_object = ExternallyAppliedSpatialForce()
force_object.body_index = plant.GetBodyIndices(object_instance).pop()
force_object.F_Bq_W = SpatialForce(tau=np.zeros(3), f=np.array([0., 0., 10.]))

forces = VectorExternallyAppliedSpatialForced()
forces.append(force_object)

value = AbstractValue.Make(forces)
force_system = builder.AddSystem(ConstantValueSource(value))

builder.Connect(force_system.get_output_port(0), plant.get_applied_spatial_force_input_port())

diagram = builder.Build()
simulator = Simulator(diagram)

context = simulator.get_mutable_context()

plant.SetPositions(context, object_instance, [0, 0, 0, 1, 0, 0, 0])

time_ = 0
while True:
    time_ += sim_time_step
    simulator.AdvanceTo(time_)
    time.sleep(sim_time_step)

但是，后来我无法在模拟循环中更改外加力。

为了实现这一点，我必须制作一个 LeafSystem。

可以找到允许您随时间改变施加到刚性对象上的力的实现 here

静态和动态示例都可以找到here。

对 pydrake 中的 object 施加外力

Applying an external force to an object in pydrake

drake

根据建议更新