在Drake中如何可视化2D接触力？含PlanarSceneGraphVisualizer相关疑问

一、扩展PlanarSceneGraphVisualizer实现接触力投影 #

Drake的PlanarSceneGraphVisualizer本身没有内置接触力可视化功能，但可以通过添加自定义回调函数，在其可视化窗口中绘制接触力箭头。核心思路是监听ContactResults端口获取接触数据，再利用matplotlib的绘图API在同一坐标轴上添加力的可视化元素。

实现步骤与代码示例 #

from pydrake.all import (
    DiagramBuilder, PlanarSceneGraphVisualizer, Simulator,
    ContactResults, MultibodyPlant, SceneGraph, SpatialInertia, UnitInertia, RigidTransform
)

# 搭建仿真图
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0))
# 添加2D模型（示例：地面+立方体）
plant.AddGround()
box_instance = plant.AddRigidBody("box", SpatialInertia(1.0, [0,0,0], UnitInertia(1,1,1)))
plant.WeldFrames(plant.world_frame(), box_instance.body_frame(), RigidTransform([0, 0, 0.5]))

scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
plant.Finalize()

# 创建Planar可视化器
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0))

# 获取接触结果端口
contact_results_port = plant.get_contact_results_output_port()

# 存储接触力绘图元素，避免重复绘制
visualizer._contact_artists = []

# 定义接触力绘制回调
def draw_contacts(context, viz):
    # 清除上一帧的力箭头
    for artist in viz._contact_artists:
        artist.remove()
    viz._contact_artists.clear()

    contact_results = contact_results_port.Eval(context)
    ax = viz.axes()
    force_scale = 0.1  # 缩放因子，根据场景调整

    for i in range(contact_results.num_point_pair_contacts()):
        contact_info = contact_results.point_pair_contact_info(i)
        # 接触点的世界坐标
        p_WC = contact_info.contact_point()
        # 获取作用在被接触物体上的力（取反ContactResults中的力）
        f_W = -contact_info.contact_force()
        # 绘制红色箭头表示接触力
        arrow = ax.arrow(
            p_WC[0], p_WC[1],
            force_scale * f_W[0], force_scale * f_W[1],
            head_width=0.05, head_length=0.05, fc="red", ec="red"
        )
        viz._contact_artists.append(arrow)

# 给可视化器添加动画回调
visualizer.add_animate_callback(draw_contacts)

# 启动仿真
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.AdvanceTo(5.0)

二、Drake与Pyplot集成实现2D接触力可视化 #

如果需要完全自定义可视化逻辑，可以直接将Drake的仿真数据与Pyplot集成，手动绘制场景和接触力。

实现步骤与代码示例 #

from pydrake.all import (
    DiagramBuilder, Simulator, ContactResults, MultibodyPlant, SceneGraph,
    SpatialInertia, UnitInertia, RigidTransform
)
import matplotlib.pyplot as plt

# 搭建仿真图
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0))
# 添加2D模型（示例：地面+立方体）
plant.AddGround()
box_instance = plant.AddRigidBody("box", SpatialInertia(1.0, [0,0,0], UnitInertia(1,1,1)))
plant.WeldFrames(plant.world_frame(), box_instance.body_frame(), RigidTransform([0, 0, 0.5]))

scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
plant.Finalize()

# 获取关键端口
contact_results_port = plant.get_contact_results_output_port()
pose_bundle_port = scene_graph.get_pose_bundle_output_port()

diagram = builder.Build()
context = diagram.CreateDefaultContext()
simulator = Simulator(diagram, context)

# 设置Pyplot画布
fig, ax = plt.subplots(figsize=(8, 6))
ax.set_aspect("equal")
ax.set_xlim(-1.5, 1.5)
ax.set_ylim(-0.5, 2.0)
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_title("2D Contact Force Visualization")

# 存储绘图元素，用于每一帧清除
plot_artists = []

# 定义帧更新回调
def update_visualization(context):
    # 清除上一帧的所有绘图元素
    for artist in plot_artists:
        artist.remove()
    plot_artists.clear()

    # 1. 绘制场景刚体
    pose_bundle = pose_bundle_port.Eval(context)
    for pose in pose_bundle.get_poses():
        p_W = pose.translation()
        # 提取2D旋转角度
        rot_angle = pose.rotation().ToRotationMatrix().angle()
        # 绘制立方体（示例，可根据实际几何调整）
        rect = plt.Rectangle(
            (p_W[0]-0.1, p_W[1]-0.1), 0.2, 0.2,
            angle=rot_angle, color="steelblue", alpha=0.6
        )
        ax.add_patch(rect)
        plot_artists.append(rect)

    # 2. 绘制接触力
    contact_results = contact_results_port.Eval(context)
    force_scale = 0.15

    for i in range(contact_results.num_point_pair_contacts()):
        contact_info = contact_results.point_pair_contact_info(i)
        p_WC = contact_info.contact_point()
        f_W = -contact_info.contact_force()  # 转换为作用在被接触物体的力
        # 绘制力箭头
        arrow = ax.arrow(
            p_WC[0], p_WC[1],
            force_scale * f_W[0], force_scale * f_W[1],
            head_width=0.06, head_length=0.06, fc="crimson", ec="crimson"
        )
        plot_artists.append(arrow)

    # 更新画布
    plt.draw()
    plt.pause(0.01)

# 将回调绑定到仿真器
simulator.set_monitor(update_visualization)
# 启动仿真
simulator.AdvanceTo(5.0)
plt.show()

关键注意事项 #

• 力的方向：ContactResults中返回的contact_force()是作用在第一个接触物体（A）上的力，若要绘制作用在第二个物体（B）上的接触力，需取反。
• 缩放因子：根据场景大小调整force_scale，确保力箭头清晰可见且不遮挡场景。
• 几何细节：示例中简化了刚体几何绘制，若需更精确的可视化，可通过SceneGraph的几何查询接口获取物体的具体形状信息。

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