How to add end effector constraints to vertices and edges of a GCS?

I have been trying to add some constraints on the end effector during some parts of the trajectory when using the GCSTrajectoryOptimization.

Here’s an example task:

Let’s say I’ve managed to have the arm grasp a cup and place it on the shelf. Now I’d like to make sure its content won’t spill out during the motion! To do this I’ll need to add some constraints like this (if I was adding them to an IK problem):

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


But I’m not sure how I can enforce a constraint like this on the nodes and edges of my GCS so that the motion complies to such constraints.

1. Here’s the first naive approach I tried

I tried defining variables for q and then passing them through this function:


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

And then did this:



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

To get the pose of the EE frame in symbolic form, and then added my constraints via: vertix.AddConstraint() and edge.AddConstraint().

Everything completely freezes given how large and non-convex this expression ends up being.


2. The second approach I tried

I realize that the standard approach is to move the constraints from trajectory optimization phase to convex decomposition phase. I did try this when using IRIS builder by creating an IK with constraints and then adding the constrained program to the IrisOptions via options.prog_with_additional_constraints:

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

This approach sometimes works, but is very slow and fails to converge frequently. Another issue with this approach is that because of how slow running IRIS is (even without the above constraints), it's impractical to do this on the fly. Which means I need to know all my constraints ahead of time, and generate many subgraphs with different constraints to be used when planning.

My question

What is the correct way to add end effector constraints to vertices and edges of a GCS?

Great question. For most robots, the relationship between joint coordinates and end-effector coordinates is nonlinear/nonconvex (forward kinematics). GcsTrajectoryOptimization is trying to formulate as much of the motion planning as possible using (mixed-integer) convex optimization. Roughly speaking, we have three options:

1. Convex decomposition of the nonconvex constraints (e.g. IRIS). As you say, the IRIS in Drake master today is slow, but it's getting MUCH faster very soon; so much so that using in on the fly might be very reasonable. See, for instance, the (current) top issues on our GCS project board and this recent paper. But Peter has even better/faster versions in the pipeline.

2. Convex approximations of the nonconvex constraints. In this direction, you can often come up with arbitrarily tight approximations of the constraint (e.g. by lifting to higher dimensions), but then you have to balance the cost of the optimization vs the tightness of the constraints. One example I like very much is in this recent paper, where we used an SDP relaxation to relate the kinematics between different frames; in this work the first-order SDP relaxation (with some additional linear tightening constraints) appeared to be effectively always tight. Typically, in this line of work we try to make convex relaxations (outer approximations) so that we never throw away feasible solutions, but other choices are possible, too.

3. Add the nonconvex constraint directly to GcsTrajOpt, to be used (only) in the rounding stage. This can be combined with approach 2, as we did in this recent paper. This is probably the easiest approach to try first, assuming the current API gives you enough access to do what you need. The IRIS configuration space reasoning still gives you some global reasoning, but the nonconvex constraints that you add this way can potentially reintroduce local minima.

Hope that helps.