Motion Planning

Part I: Optimization-based

MIT 6.4210/2

Robotic Manipulation

Fall 2022, Lecture 13

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

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

Failure modes of my "Clutter Clearing" demo

I would group them into:

MultibodyPlant can crash on startup
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)
...

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

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.