Assignment #4 Solution
Inverse Dynamics of 2R Manipulator

For reference, here is the figure showing the robot:

For the following analysis, theta_1 will be measured from the X axis and theta_2 relative to the first link.

Part 1: Formulating The Equations of Motion

By computing the velocity of the center of mass of each link, then the kinetic energy, and combining with the gravitational potential energy, we form the Lagrangian (somewhat laboriously!) as follows:

Now we can take some derivatives of L in order to get the joint torques as follows:

After some painstaking algebra, we arrive at the following expressions for the joint torques:

These dynamics terms can be arranged in the form of the generic dynamics equation:

If we write the equations this way, we will get the following dynamics terms:

Verification

Verifying these equations by inspection is a bit tricky. First, we should make sure that the equations are dimensionally correct. Then, we must be sure of both what the equations contain and what they might be (or should be) missing. The gravity terms can be verified directly. With a little imagination, you can perform some thought experiments to examine at least the signs on the other terms. For example if we want to maintain the first link stationary while we accelerate the second link, then a term must appear involving the acceleration of theta_2 in the expression for T_1. Moreover, this term must be in the same direction as the acceleration of link 2. Verifying the coriolis and centripetal terms is a bit more difficult.

It is always useful to have an independent way of computing the answer. A MATLAB file (r2idyn.m) was created to compute the inverse dynamics for this manipulator using a symbolic generator (based on the Newton-Euler approach). The equations derived above were encoded into a second file (idyn2r.m). The two routines take the same input arguments. A test program (idyntest.m) was written in which kinematic states were generated with values of position, velocity, and acceleration for the two links randomly selected in the range [-2,2]. The results for the two routines are shown below. (The diary file has been slightly edited to conserve space.) 

>idyntest
TH =
   -0.9651    -0.4453
THD =
   -0.5067    -0.3030
THDD =
    0.8546    -0.2633
Lagrangian   203.6962   17.1286 flops = 48
symbolic     203.6965   17.1289 flops = 40
>idyntest
TH =
   -0.7694     0.4546
THD =
    0.1272    -0.5330
THDD =
    0.0445     0.5245
Lagrangian   286.4453   71.8239 flops = 48
symbolic     286.4456   71.8242 flops = 40
>idyntest
TH =
    0.4502     0.2815
THD =
    0.6670     0.0270
THDD =
   -0.7953    -0.8184
Lagrangian   288.6881   46.6037 flops = 48
symbolic     288.6873   46.6029 flops = 40
>idyntest
TH =
    0.0241     0.9354
THD =
    1.6371    -1.7911
THDD =
    1.7050    -0.0028
Lagrangian   393.3998   64.8382 flops = 48
symbolic     393.4007   64.8391 flops = 40
>idyntest
TH =
    0.4634     0.8917
THD =
   -1.6553    -0.1190
THDD =
    0.1422    -1.7639
Lagrangian   269.8307   23.6807 flops = 48
symbolic     269.8299   23.6799 flops = 40
>idyntest
TH =
    1.7997    -1.0461
THD =
   -1.0808    -1.3113
THDD =
    1.4985     1.9365
Lagrangian    57.9049   61.8500 flops = 48
symbolic      57.9066   61.8517 flops = 40
Fortunately, the two routines provide essentially identical results. It is rather interesting that there are very small differences down in the 5th or 6th digit. This seems a little large to be blamed on round-off error. The explanation, I think, is that in the symbolically generated code, some of the constants that are pre-computed are not written into the code with full precision. As I recall, they are only written to five digits of accuracy. So this could explain the slight discrepancies noted here.

It is also interesting to note the efficiency of the symbolically generated code compared to that written manually. The manually written code could maybe be cleaned up a little bit to reduce its flop count. We have found in other cases that the symbolic generator often out-performs even a moderately careful programmer in producing efficient code.

Another way to verify the equations would be to compare with the results produced by the Robotics Toolbox function rne(). I didn't have time to play with that function - maybe next year!

Part 2: Motion Polynomial

Examining the requirements of the trajectory function s, we note that with 6 required end conditions we will have to use a polynomial of order 5. If we consider the six coefficients of this polynomial as unknowns, we can substitute the six end conditions to come up with six equations that must be satisfied by the coefficients. Actually, this boils down to solving for only three coefficients, since three of them drop out right away using the conditions at t* = 0. The resulting polynomial is

A plot of this polynomial and its derivatives for the 3 second move used in the next part looks like this:

Notice that although the acceleration starts and ends at zero, the rate of change of acceleration at these points (known as "jerk") is discontinuous. This can cause some problems when commanding fast motion, since it will excite high frequency vibrations in the physical system. We could have overcome this, too, if we had used an 7th order polynomial.

Part 3: Joint Torques

With all the pieces ready, we can compute joint torques required for the move given. A MATLAB script (trajdyn.m) was written to do this. The trajectory starts at [theta_1, theta_2] = (30,-60) and ends at (80,-30). The script also lets you set the value of T_seg.

First, calculate the torques if the move is to be done in 3 seconds:

Notice that the majority of the torque arises from the gravity terms. You can imagine if the move is very slow, then none of the other dynamics terms (inertia, coriolis, etc.) will have much effect at all. This is called a quasi-static move, since only static gravity torques are required. For this reason, robot controllers that contain only compensation for gravity tend to work only for slow motions.

Now repeat the computation so that the motion occurs in only 1 second:

Now we see that the inertia torques are very important. Even the coriolis and centripetal torques are large enough that they can't be neglected. The maximum required torque on both joints is considerably more than that required for the 3 second move. Robots that move quickly need very powerful actuators.

That's All Folks!!
Hope you enjoyed the assignments and found them useful.

Questions?

Send a message to roger.toogood@ualberta.ca

Return to Assignments List