Motion Planning

Part I: Optimization-based

MIT 6.421

Robotic Manipulation

Fall 2023, Lecture 11

Follow live at https://slides.com/d/naUMBG4/live

(or later at https://slides.com/russtedrake/fall23-lec11)

Failure modes of my "Clutter Clearing" demo

I would group them into:

Motion planning. Collisions and DiffIK limitations.
Perception.
- grasp far from object center of mass,
- double-picks,
- phantoms...
More than pick and place.
- objects stuck in the corner
- objects too big for the hand

Motivation: Moving fast! (at the dynamic limits)

"Using trajectory optimization made scooping up to twice as fast"

credit:

Charles W. Wampler and Andrew J. Sommese. Numerical algebraic geometry and algebraic kinematics. Acta Numerica, 20:469-567, 2011.

Inverse kinematics (a rich history!)

Inverse kinematics as an optimization

\min_q | q-q_{desired}|

subject to:

rich end-effector constraints
joint limits
collision avoidance
"gaze constraints"
"feet stay put"
balance (center of mass)
...

X^B = f_{kin}^B(q)

        ik = InverseKinematics(plant, plant_context)
        ik.AddPositionConstraint(
            gripper_frame,
            [0, 0, 0],
            plant.world_frame(),
            pose.translation(),
            pose.translation(),
        )
        ik.AddOrientationConstraint(
            gripper_frame,
            RotationMatrix(),
            plant.world_frame(),
            pose.rotation(),
            0.0,
        )
        prog = ik.get_mutable_prog()
        q = ik.q()
        prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)
        prog.SetInitialGuess(q, q0)
        result = Solve(ik.prog())

ik.AddPositionConstraint(
    frameB=gripper_frame, p_BQ=[0, 0.1, -0.02],
    frameA=cylinder_frame, p_AQ_lower=[0, 0, -0.5], p_AQ_upper=[0, 0, 0.5])
ik.AddPositionConstraint(
    frameB=gripper_frame, p_BQ=[0, 0.1, 0.02], 
    frameA=cylinder_frame, p_AQ_lower=[0, 0, -0.5], p_AQ_upper=[0, 0, 0.5])

\begin{bmatrix} 0 \\ 0 \\ -0.5 \end{bmatrix} \le {}^CX^G {}^Gp^{Q} \le \begin{bmatrix} 0 \\ 0 \\ 0.5 \end{bmatrix}

for

{}^Gp^Q = \begin{bmatrix} 0 \\ 0.1 \\ -0.02 \end{bmatrix}, \begin{bmatrix} 0 \\ 0.1 \\ 0.02 \end{bmatrix}

where C - cylinder, G - Gripper

Motion planning as a (nonconvex) optimization

\begin{aligned} \min_{q_0, ..., q_N} \quad & \sum_{n=0}^{N-1} | q_{n+1} - q_n|_2^2 & \\ \text{subject to} \quad & q_0 = q_{start} \\ & q_N = q_{goal} \\ & |q_n|_1 \ge 1 & \forall n \end{aligned}

start

goal

fixed number of samples

collision-avoidance

(outside the \(L^1\) ball)

nonconvex

work by Hongkai Dai et al. at TRI

Danny Driess and Jung-Su Ha and Marc Toussaint, Deep Visual Reasoning: Learning to Predict Action Sequences for Task and Motion Planning from an Initial
Scene Image, Robotics: Science and Systems (R:SS) 2020.