Approximating Displacement with the Body Velocity Integral
kinematic controllability for decoupled trajectory planning in underactuated mechanical systems .