In the last video we derived the recursive Newton-Euler inverse dynamics algorithm for open chains. In this video we address the forward dynamics, which solves for theta-double-dot given the joint forces and torques tau, the joint positions and velocities, and optionally an end-effector wrench F_tip. We can solve the forward dynamics using the inverse dynamics algorithm. First, we use the inverse dynamics to calculate the joint forces and torques if the joint accelerations theta-double-dot are zero. This gives us the Coriolis terms, the gravity terms, and the end-effector wrench terms of the joint forces and torques.

Next, we use the inverse dynamics to solve for the mass matrix M of theta. To do this, we call the inverse dynamics algorithm n times, once for each joint, and each time we set gravity, the end-effector wrench, the joint velocities, and all joint accelerations except one, equal to zero. We set the i'th joint acceleration to be one. Then the joint torque vector tau found by the inverse dynamics algorithm is the same as the i'th column of the mass matrix M. By calling the inverse dynamics algorithm n times, we can construct the mass matrix M.

Now referring back to the original problem statement, by calling the inverse dynamics algorithm n-plus-1 times, we have the mass matrix M of theta as well as c of theta, theta-dot, g of theta, and J-transpose times F_tip. We are given tau, so we just need to solve an equation of the form M times theta-double-dot equals a known vector. We can use any efficient algorithm to solve this for theta-double-dot.

The forward dynamics can be numerically integrated to simulate the motion of a robot. At each timestep, you use the forward dynamics to calculate the joint accelerations, then use the accelerations and the current joint positions and velocities to calculate the joint positions and velocities at the next timestep.

Let's use the forward dynamics to simulate the motion of this 6R arm as it falls in gravity with zero joint torques applied by the motors.

The motion may look somewhat unrealistic, because we have not modeled friction in the joints. There are many approximate models of friction, and you can add your favorite model of friction torque, replacing the zero joint torques by joint torques that depend on the joint velocities. One advantage of having zero friction and zero joint torques, however, is that we know that no energy is dissipated. Therefore, the total energy of the robot, the kinetic energy plus the potential energy, must be conserved. This gives us one test of whether our simulation is working properly. Let's watch the simulation one more time. Notice that the arm swings to approximately the same maximum height each time, indicating that the potential energy at the end of each swing is approximately the same. In fact, if we plotted the sum of the kinetic and potential energy as a function of time, we would see it is nearly constant. This is one indication that our simulator is working properly. Because of numerical integration drift, the total energy will slowly change with time, but this effect can be mitigated by using smaller integration timesteps or more complex numerical integration procedures.

Now that we can derive the forward and inverse dynamics of an open-chain robot, in the last few videos of Chapter 8 we address advanced concepts, such as dynamics in task space, dynamics of robots subject to constraints such as loop-closure constraints, and dynamics considering the details of geared motors driving the joints.