# Motion Planning

Part I: Optimization-based

MIT 6.421

Robotic Manipulation

Fall 2023, Lecture 11

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

## Failure modes of my "Clutter Clearing" demo

I would group them into:

1. Motion planning. Collisions and DiffIK limitations.
2. Perception.
• grasp far from object center of mass,
• double-picks,
• phantoms...
3. 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 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)
gripper_frame,
[0, 0, 0],
plant.world_frame(),
pose.translation(),
pose.translation(),
)
gripper_frame,
RotationMatrix(),
plant.world_frame(),
pose.rotation(),
0.0,
)
prog = ik.get_mutable_prog()
q = ik.q()
prog.SetInitialGuess(q, q0)
result = Solve(ik.prog())
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])
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.

By russtedrake

# Lecture 11: Motion Planning I

MIT Robotic Manipulation Fall 2023 http://manipulation.csail.mit.edu

• 747