Work in Motion Planning generally requires that the forward and inverse dynamic equations be used. The equations of motion were formulated for the two link case using the Lagrange-Euler formulation. To ensure that the equations are quite general, they were formulated to include gravity and a payload mass, although neither is used in this thesis.
The computer algebra package Macsyma [Symbolics Inc., 1985] was used to do the algebra. This was done for two reasons. The extent of the calculations was large, and prone to errors if done by hand. Thus, Macsyma was used to help provide error free calculation of results. Also, a set of routines was available for reducing the number of terms in the expression. These routines also produce Fortran code with a near minimum number of mathematical operations.
Many techniques exist for determining the equations of motion [Shahinpoor, 1987][Fu, Gonzalez, Lee, 1987][Spong, Vidyasagar, 1989][Craig, 1986][Brady et. al., 1982]. For example, the Newton-Euler formulation involves determining equations of motion based on balanced forces acting upon each link. This method results in an equation for each body, or link in the robot, which makes the system of equations bulky and hard to work with. Another method, which provides easy formulation, is the Bond Graph method which describes a robot in terms of a ‘power flow graph’ [Shahinpoor 1987]. One disadvantage is that this method requires a sophisticated program to handle the Bond Graph representation. The ideal solution for this work is a set of explicit closed form equations. The Lagrange-Euler method uses the conservation of energy in a system, with a Lagrangian, to determine the equations of motion. This method works very well, and has a simple formulation, and therefore will be used for dynamic calculations in this thesis.
The schematic for the manipulator is shown below.
Figure 17.1 Figure B.1: Schematic Diagram of Two Link Manipulator
The schematic shows two links and a payload. The first link has a mass ‘M1’ and a moment of inertia ‘I1’, the second link has a mass ‘M2’ and a moment of inertia ‘I2’. The payload has a mass ‘Mp’ and a moment of inertia ‘Ip’. The joint that drives the first link has an angular displacement of ‘θ1’ with a torque of ‘τ1’, and joint 2 has a displacement of ‘θ2’, and a torque of ‘τ2’. The entire system is exposed to a gravity of ‘g’.
The Lagrange-Euler dynamic formulation may be seen below, in a general form.
To simplify the equations, the moment of inertia will be assumed to be only effective in the plane of motion. Thus, the inertia tensor will be a diagonal matrix, and the inertial will only have an effect about the axis of rotation. The Inertia tensor is simplified further if the payload has a negligible moment of inertia, thus reducing the number of non-zero inertia tensors to two.
The specific values to be used with the formulation are shown below. These come from both the Robot Kinematics (Appendix A), and some basic definitions,
The definitions to this point are sufficient for Macsyma to deal with. The next stage is to use Macsyma to produce the equations of motion.
The computer algebra package, Macsyma [Symbolics Inc., 1985], was used to perform the mathematics of the problem. Macsyma is an artificial intelligence based tool capable of symbolic algebra, symbolic calculus, symbolic approximation methods, and numerical methods. Development of the package was begun in the late 1960’s at MIT, using the LISP programming language. The package became generally available to the public in the 1980’s.
The problem was set up using a Macsyma batch file (this file follows, with comments). The file contains both the basic definitions, and operations that were done. The file instructs Macsyma to output the record of the work to another file. Excerpts from this batch file appear at the end of this appendix.
Macsyma will read the instructions from the batch file and operate on them. The resultant equations are quite lengthy, and thus some facilities available in Macsyma for equation reduction are used. The routines were standard for trigonometric and algebraic reduction (i.e., ‘trigsimp()’ and ‘ratsimp()’).
The Macsyma libraries do not necessarily produce optimum Fortran output. The Fortran output from Macsyma makes no attempt to reduce mathematical operations by replacing clusters of similar expressions, and it does not improve factorization. A set of computer algebra routines, developed by Budgell [1990a, 1990b], were used for removing common expressions. This optimization took place as a two part operation. The first stage involved removing terms that could be pre-calculated, as if they were constants. To do this a function called ‘crunch()’ was used which would replace the unused terms with variables, and place the terms in a list. The modified equation, and the new list of redundant expressions could then be converted to Fortran code. A second routine was used, called ‘fortbigopt()’. It would take an algebraic expression, replace redundant operations with a variable, then produce Fortran Code which evaluates the redundant expressions, and then evaluates the main expression.
‘crunch()’ will extract clusters of constants in an expression, and replace them with single terms.
‘fortbigopt()’ produces Fortran code for an equation, or list of equations. This code is headed by a set of pre-calculations of redundant terms. Then the actual calculation of the expressions.
The run time for the entire job (start to end) is under 5 minutes on a sun 3/260. The Fortran code produced had a near optimal run time, and may be viewed as part of the Macsyma output.
/*************************************************************/
/* This is the calculation of the Lagrange-Euler formulation */
/* of the two link manipulator with a point mass payload */
/* This macsyma batch file was written by Hugh Jack */
/*************************************************************/
/* Import Peter Budgell’s equation reduction routines */
load(“/usr/people/peter/beam/util”);
/* Define the column width as 132 and name an output file */
/*************************************************************/
/* Define the general constraints associated with the */
/* manipulator, including joint coordinates, joint */
/* velocities, gravity. The dependency of the joint angles */
/*************************************************************/
/* Define the time dependency of the joint angles */
/* Define the joint coordinate vector */
/* Find the Joint Velocities as a function of the coordinates*/
/*************************************************************/
/* Define the payload parameters, and find the Kinetic Energy*/
/*************************************************************/
/* Define the position of the payload in cartesian space */
x_cp:l1*cos(t1)+l2*cos(t1+t2);
y_cp:l1*sin(t1)+l2*sin(t1+t2);
/* Find the jacobian of the manipulator payload */
jcp:matrix([diff(x_cp, t1), diff(x_cp, t2)],
[diff(y_cp, t1), diff(y_cp, t2)],
[diff(z_cp, t1), diff(z_cp, t2)]);
/* Find the velocity of the payload using the jacobian */
/* The Inertia tensor for the payload */
/* The angular velocity of the payload */
/* The kinetic energy of the payload */
kp:mp/2*transpose(vcp).vcp + 1/2*transpose(wp).ip.wp;
/* The Potential Energy of the Payload */
/*************************************************************/
/* Define the link 2 parameters, and find the Kinetic Energy */
/*************************************************************/
/* Define the position of joint 2 in cartesian space */
x_c2:l1*cos(t1)+l2/2*cos(t1+t2);
y_c2:l1*sin(t1)+l2/2*sin(t1+t2);
/* Find the jacobian of the manipulator joint 2 */
jc2:matrix([diff(x_c2, t1), diff(x_c2, t2)],
[diff(y_c2, t1), diff(y_c2, t2)],
[diff(z_c2, t1), diff(z_c2, t2)]);
/* Find the velocity of joint 2 using the jacobian */
/* The Inertia tensor for joint 2 */
/* The angular velocity of joint 2 */
/* The kinetic energy of joint 2 */
k2:m2/2*transpose(vc2).vc2 + 1/2*transpose(w2).i2.w2;
/* The Potential Energy of joint 2 */
/*************************************************************/
/* Define the link 1 parameters, and find the Kinetic Energy */
/*************************************************************/
/* Define the position of joint 1 in cartesian space */
/* Find the jacobian of the manipulator joint 1 */
jc1:matrix([diff(x_c1, t1), diff(x_c1, t2)],
[diff(y_c1, t1), diff(y_c1, t2)],
[diff(z_c1, t1), diff(z_c1, t2)]);
/* Find the velocity of joint 1 using the jacobian */
/* The Inertia tensor for joint 1 */
/* The angular velocity of joint 1 */
/* The kinetic energy of joint 1 */
k1:m1/2*transpose(vc1).vc1 + 1/2*transpose(w1).i1.w1;
/* The Potential Energy of joint 1 */
/*************************************************************/
/* The lagrangian will be calculated here, for use in */
/* calculating the joint torques. */
/*************************************************************/
lagrange:expand(kp+k2+k1-pp-p2-p1);
/*************************************************************/
/* The joint torques are calculate here */
/*************************************************************/
tor_1:diff(diff(lagrange, diff(t1, t)), t): diff(lagrange, t1);
tor_2:diff(diff(lagrange, diff(t2, t)), t): diff(lagrange, t2);
/*************************************************************/
/* This is where peters routines are used for simplification */
/* and the routines are turned into fortran code. */
/*************************************************************/
/* Create buffer for storing reduced expression terms */
/* Make a list of independent variables in expression */
/* For the expression of torque for joint 1 */
/* Simplify Trigonometric terms of equation */
/* Expand all trigonometric expressions in equation */
/* simplify equation by collecting common terms */
/* Budgell’s method for removing common terms */
/* Same as above, but for joint 2 */
/* Turn this stuff into fortran code */
/* This will produce fortran code for the equation constants */
/* This will produce the fortran code for the equations of */
/* motion (not including calculations of constants) */
fortbigopt([‘torque1=tt1, ‘torque2=tt2]);
/*************************************************************/
/* The forward kinematics are also a concern, but are very */
/* simple to derive once the inverse kinematic equations */
/*************************************************************/
/* Next The Forward Dynamics May be found by solving these */
/* two parametric equations */
solve([‘torque1 = tt1, ‘torque2 = tt2],[diff(t1,t,2),diff(t2,t,2)]);
/* The forward kinematic results will now be simplified and */
/* turned into Fortran code. */
The results of the Macsyma run may be seen below. The Fortran code may also be seen, within the output. It may be noted that the count of basic operations accompanies this. From these numbers it is possible to obtain some idea of how optimal the solution is, by comparison. Brady et. al., [1982, pg.78] state that the minimum number of multiplies for the Inverse Dynamics of a 2 degree of freedom robot is 20, and minimum number of adds is 14 (this is for the Raibert-Horn Method). As can be seen, Macsyma was able to obtain 30 multiplies and 17 adds for the Inverse Dynamics. Thus, the results from Macsyma are near optimum. Note that these counts do not include the constant calculation subroutine which proceeds the actual dynamics calculations, it had 24 multiplies and 16 adds.
When finding the Forward Dynamics, Macsyma was able to reduce the equations satisfactorily. The Constants, which are to be precalculated, are the same as those for the Inverse Dynamics, thus the counts of adds and multiplies are identical. To evaluate the Forward Dynamic expressions Macsyma used 33 adds, and 57 multiplies.
The Fortran results from Macsyma were removed from the output file and converted from Fortran to ‘C’ and inserted into the ‘C’ Code of the dynamics simulation program. The code was kept broken into three parts. The first subroutine was for the calculation of constants which needed to be calculated only once in the execution of the program. The second subroutine is the Inverse Dynamics, which needs to be called whenever the equations of motion need to be reevaluated. The third subroutine is called to calculate the Forward Dynamics.
The accuracy of the results was a slight concern, thus two informal tests were conducted to ensure their validity. In one test the arm was allowed to swing freely, with no forces at the actuators. The frequency of the joint swing was compared to theoretical estimates, and found to be appropriately equivalent. The simulation was also seen to conserve energy. The static properties were also tested by applying a force to the first joint, and letting the arm hang near the statically balanced position. The arm vibrated about the static equilibrium, thus showing the arm was statically modelled.
The following pages contain selected portions of the Macsyma job to derive the forward and inverse dynamics.
Basic Calculations and Definitions
(c42) /*************************************************************/
/* This is where peters routines are used for simplification */
/* and the routines are turned into fortran code. */
/*************************************************************/
/* Create buffer for storing reduced expression terms */
(c43) /* Make a list of independant variables in expression */
(c44) /* For the expression of torque for joint 1 */
/* Simplify Trigonometric terms of equation */
(d44) (((4 l1 l2 mp + 2 l1 l2 m2) sin(t1) ---- + (- 4 l1 l2 mp: 2 l1 l2 m2) cos(t1) (---)
+ (- 8 l1 l2 mp: 4 l1 l2 m2) cos(t1) --- --- + (8 l1 l2 mp + 4 l1 l2 m2) sin(t1) ----) sin(t2 + t1)
+ ((4 l1 l2 mp + 2 l1 l2 m2) cos(t1) ---- + (4 l1 l2 mp + 2 l1 l2 m2) sin(t1) (---) + (8 l1 l2 mp + 4 l1 l2 m2) sin(t1) --- ---
+ (8 l1 l2 mp + 4 l1 l2 m2) cos(t1) ---- + 4 gravity l2 mp + 2 gravity l2 m2) cos(t2 + t1) + (4 l2 mp + l2 m2 + 4 i_2) ----
+ ((4 l2 + 4 l1 ) mp + (l2 + 4 l1 ) m2 + l1 m1 + 4 i_2 + 4 i_1) ----
+ (4 gravity l1 mp + 4 gravity l1 m2 + 2 gravity l1 m1) cos(t1))/4
(c45) /* Expand all trigonometric expressions in equation */
(d45) ((cos(t1) sin(t2) + sin(t1) cos(t2)) ((4 l1 l2 mp + 2 l1 l2 m2) sin(t1) ---- + (- 4 l1 l2 mp: 2 l1 l2 m2) cos(t1) (---)
+ (- 8 l1 l2 mp: 4 l1 l2 m2) cos(t1) --- --- + (8 l1 l2 mp + 4 l1 l2 m2) sin(t1) ----)
+ (cos(t1) cos(t2): sin(t1) sin(t2)) ((4 l1 l2 mp + 2 l1 l2 m2) cos(t1) ---- + (4 l1 l2 mp + 2 l1 l2 m2) sin(t1) (---)
+ (8 l1 l2 mp + 4 l1 l2 m2) sin(t1) --- --- + (8 l1 l2 mp + 4 l1 l2 m2) cos(t1) ---- + 4 gravity l2 mp + 2 gravity l2 m2)
+ (4 l2 mp + l2 m2 + 4 i_2) ---- + ((4 l2 + 4 l1 ) mp + (l2 + 4 l1 ) m2 + l1 m1 + 4 i_2 + 4 i_1) ----
+ (4 gravity l1 mp + 4 gravity l1 m2 + 2 gravity l1 m1) cos(t1))/4
(c46) /* simplify equation by collecting common terms */
(d46) ((((4 l1 l2 mp + 2 l1 l2 m2) sin (t1) + (4 l1 l2 mp + 2 l1 l2 m2) cos (t1)) cos(t2) + 4 l2 mp + l2 m2 + 4 i_2) ----
+ ((- 4 l1 l2 mp: 2 l1 l2 m2) sin (t1) + (- 4 l1 l2 mp: 2 l1 l2 m2) cos (t1)) sin(t2) (---)
+ ((- 8 l1 l2 mp: 4 l1 l2 m2) sin (t1) + (- 8 l1 l2 mp: 4 l1 l2 m2) cos (t1)) --- sin(t2) ---
+ (- 4 gravity l2 mp: 2 gravity l2 m2) sin(t1) sin(t2) + (((8 l1 l2 mp + 4 l1 l2 m2) sin (t1)
+ (8 l1 l2 mp + 4 l1 l2 m2) cos (t1)) ---- + (4 gravity l2 mp + 2 gravity l2 m2) cos(t1)) cos(t2)
+ ((4 l2 + 4 l1 ) mp + (l2 + 4 l1 ) m2 + l1 m1 + 4 i_2 + 4 i_1) ----
+ (4 gravity l1 mp + 4 gravity l1 m2 + 2 gravity l1 m1) cos(t1))/4
(c47) /* Budgell’s method for removing common terms */
r1 = 4 gravity l1 mp + 4 gravity l1 m2 + 2 gravity l1 m1
r2 = (4 l2 + 4 l1 ) mp + (l2 + 4 l1 ) m2 + l1 m1 + 4 i_2 + 4 i_1
r3 = 4 gravity l2 mp + 2 gravity l2 m2
/usr/local/macsyma_309/das/mstuff.o being loaded.
(d47) (((r5 sin (t1) + r5 cos (t1)) cos(t2) + r6) ---- + (- r5 sin (t1): r5 cos (t1)) sin(t2) (---)
+ (- r4 sin (t1): r4 cos (t1)) --- sin(t2) ---: r3 sin(t1) sin(t2) + ((r4 sin (t1) + r4 cos (t1)) ---- + r3 cos(t1)) cos(t2)
(c48) /* Same as above, but for joint 2 */
(d48) (((4 l1 l2 mp + 2 l1 l2 m2) sin(t1) ---- + (4 l1 l2 mp + 2 l1 l2 m2) cos(t1) (---) ) sin(t2 + t1)
+ ((4 l1 l2 mp + 2 l1 l2 m2) cos(t1) ---- + (- 4 l1 l2 mp: 2 l1 l2 m2) sin(t1) (---) + 4 gravity l2 mp + 2 gravity l2 m2)
cos(t2 + t1) + (4 l2 mp + l2 m2 + 4 i_2) ---- + (4 l2 mp + l2 m2 + 4 i_2) ----)/4
(d49) ((4 l2 mp + l2 m2 + 4 i_2) ---- + ((4 l1 l2 mp + 2 l1 l2 m2) cos(t1) ---- + (- 4 l1 l2 mp: 2 l1 l2 m2) sin(t1) (---)
+ 4 gravity l2 mp + 2 gravity l2 m2) (cos(t1) cos(t2): sin(t1) sin(t2))
+ ((4 l1 l2 mp + 2 l1 l2 m2) sin(t1) ---- + (4 l1 l2 mp + 2 l1 l2 m2) cos(t1) (---) ) (cos(t1) sin(t2) + sin(t1) cos(t2))
+ (4 l2 mp + l2 m2 + 4 i_2) ----)/4
(d50) ((4 l2 mp + l2 m2 + 4 i_2) ---- + (((4 l1 l2 mp + 2 l1 l2 m2) sin (t1) + (4 l1 l2 mp + 2 l1 l2 m2) cos (t1)) (---)
+ (- 4 gravity l2 mp: 2 gravity l2 m2) sin(t1)) sin(t2) + (((4 l1 l2 mp + 2 l1 l2 m2) sin (t1)
+ (4 l1 l2 mp + 2 l1 l2 m2) cos (t1)) ---- + (4 gravity l2 mp + 2 gravity l2 m2) cos(t1)) cos(t2)
+ (4 l2 mp + l2 m2 + 4 i_2) ----)/4
(c51) tt2:crunch(tt2, dont, ‘r);
(d51) (r6 ---- + ((r5 sin (t1) + r5 cos (t1)) (---) : r3 sin(t1)) sin(t2) + ((r5 sin (t1) + r5 cos (t1)) ---- + r3 cos(t1)) cos(t2)
(d52) [r1 = 4 gravity l1 mp + 4 gravity l1 m2 + 2 gravity l1 m1, r2 =
(4 l2 + 4 l1 ) mp + (l2 + 4 l1 ) m2 + l1 m1 + 4 i_2 + 4 i_1, r3 = 4 gravity l2 mp + 2 gravity l2 m2,
r4 = 8 l1 l2 mp + 4 l1 l2 m2, r5 = 4 l1 l2 mp + 2 l1 l2 m2, r6 = 4 l2 mp + l2 m2 + 4 i_2]
(c53) /* Turn this stuff into fortran code */
/* This will produce fortran code for the equation constants */
r2 = mp*(4*pfx4+pfx3)+m2*(pfx4+pfx3)+m1*pfx2+pfx1+4*i_1
(c54) /* This will produce the fortran code for the equations of */
/* motion (not including calculations of constants) */
fortbigopt([‘torque1=tt1, ‘torque2=tt2]);
torque1 = (pfx12*(r6+pfx11*pfx7)+pfx2*r2+pfx1*r1+(-b5-b4)*pfx10*pf
1 x8*pfx9+(-b3-b1)*pfx10**2*pfx8-b2*pfx8+(pfx3+(b5+b4)*pfx2)*pfx7
torque2 = (pfx2*r6+pfx12*r6+pfx8*(pfx11*pfx9**2-b2)+(pfx3+pfx11*pf
(c55) /*************************************************************/
/* The forward kinematics are also a concern, but are very */
/* simple to derive once the inverse kinematic equations */
/*************************************************************/
/* Next The Forward Dynamics May be found by solving these */
/* two parametric equations */
solve([‘torque1 = tt1, ‘torque2 = tt2],[diff(t1,t,2),diff(t2,t,2)]);
(d55) [[---- =: (cos(t2) (- 4 r5 sin (t1) torque2: 4 r5 cos (t1) torque2): 4 r6 torque2 + 4 r6 torque1
+ (r5 r6 sin (t1) + r5 r6 cos (t1)) sin(t2) (---) + (r4 r6 sin (t1) + r4 r6 cos (t1)) --- sin(t2) ---
+ (((r5 sin (t1) + 2 r5 cos (t1) sin (t1) + r5 cos (t1)) (---) : r3 r5 sin (t1): r3 r5 cos (t1) sin(t1)) cos(t2)
+ (r5 r6 sin (t1) + r5 r6 cos (t1)) (---) ) sin(t2) + (r3 r5 cos(t1) sin (t1) + r3 r5 cos (t1)) cos (t2): r1 r6 cos(t1))
/((r5 sin (t1) + 2 r5 cos (t1) sin (t1) + r5 cos (t1)) cos (t2) + ((2 r5: r4) r6 sin (t1) + (2 r5: r4) r6 cos (t1)) cos(t2)
+ r6 : r2 r6), ---- = (cos(t2) (sin (t1) (- 4 r4 torque2: r1 r5 cos(t1)): 4 r4 cos (t1) torque2: r1 r5 cos (t1)
+ (r2 r3: r3 r6) cos(t1)): 4 r2 torque2 + ((4 r5 sin (t1) + 4 r5 cos (t1)) cos(t2) + 4 r6) torque1
+ ((r5 sin (t1) + 2 r5 cos (t1) sin (t1) + r5 cos (t1)) cos(t2) + r5 r6 sin (t1) + r5 r6 cos (t1)) sin(t2) (---)
+ ((r4 r5 sin (t1) + 2 r4 r5 cos (t1) sin (t1) + r4 r5 cos (t1)) --- cos(t2) + (r4 r6 sin (t1) + r4 r6 cos (t1)) ---) sin(t2) ---
+ (((r4 r5 sin (t1) + 2 r4 r5 cos (t1) sin (t1) + r4 r5 cos (t1)) (---) + (r3 r5: r3 r4) sin (t1)
+ (r3 r5: r3 r4) cos (t1) sin(t1)) cos(t2) + (r2 r5 sin (t1) + r2 r5 cos (t1)) (---) + (r3 r6: r2 r3) sin(t1)) sin(t2)
+ ((r3 r4: r3 r5) cos(t1) sin (t1) + (r3 r4: r3 r5) cos (t1)) cos (t2): r1 r6 cos(t1))
/((r5 sin (t1) + 2 r5 cos (t1) sin (t1) + r5 cos (t1)) cos (t2) + ((2 r5: r4) r6 sin (t1) + (2 r5: r4) r6 cos (t1)) cos(t2)
(c56) /* The forward kinematic results will now be simplified and */
/* turned into Fortran code. */
pfx4 = 1/(r6**2+pfx1*(2*r5-r4)*r6-r2*r6+pfx2*pfx3)
‘DIFF(t1,t,2) = pfx4*((b9+b6)*torque2-b6*torque1+pfx10*(pfx1*(b2*p
1 fx9-pfx2*pfx8)-b4*pfx8)+pfx6-b12*b8-b4*b5-b1*b2)
‘DIFF(t2,t,2) = -pfx4*((4*pfx1*r4+4*r2)*torque2+(-b9-b6)*torque1+p
1 fx1*pfx5*(r1*r5-b14+b10)+pfx10*(pfx1*((b13-b2)*pfx9-b11*r4)-b11
2 *r2+(b14-b10)*pfx9)+pfx6+b5*(-pfx1*pfx2-b4)+(-b3*b7-b12)*b8+b1*