PyDrake：自由浮动模拟中的质心在没有施加任何（外部）力的情况下移动
PyDrake: Center of Mass in Free-Floating Simulation is moving wihtout any (External) Forces Applied
据我了解，在不施加任何外力（例如重力）的情况下，如果仅施加内部扭矩（在关节处），多体系统的质心应该是恒定的。
在 PyDrake 中，我设置了一个带有 6 DoF 臂的箱式航天器的自由漂浮模拟。一开始，我使用以下命令为第一个关节设置初始速度：
plant.GetJointByName("Joint_1").set_angular_rate(plant_context, -2.5)
不出所料，整个多体系统正在根据惯性属性进行翻滚。然而，在查看 meshcat 模拟时，航天器系统似乎偏离了初始位置。我使用以下命令检查了系统质量中心位置：
print(plant.CalcCenterOfMassPosition(plant_context))
实际上，命令 returns 在模拟的开始和结束时使用不同的位置向量。据我所知，这不应该发生，因为除了初始 angular 速率外，没有任何力施加到系统上，它应该只向系统引入 angular 动量而不是线性动量，因此没有 CoM 的平移但是模拟与此不同。
我想知道这是否是正在使用的积分器的产物，或者我遗漏或设置不正确的 drake 中的其他一些现象。
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,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, 0, 0, 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 不应该在没有施加任何力的情况下平移，但它会平移，这意味着我可能设置了错误的模拟。
这是我的快速复制，以防有帮助。我已经确认设置一个非常小的目标精度会产生相同的数字，将初始 angular 速度调低以使其看起来更合理，确认重力已正确归零，等等
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,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, 0, 0, 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() 函数确实设置了两个物体之间的初始 angular 速率，但在强制执行此约束的同时，它还会导致质心的初始速度。移动关节以快速测试 free-floating 系统的更好方法是改用 get_actuation_input_port().FixValue() 函数。下面给出了一个简单的例子（没有 CoM 运动）：
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,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, 0, 0, 0, 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 帮助我（希望未来的用户）理解这一点。
还有另外一项检查，确保没有外力作用在您的系统上。
有一个 C++ 方法（类似于 Python），其签名类似于：
SpatialMomentum<T> MultibodyPlant::CalcSpatialMomentumInWorldAboutPoint(
const systems::Context<T>& context,
const Vector3<T>& p_WoP_W)
如果系统上的净外力为零，则返回的空间动量的 .translational() 分量应保持不变。这是事实，无论您为 p_WoP_W.
传递什么
如果系统上没有外力（因此系统上的净外部力矩为零），只要 p_WoP_W 是，返回空间动量的 .rotational() 分量就应该保持不变p_WoScm_W（从世界原点Wo到系统质心Scm的位置向量，以世界W表示）或零向量。
据我了解，在不施加任何外力（例如重力）的情况下，如果仅施加内部扭矩（在关节处），多体系统的质心应该是恒定的。
在 PyDrake 中，我设置了一个带有 6 DoF 臂的箱式航天器的自由漂浮模拟。一开始，我使用以下命令为第一个关节设置初始速度：
plant.GetJointByName("Joint_1").set_angular_rate(plant_context, -2.5)
不出所料，整个多体系统正在根据惯性属性进行翻滚。然而，在查看 meshcat 模拟时，航天器系统似乎偏离了初始位置。我使用以下命令检查了系统质量中心位置：
print(plant.CalcCenterOfMassPosition(plant_context))
实际上，命令 returns 在模拟的开始和结束时使用不同的位置向量。据我所知，这不应该发生，因为除了初始 angular 速率外，没有任何力施加到系统上，它应该只向系统引入 angular 动量而不是线性动量，因此没有 CoM 的平移但是模拟与此不同。
我想知道这是否是正在使用的积分器的产物，或者我遗漏或设置不正确的 drake 中的其他一些现象。
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,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, 0, 0, 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 不应该在没有施加任何力的情况下平移，但它会平移，这意味着我可能设置了错误的模拟。
这是我的快速复制，以防有帮助。我已经确认设置一个非常小的目标精度会产生相同的数字，将初始 angular 速度调低以使其看起来更合理，确认重力已正确归零，等等
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,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, 0, 0, 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() 函数确实设置了两个物体之间的初始 angular 速率，但在强制执行此约束的同时，它还会导致质心的初始速度。移动关节以快速测试 free-floating 系统的更好方法是改用 get_actuation_input_port().FixValue() 函数。下面给出了一个简单的例子（没有 CoM 运动）：
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,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, 0, 0, 0, 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 帮助我（希望未来的用户）理解这一点。
还有另外一项检查，确保没有外力作用在您的系统上。 有一个 C++ 方法（类似于 Python），其签名类似于：
SpatialMomentum<T> MultibodyPlant::CalcSpatialMomentumInWorldAboutPoint(
const systems::Context<T>& context,
const Vector3<T>& p_WoP_W)
如果系统上的净外力为零，则返回的空间动量的 .translational() 分量应保持不变。这是事实，无论您为 p_WoP_W.
传递什么如果系统上没有外力（因此系统上的净外部力矩为零），只要 p_WoP_W 是，返回空间动量的 .rotational() 分量就应该保持不变p_WoScm_W（从世界原点Wo到系统质心Scm的位置向量，以世界W表示）或零向量。