添加KinematicTrajectoryOptimization位置约束时lb.size() == num_positions()报错求解

你遇到的报错核心是约束变量维度与KinematicTrajectoryOptimization的优化变量维度不匹配：

• 你通过station.GetSubsystemByName("plant")获取的完整plant包含16个自由度（iiwa机械臂+WSG夹爪+物体），但KinematicTrajectoryOptimization仅针对iiwa的7个关节构造，因此其num_positions()返回值为7。
• 第一种方法中，PositionConstraint默认绑定整个plant的所有16个自由度，导致约束上下界维度（16）与trajopt的优化变量维度（7）不匹配。
• 第二种方法中，你传入的是3维任务空间位置约束，但该重载方法实际期望关节空间的上下界（维度需等于7），3≠7直接触发报错。

方案1：使用指定末端Frame的重载方法 #

直接调用支持任务空间位置约束的AddPathPositionConstraint重载，明确指定末端执行器Frame，trajopt会自动基于构造时的iiwa关节子集计算约束，无需处理整个plant的冗余自由度：

trajopt.AddPathPositionConstraint(
    frame=gripper_frame,
    p_BQ_lower=X_WStart.translation(),
    p_BQ_upper=X_WStart.translation(),
    frameB=plant.world_frame(),
    waypoint_index=0
)

方案2：手动构造任务空间约束函数 #

通过自定义函数映射iiwa关节位置到末端执行器位置，再添加约束，完全适配trajopt的7维优化变量：

def gripper_position(q):
    # q为iiwa的7维关节位置数组
    plant.SetPositions(plant_context, iiwa_joints, q)
    # 计算夹爪在世界坐标系下的位置
    X_WG = plant.CalcRelativeTransform(plant_context, plant.world_frame(), gripper_frame)
    return X_WG.translation()

# 添加路径点0的位置约束：夹爪位置严格等于目标位置
trajopt.AddPathConstraint(
    gripper_position,
    lower_bound=X_WStart.translation(),
    upper_bound=X_WStart.translation(),
    waypoint_index=0
)

• 构造KinematicTrajectoryOptimization时，必须明确传入iiwa的关节列表，而非整个plant：

  # 获取iiwa的7个关节
  iiwa_joints = [plant.GetJointByName(f"iiwa_joint_{i}") for i in range(1, 8)]
  # 基于iiwa关节构造trajopt
  trajopt = KinematicTrajectoryOptimization(plant, iiwa_joints, plant_context)
  

• 无需新建仅含iiwa的小型plant，原plant在约束生命周期内保持有效即可，上述方案均基于原plant但仅关联iiwa关节。

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