# Motion Planning

Part I: Optimization-based

MIT 6.4210/2

Robotic Manipulation

Fall 2022, Lecture 13

(or later at https://slides.com/russtedrake/fall22-lec13)

## Failure modes of my "Clutter Clearing" demo

I would group them into:

1. MultibodyPlant can crash on startup
2. Motion planning. Collisions and DiffIK limitations.
3. Perception.
• grasp far from object center of mass,
• double-picks,
• phantoms...
4. 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)
• ...
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])
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

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

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 13: Motion Planning I

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

• 339