基于Multibody Plant的Direct Transcription小球移动任务求解失败咨询

我在解决小球移动至目标位置的问题，现有代码如下：

plant = MultibodyPlant(time_step=0.01)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(plant)
ConfigureParser(parser)
parser.AddModelsFromString(minigolf_urdf, 'urdf')
plant.Finalize()

context = plant.CreateDefaultContext()
dirtran = DirectTranscription(
    plant,
    context,
    400,
    input_port_index=plant.get_actuation_input_port().get_index(),
)
prog = dirtran()

initial_state = (-2.0, 0.0, 0.0, 0.0)
prog.AddBoundingBoxConstraint(
    initial_state, initial_state, dirtran.initial_state()
)
# More elegant version is blocked by drake #8315:
# prog.AddLinearConstraint(dirtran.initial_state() == initial_state)

final_state = (2.0, 0.5, 0.0, 0.0)
prog.AddBoundingBoxConstraint(
    final_state, final_state, dirtran.final_state()
)
# prog.AddLinearConstraint(dirtran.final_state() == final_state)

dirtran.AddConstraintToAllKnotPoints(dirtran.input()[0] <= 1)
dirtran.AddConstraintToAllKnotPoints(dirtran.input()[1] <= 1)
dirtran.AddConstraintToAllKnotPoints(dirtran.input()[0] >= -1)
dirtran.AddConstraintToAllKnotPoints(dirtran.input()[1] >= -1)

initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
    [0.0, 16.0], np.column_stack((initial_state, final_state))
)  # yapf: disable
dirtran.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)

result = Solve(prog)

该代码使用Direct Collocation时运行完全正常，但切换为Direct Transcription后无法得到有效结果，生成的轨迹中小球始终保持静止。我已设置初始与终态的BoundingBox约束，且初始轨迹本身是可行解，请问是否是问题离散化导致的该现象？

我的URDF模型如下：

base_urdf = """
  <link name="base">

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="8 2 0.002" />
      </geometry>
      <material>
        <color rgba="0 1 0 1" />
      </material>
    </visual>

  </link>
"""

hole_urdf = """
  <link name="hole">

    <visual>
      <origin xyz="2 0.5 -0.024" rpy="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius=".05" />
      </geometry>
      <material>
        <color rgba="0 0 0 1" />
      </material>
    </visual>

  </link>
"""

ball_urdf = """
  <link name="ball">

    <inertial>
      <origin xyz="0 0 0.051" />
      <mass value="1" />
    </inertial>

    <visual>
      <origin xyz="0 0 0.051" rpy="0 0 0" />
      <geometry>
        <sphere radius=".05" />
      </geometry>
      <material>
        <color rgba="1 1 1 0" />
      </material>
    </visual>

  </link>
"""

mass = """
  <link name="mass">

    <inertial>
      <origin xyz="0 0 0.051" />
      <mass value="1" />
    </inertial>

    <visual>
      <origin xyz="0 0 0.051" rpy="0 0 0" />
      <geometry>
        <sphere radius=".05" />
      </geometry>
      <material>
        <color rgba="1 1 1 1" />
      </material>
    </visual>

  </link>
"""

base_joint = """
  <joint name="base_joint" type="fixed">
    <parent link="world" />
    <child link="base" />
  </joint>
"""

hole_joint = """
  <joint name="hole_joint" type="fixed">
    <parent link="world" />
    <child link="hole" />
  </joint>
"""

ball_joint = """
  <joint name="ball_joint" type="prismatic">
    <parent link="base" />
    <child link="ball" />
    <axis xyz="1 0 0" />
  </joint>
"""

mass_joint = """
  <joint name="mass_joint" type="prismatic">
    <parent link="ball" />
    <child link="mass" />
    <axis xyz="0 1 0" />
    <limit effort="1" lower="0" upper="0" />
  </joint>
"""

ball_transmission = """
  <transmission type="SimpleTransmission" name="ball_force">
    <actuator name="ball_force" />
    <joint name="ball_joint" />
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
"""

mass_transmission = """
  <transmission type="SimpleTransmission" name="mass_force">
    <actuator name="mass_force" />
    <joint name="mass_joint" />
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
"""

minigolf_urdf = f"""
<?xml version="1.0"?><robot name="Minigolf">
    {base_urdf}
    {hole_urdf}
    {ball_urdf}
    {mass}
    {base_joint}
    {hole_joint}
    {ball_joint}
    {mass_joint}
    {ball_transmission}
    {mass_transmission}
    </robot>
</xml>
"""

问题分析与解决方案 #

1. 核心冲突：关节限位与终态约束矛盾 #

你的URDF中mass_joint设置了<limit effort="1" lower="0" upper="0"/>，这直接将mass部件的Y轴位置固定为0，但终态约束要求mass的Y位置为0.5，这是不可能满足的约束冲突。

Direct Collocation可能因为离散化方式或求解器的松弛特性，暂时忽略了这个硬限位约束，但Direct Transcription对关节约束的执行更严格，求解器无法找到满足所有约束的运动轨迹，只能退回到静止的初始状态（唯一不违反关节限位的解）。

解决方法：修改mass_joint的限位设置，比如将lower和upper调整到包含0.5的范围，或者直接移除limit标签（如果需要完全自由运动）：

<joint name="mass_joint" type="prismatic">
  <parent link="ball" />
  <child link="mass" />
  <axis xyz="0 1 0" />
  <!-- 移除或修改限位，比如：<limit effort="1" lower="-1" upper="1" /> -->
</joint>

2. Direct Transcription的额外配置建议 #

即使解决了关节限位问题，还需要补充以下配置确保求解正常：

• 显式设置时间范围：DirectTranscription默认不会限制总时间，添加时间约束匹配你的初始轨迹时长：

  dirtran.AddTimeIntervalBounds(0.0, 16.0)
  

• 添加目标函数：没有目标函数时，求解器会选择满足约束的最简解（比如静止）。建议添加最小化输入能量或时间的目标：

  # 最小化输入能量
  prog.AddQuadraticCost(dirtran.input().dot(dirtran.input()), is_convex=True)
  # 或最小化总时间
  # prog.AddCost(dirtran.final_time())
  

• 确认状态向量顺序：验证initial_state和final_state的顺序与plant.GetPositionsAndVelocities(context)返回的顺序一致，避免约束加错位置。

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