Input data Input the body and joint number Input initial position of each body Input joint kind and driving information Q Input inner and outer body number for each joint Input relative position vectors of the joint points Input kinematic coefficients Q Input driving coefficients (only for driving constraints
Input data Input the body and joint number Input initial position of each body Input joint kind and driving information Input inner and outer body number for each joint Input relative position vectors of the joint points Input kinematic coefficients Input driving coefficients (only for driving constraints
Establish constraint equations Derive position constraint equations 见 Calculate Jacobian matrix 夏 Calculate velocity right terms 0 Calculate acceleration right terms
Establish constraint equations Derive position constraint equations Calculate Jacobian matrix Calculate velocity right terms Calculate acceleration right terms