Establish constraint equations Derive position constraint equations 见 Calculate Jacobian matrix 夏 Calculate velocity right terms 0 Calculate acceleration right termsEstablish constraint equations Derive position constraint equations Calculate Jacobian matrix Calculate velocity right terms Calculate acceleration right terms