Step 4. Integration for the Next State

The final stq in any dynamic simulation algorithm is the numerical integration of the joint accel ations and rates to determine the next state joint rates and positions. Many different integration methods may be found in the literature to accomplish this task. FamUiar exaipples include the fourth-order Runge-Kutta 

A problem faced in this last step of any simulation algorithm is the drift due to numerical integration, l pically, the joint positions obtained through integration and transformed to an end effector position do not satisfy the [Aysical constraints of the robot system. To compensate for this, end effector position and rate feedback may be used to modify the general end effecuv contact f ce vector as follows  

This modification is modelled after a similar treatment of the integration drift problem described in [20,41]. Note that x and x are known and constant for the given application. The two terms, x and x, may be found from the present state joint variables. The position difference term, (x , - x), may be found fix)m a position deviation matrix as described in [41]. Thus, the entire modified contact force vector is known as a function of the present state only. 

The new modified value of the contact force vector may now be used in the calculation of the closed-chain joint accelerations as follows  

---