Pickup with trajectory optimization

Trajectory optimization for picking up an object.