如何在Meshcat中可视化运动轨迹优化器的中间解

环境信息 #

• Drake版本：Dockerhub (1.20.0)
• Python版本：3.10.12
• 操作系统：Ubuntu 20.04

问题描述 #

我们需要在运动轨迹优化求解过程中，实时可视化优化器的中间解。目前已经通过AddVisualizationCallback(visualization_callback, opt.prog().decision_variables())把优化的关节位置控制点传递到回调函数里，能拿到iiwa机器人7个关节的位置数值，但不清楚怎么用Meshcat把这些数值可视化出来。MeshcatVisualizer应该是合适的工具，但文档里没明确讲直接可视化现有控制点的方法，或者更理想的——在Meshcat里加个机器人复制品来展示中间解的方式。

当前代码 #

def visualization_callback(joint_positions):
    # 更新Meshcat可视化器状态以匹配优化器当前配置
    context = robot.CreateDefaultContext()

    # 拼接速度为0的状态
    joint_states = np.concatenate((joint_positions, np.zeros(len(joint_positions))))

    context.get_mutable_continuous_state().get_mutable_vector().SetFromVector(joint_states)
    meshcat_vis.Publish(context)

# 主函数中
meshcat = StartMeshcat()
global meshcat_vis
meshcat_vis = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)

实现方案 #

方案1：复用现有可视化器实时更新状态 #

你当前的思路是对的，只是可以做些优化：

• 不用每次回调都新建context，提前在外部创建一个全局或闭包内的context，减少重复创建的开销
• 因为iiwa的连续状态是7个关节位置+7个关节速度，所以拼接0速度的操作是正确的，只要joint_positions是单个时刻的7维关节位置数组
• 要保证robot是你加载的iiwa模型实例，且和meshcat_vis关联的是同一个模型

调整后的代码示例：

# 提前创建context，避免重复初始化
robot_context = robot.CreateDefaultContext()
joint_state_vec = robot_context.get_mutable_continuous_state().get_mutable_vector()

def visualization_callback(joint_positions):
    # 拼接速度为0的完整状态
    full_state = np.concatenate((joint_positions, np.zeros_like(joint_positions)))
    joint_state_vec.SetFromVector(full_state)
    meshcat_vis.Publish(robot_context)

# 主函数部分不变
meshcat = StartMeshcat()
meshcat_vis = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)

方案2：添加机器人复制品展示中间解 #

如果想同时展示多个中间状态（比如当前迭代的轨迹关键点），可以给Meshcat添加一个机器人复制品，用不同的前缀区分：

meshcat = StartMeshcat()
# 原机器人的可视化器
meshcat_vis = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
# 创建复制品可视化器，指定前缀避免冲突
replica_vis = MeshcatVisualizer(meshcat, scene_graph, robot, prefix="/iiwa_replica")
replica_vis.Load()

# 提前创建两个context
robot_context = robot.CreateDefaultContext()
replica_context = robot.CreateDefaultContext()
robot_state_vec = robot_context.get_mutable_continuous_state().get_mutable_vector()
replica_state_vec = replica_context.get_mutable_continuous_state().get_mutable_vector()

def visualization_callback(joint_positions):
    # 假设joint_positions是N*7的轨迹点数组，取第一个点给原机器人，最后一个点给复制品
    initial_joints = joint_positions[:7]
    current_joints = joint_positions[-7:]

    # 更新原机器人状态
    robot_state_vec.SetFromVector(np.concatenate((initial_joints, np.zeros(7))))
    meshcat_vis.Publish(robot_context)

    # 更新复制品状态
    replica_state_vec.SetFromVector(np.concatenate((current_joints, np.zeros(7))))
    replica_vis.Publish(replica_context)

注意事项 #

• 要确认传给回调的decision_variables()确实只有关节位置变量，别混入时间、力等其他优化变量，不然joint_positions维度会出错
• 如果回调触发太频繁，可加个计数器，每N次迭代才更新一次Meshcat，避免卡顿
• 用复制品时，必须通过prefix参数区分不同实例，不然Meshcat里的模型会互相覆盖

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