PyDrake：自由浮动仿真中的质量中心正在移动而没有施加任何外部力

问题描述

据我了解，在不施加任何外力（例如重力）的情况下，如果仅施加内部扭矩（在关节处），则多体系统的质心应保持恒定。

在PyDrake中，我设置了一个装有6自由度臂的盒子飞船的自由浮动仿真。首先，我使用以下命令为第一个关节设置初始速度：

plant.GetJointByName("Joint_1").set_angular_rate(plant_context,-2.5)

正如预期的那样，整个多体系统正在基于惯性特性而滚动。但是，在观察网状猫仿真时，航天器系统似乎偏离了初始位置。我使用以下命令检查了系统重心位置：

print(plant.CalcCenterOfMassposition(plant_context))

实际上，该命令在模拟的开始和结束时返回不同的位置向量。据我了解，这不会发生，因为除了初始角速率外，没有任何力施加到系统上，初始角速率仅应将角动量引入系统，而不是线性动量，因此不会引起CoM的平移，但仿真与此不同。>

我想知道这是正在使用的积分器的人工产物还是德雷克（Dreak）中缺少或设置不正确的其他现象。

可以在这里找到urdf文件的Pastebin：https://pastebin.com/rUVRkBBf

我正在运行的pydrake脚本在这里：

builder = DiagramBuilder()

# Adds both MultibodyPlant and the SceneGraph,and wires them together.
plant,scene_graph = AddMultibodyPlantSceneGraph(builder,time_step=0)
# Note that we parse into both the plant and the scene_graph here.
Parser(plant,scene_graph).AddModelFromFile(flairsCpath)
# Don't Weld Frame for Free-Floating Bodies.
# plant.WeldFrames(plant.world_frame(),plant.GetFrameByName("Base_SC"))
# Reduce/remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder,scene_graph,zmq_url=zmq_url,delete_prefix_on_load=True)

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromroot(context)
# plant.SetPositions(plant_context,[0,0])
plant.get_actuation_input_port().FixValue(plant_context,np.zeros(6))
# plant.GetJointByName("Joint_1").set_angular_rate(context,-2.5)
plant.GetJointByName("Joint_1").set_angular_rate(plant_context,-2.5)

# Print CoM Value at the Start
print("CoM Location Before Simulation:")
print(plant.CalcCenterOfMassposition(plant_context))

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

meshcat.load()
# MakeJointSlidersThatPublishOnCallback(plant,meshcat,context);

# meshcat.start_recording()
simulator.Advanceto(60.0 if running_as_notebook else 0.1)
# meshcat.stop_recording()
# meshcat.publish_recording()
print("CoM Location After Simulation")
print(plant.CalcCenterOfMassposition(plant_context))

即使为积分器设置了不同的时间步，CoM的运动仍然存在。我担心的是，系统CoM不应在没有施加任何力的情况下进行平移，但是它确实会平移，这意味着我可能会错误地设置了仿真。

解决方法

还有另一项检查，确定系统上没有外力作用。 有一个C ++方法（与Python类似），其签名类似于：

SpatialMomentum<T> MultibodyPlant::CalcSpatialMomentumInWorldAboutPoint(
      const systems::Context<T>& context,const Vector3<T>& p_WoP_W)

如果系统上的净外力为零，则返回的空间动量的.translational（）分量应保持恒定。不管您为p_WoP_W传递了什么，都是如此。

如果系统上没有外力（因此系统上的净外部力矩为零），则只要p_WoP_W为p_WoScm_W（来自世界的位置矢量），返回的空间动量的.rotational（）分量应保持恒定。从原点Wo到系统质心Scm（用世界W表示）或零向量。

,

这里是我的速写本，以防万一。我已经确认，设置非常小的目标精度会产生相同的数字，将初始角速度设置得较低，以便看起来更合理，并确认重力已正确归零，等等。

import numpy as np
import pydrake.all
import pydot

builder = pydrake.systems.framework.DiagramBuilder()

# Adds both MultibodyPlant and the SceneGraph,and wires them together.
plant,scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder,time_step=0)
# Note that we parse into both the plant and the scene_graph here.
flair = pydrake.multibody.parsing.Parser(plant,scene_graph).AddModelFromFile("/home/russt/Downloads/flair.urdf")
# Don't Weld Frame for Free-Floating Bodies.
# plant.WeldFrames(plant.world_frame(),plant.GetFrameByName("Base_SC"))
# Reduce/remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(builder,scene_graph,zmq_url="tcp://127.0.0.1:6000",delete_prefix_on_load=True)


diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
# plant.SetPositions(plant_context,[0,0])
plant.get_actuation_input_port().FixValue(plant_context,np.zeros(6))
plant.GetJointByName("Joint_1").set_angular_rate(plant_context,-.25)
plant.get_actuation_input_port().FixValue(plant_context,np.zeros(6))

pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("flair_topology.svg")

simulator = pydrake.systems.analysis.Simulator(diagram,context)
simulator.get_mutable_integrator().set_target_accuracy(1e-10)

# Print CoM Value at the Start
print("CoM Location Before Simulation:")
print(plant.CalcCenterOfMassPosition(plant_context,[flair]))

simulator.AdvanceTo(60.0)
print("CoM Location After Simulation")
print(plant.CalcCenterOfMassPosition(plant_context,[flair]))

,

因此，最后，答案的确是从评论到问题。 set_angular_rate（）函数的确设置了两个物体之间的初始角速度，但在强制执行此约束的同时，还会引起质心的初始速度。移动关节以快速测试自由浮动系统的更好方法是使用get_actuation_input_port（）。FixValue（）函数。下面给出了一个简单的示例（没有CoM移动）：

builder = DiagramBuilder()

# Adds both MultibodyPlant and the SceneGraph,scene_graph = AddMultibodyPlantSceneGraph(builder,time_step=0)
# Note that we parse into both the plant and the scene_graph here.
Parser(plant,scene_graph).AddModelFromFile(flairSCpath)
# Don't Weld Frame for Free-Floating Bodies.
# plant.WeldFrames(plant.world_frame(),0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder,zmq_url=zmq_url,delete_prefix_on_load=True)

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
# plant.SetPositions(plant_context,0])
jointAcutation =  np.zeros((6,1))
jointAcutation[0] = 1
plant.get_actuation_input_port().FixValue(plant_context,jointAcutation)

# Print CoM Value at the Start
print("CoM Location Before Simulation:")
print(plant.CalcCenterOfMassPosition(plant_context))

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

meshcat.load()
# MakeJointSlidersThatPublishOnCallback(plant,meshcat,context);

# meshcat.start_recording()
simulator.AdvanceTo(60.0 if running_as_notebook else 0.1)
# meshcat.stop_recording()
# meshcat.publish_recording()
print("CoM Location After Simulation")
print(plant.CalcCenterOfMassPosition(plant_context))

感谢Russ Tedrake和Sherm帮助我（以及希望将来的用户）理解这一点。