轨迹优化中为抓手添加外部Wrench的最新工作流咨询

一、正确构造外部空间力的AbstractValue #

你混淆了ExternallyAppliedSpatialForceMultiplexer的用途——它是用来合并多个外部力输入的系统，而非要打包成AbstractValue的数据。Plant的applied_spatial_force_input_port()接收的是**list[ExternallyAppliedSpatialForce]类型的AbstractValue**，正确的构造方式如下：

from pydrake.multibody.tree import ExternallyAppliedSpatialForce, SpatialForce
from pydrake.systems.framework import AbstractValue, ConstantValueSource

# 构造单个外部力对象
force_object = ExternallyAppliedSpatialForce()
force_object.body_index = plant.GetBodyIndices(hand_model_index).pop()
# 设置作用在抓手质心Bq点的空间力（W系下）
force_object.F_Bq_W = SpatialForce(tau=np.zeros(3), f=np.array([0., 0., 10.]))
# 必须指定力的作用点在B系下的位置（质心对应原点）
force_object.p_Bq_B = np.zeros(3)

# 将力对象放入列表，再打包为AbstractValue
forces_list = [force_object]
force_value = AbstractValue.Make(forces_list)

# 创建常量值源并连接到Plant的输入端口
force_system = builder.AddSystem(ConstantValueSource(force_value))
builder.Connect(force_system.get_output_port(0), plant.get_applied_spatial_force_input_port())

注意：必须显式设置p_Bq_B（力作用点在刚体自身坐标系下的位置），否则会导致动力学计算错误。

二、AutoDiff兼容的Wrench轨迹优化工作流 #

若要将Wrench作为全时段决策变量进行轨迹优化，推荐使用DirectCollocation框架，结合AutoDiffXd类型完成符号微分，具体步骤如下：

1. 构建AutoDiff版本的MultibodyPlant #

from pydrake.multibody.plant import MultibodyPlant
from pydrake.autodiffutils import AutoDiffXd

# 基于原Plant拓扑构建AutoDiff版本的Plant
plant_autodiff = plant.ToAutoDiffXd()
builder_autodiff = DiagramBuilder[AutoDiffXd]()
plant_autodiff = builder_autodiff.AddSystem(plant_autodiff)

2. 将Wrench设为分段多项式决策变量 #

使用DirectCollocation将Wrench的6个分量（3个力矩+3个力）作为分段多项式的系数纳入优化变量：

from pydrake.systems.trajectory_optimization import DirectCollocation
from pydrake.math import eq

# 初始化DirectCollocation
dircol = DirectCollocation(
    plant=plant_autodiff,
    context=plant_autodiff.CreateDefaultContext(),
    num_time_samples=20,
    minimum_timestep=0.01,
    maximum_timestep=0.1
)

# 获取外部力输入端口的索引
force_port = plant_autodiff.get_applied_spatial_force_input_port()
port_index = force_port.get_index()

# 为每个时间样本的Wrench创建优化变量
for i in range(dircol.num_time_samples()):
    input_context = dircol.input_port_context(i, port_index)
    forces_list = input_context.get_mutable_value()
    force = forces_list[0]
    # 将空间力的6个分量设为优化变量
    tau_vars = dircol.NewContinuousVariables(3, f"tau_{i}")
    f_vars = dircol.NewContinuousVariables(3, f"f_{i}")
    force.F_Bq_W = SpatialForce(tau=tau_vars, f=f_vars)
    # 固定刚体索引和作用点（抓手质心）
    force.body_index = plant_autodiff.GetBodyIndices(hand_model_index).pop()
    force.p_Bq_B = np.zeros(3)

# 添加优化目标：最小化Wrench的平方和
cost = 0
for i in range(dircol.num_time_samples()):
    force_list = dircol.GetInputPort(i, port_index).Eval()
    tau = force_list[0].F_Bq_W.tau()
    f = force_list[0].F_Bq_W.f()
    cost += dircol.AddCost(tau.dot(tau) + f.dot(f))

# 添加自定义约束（示例：约束最终时刻抓手的目标位置）
final_context = dircol.final_context()
final_pos = plant_autodiff.GetBodyByName("hand").EvalPoseInWorld(final_context).translation()
dircol.AddConstraint(eq(final_pos, np.array([1.0, 0.0, 0.5])))

# 求解优化问题
result = Solve(dircol.prog())
if result.is_success():
    # 重构最优Wrench轨迹
    force_trajectory = dircol.ReconstructInputTrajectory(result, port_index)

3. 关键注意事项 #

• 所有涉及优化的对象必须统一使用AutoDiffXd类型，包括Plant、DiagramBuilder和DirectCollocation。
• 若需要轨迹平滑性，可添加分段多项式的导数连续约束（例如相邻时间段的一阶导数相等）。
• 针对浮动抓手，需确保Plant已正确配置浮动基座（通过AddMultibodyPlantSceneGraph时设置合适的time_step，根据动力学需求选择连续或离散系统）。

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