如何为GCS的顶点与边添加末端执行器约束？

示例任务 #

假设机械臂已抓取杯子并要放置到货架上，需保证运动过程中杯内液体不洒。在IK问题中，我会添加如下约束：

R_C_desired = RotationMatrix.MakeZRotation(0)
ik.AddOrientationConstraint(
    frameB=EE_frame, 
    R_AbarA=R_C_desired,
    frameA=W_frame,
    theta_bound=acceptable_tilt
)

但不确定如何在GCS的顶点和边上施加此类约束以保证运动合规。

首次尝试：朴素方法 #

我尝试定义变量q并传入以下函数：

def set_positions(q):
    plant_double = MultibodyPlant(time_step=0.0)
    parser = Parser(plant_double)

    scenario = """
    directives:
    - add_model:
        name: iiwa
        file: package://drake_models/iiwa_description/sdf/iiwa7_no_collision.sdf
        default_joint_positions:
            iiwa_joint_1: [0]
            iiwa_joint_2: [0.6]
            iiwa_joint_3: [0]
            iiwa_joint_4: [-1.75]
            iiwa_joint_5: [0]
            iiwa_joint_6: [ 1.0]
            iiwa_joint_7: [0]
    - add_weld:
        parent: world
        child: iiwa::iiwa_link_0
        X_PC:
            translation: [0, 0.8, 0.8]
            rotation: !Rpy { deg: [0, 0, 0]}
    """

    iiwa_model = parser.AddModelsFromString(scenario, ".dmd.yaml")
    plant_double.Finalize()

    plant = plant_double.ToScalarType[Expression]()

    context = plant.CreateDefaultContext()
    plant.SetPositions(context, iiwa_model[0], q)
    return context, plant

随后执行以下操作获取符号形式的末端执行器位姿，再通过vertex.AddConstraint()和edge.AddConstraint()添加约束：

context, plant = set_positions(q)
ee_frame = plant.GetFrameByName("iiwa_link_7")
T_e = ee_frame.CalcPoseInWorld(context)

但生成的表达式过于庞大且非凸，导致程序完全卡顿。

第二次尝试：IRIS约束分解方法 #

我了解到标准方法是将约束从轨迹优化阶段转移到凸分解阶段。使用IRIS构建器时，我尝试创建带约束的IK问题，并通过options.prog_with_additional_constraints将约束程序添加到IrisOptions中：

options = IrisOptions()
prog, ik = generate_ik_prog(plant, region_definition)
options.prog_with_additional_constraints = prog

该方法有时可行，但速度极慢且经常无法收敛。此外，即使无额外约束，IRIS运行速度也很慢，无法实时执行，这意味着需提前知晓所有约束，生成多个带不同约束的子图用于规划。

我的问题 #

如何正确地为GCS的顶点与边添加末端执行器约束？

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