Trajectory optimization for underactuated systems