求助：在Python中使用Drake的CalcPotentialEnergy()函数计算物体随时间变化的势能

我之前用过这个函数来计算多体系统的势能，下面是针对盒子模型的完整示例，能帮你实现随时间变化的势能计算：

步骤说明与代码示例 #

先搭建包含盒子的多体系统，再在仿真过程中逐帧调用CalcPotentialEnergy()：

from pydrake.multibody.plant import MultibodyPlant, AddMultibodyPlantSceneGraph
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.analysis import Simulator
from pydrake.geometry import Box, MeshcatVisualizerConfig, MeshcatVisualizer
import numpy as np

# 1. 构建多体系统与场景图
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)

# 添加盒子模型（质量1kg，边长0.5m）
box_instance = plant.AddRigidBody(
    "box",
    mass=1.0,
    spatial_inertia=plant.MakeSpatialInertiaFromCentralInertia(
        mass=1.0,
        p_PScm_E=np.array([0, 0, 0]),
        I_Scm_E=np.diag([0.01, 0.01, 0.01])
    )
)
# 添加盒子的几何形状
plant.RegisterVisualGeometry(
    box_instance,
    np.eye(4),
    Box(0.5, 0.5, 0.5),
    "box_visual",
    np.array([0.8, 0.2, 0.2, 1.0])  # 红色
)
plant.RegisterCollisionGeometry(
    box_instance,
    np.eye(4),
    Box(0.5, 0.5, 0.5),
    "box_collision",
    0.0  # 摩擦系数
)

# 设置重力（默认是-9.81m/s²沿z轴）
plant.mutable_gravity_field().set_gravity_vector(np.array([0, 0, -9.81]))
plant.Finalize()

# 2. 添加可视化（可选，方便观察盒子运动）
config = MeshcatVisualizerConfig(open_browser=False)
MeshcatVisualizer.AddToBuilder(builder, scene_graph, config)

# 3. 构建仿真器
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)

# 4. 设置盒子初始位置（比如z轴方向1m高处）
q0 = np.array([0, 0, 1.0, 1, 0, 0, 0])  # [x,y,z,qw,qx,qy,qz]
plant.SetPositions(plant_context, box_instance, q0)

# 5. 仿真并记录势能随时间的变化
time_steps = []
potential_energies = []
sim_duration = 2.0  # 仿真2秒
step_size = 0.01    # 每0.01秒记录一次数据

while context.get_time() < sim_duration:
    # 计算当前时刻的势能
    pe = plant.CalcPotentialEnergy(plant_context)
    time_steps.append(context.get_time())
    potential_energies.append(pe)
    
    # 推进仿真
    simulator.AdvanceTo(context.get_time() + step_size)

# 输出结果（也可以用matplotlib绘图）
print("时间(s) | 势能(J)")
for t, pe in zip(time_steps, potential_energies):
    print(f"{t:.2f}    | {pe:.3f}")

关键说明 #

• CalcPotentialEnergy()是MultibodyPlant类的方法（因MultibodyPlant继承自System），必须传入对应Context对象才能获取当前系统状态下的势能
• 势能计算基于系统设置的重力场和物体位置，盒子下落时势能会逐渐转化为动能，数值随之降低
• 如果需要自定义势能（比如弹簧势能），可以通过自定义LeafSystem并重写CalcPotentialEnergy()实现

内容的提问来源于stack exchange，提问作者Kenzhi Wong