As was discussed in previous sections of the thesis, a module is required for dynamics. This module is required to determine the forward and inverse dynamics. The module is also required to determine the system state (position and velocity of joints) after joint torques have been applied for some period of time.
As seen before, the forward and inverse dynamics have closed form solutions, and thus are very easy to incorporate. To find the reaction of the system under applied torques, some form of numerical integration is required. This integration has been done using the Runge-Kutta integrator (see Appendix on Runge-Kutta Integrator), with the forward dynamics equations. This combination allows the motion integration with a single callable subroutine.
Details of the equations are available in the Dynamics Appendix, where they were derived using Macsyma. The Runge-Kutta integrator is also discussed in a previous appendix.
init_inverse_dynamics(): This is a subroutine which must be called once, before any of the other dynamics subroutines are called. This routine is responsible for setting up global variables used in both the forward and inverse dynamics equations.
inverse_dynamics(): This routine will use the system state, and accelerations, to calculate the joint torques required to produce that state. This routine is based upon the closed form Macsyma output.
forward_dynamics(): This routine requires the system state, and the joint torques. Using the formulas derived with Macsyma, the resultant accelerations will be calculated and returned.
dynamics(): This routine will use the Runge-Kutta integrator to find the effect of an applied torque over some time step. This routine requires initial system state, constant torque, and time step duration. The integrator will return the system state at the end of the time step. The Runge-Kutta integrator, will obtain the derivatives of the system state by calling the routine rk_derivs().
rk_derivs(): The Runge-Kutta integrator will call this module to determine the derivatives of the system state array. This function will in turn use the forward_dynamics() routine. This module is not intended for higher level use, and was only placed in this module, for the sake of collecting related program parts.
* The inverse dynamics equations are available here for the two link
* manipulator with a payload. These use the equations of motion,
* combined with the Runge-Kutta integrator to provide motion over
* time (with constant torque). These also provide the inverse dynamics.
* A sample program is shown to help verify development, anf future use.
* When using any of the routines in this module, inin_inverse_dynamics()
#include "/usr/people/hugh/thesis/src/math/rk.c" /* Runge-Kutta Subroutines */
#define PI_FACT (3.141592654/180.0) /* degrees to rads */
* Definitions for the Inverse Dynamic Equations (this would be a good
* time for object oriented programming)
double gravity = GRAVITY, /* This is obvious */
l1 = LEN_1, /* Link lengths */
mp = MASS_PAYLOAD, /* Payload Mass */
m2 = MASS_2, /* Link Masses */
i_1, /* Link moments of inertia */
dyn1_tor, dyn2_tor, /* Global Torque Values */
_r1, _r2, _r3, _r4, _r5, _r6; /* Global Dynamics Constants */
* This is an example program to input a set of angles then find the
* joint torques required to hold the arm there.
* sscanf(aa, "%lf,%lf", &a1, &a2);
* inverse_dynamics(a1, a2, 0.0, 0.0, 0.0, 0.0, &t1, &t2);
* printf("ang %f, %f >> tor 1 = %f, tor 2 = %f \n", a1, a2, t1, t2);
* INITIALIZE CONSTANTS FOR INVERSE DYNAMICS
* This bit of code has come from macsyma, originally written in fortran.
* In this optimized form this bit of code contains 16 adds/subtracts,
* and 24 divides/multiplies. The actual inverse dynamics are
* calculated when the other routine is called (after this is called once
* at the start of a program. This was originally produced in fortran,
* This routine only assigns values to the global variables _r1, _r2, etc.
* These global variables have been devised by Macsyma, and will be used in
* the other dynamics equations formulated by Macsyma.
static double b1, b2, b3, b4, b5,
_r1 = 4.0*b3*m2 + 2*b3*m1 + b1*b3;
_r2 = mp*(4.0*pfx4+pfx3) + m2*(pfx4 + pfx3) + m1*pfx2 + pfx1 + 4*i_1;
_r6 = m2*pfx4 + b1*pfx4 + pfx1;
void inverse_dynamics(t1_0, t2_0, t1_1, t2_1, t1_2, t2_2, torque1, torque2)
double t1_0, t2_0, t1_1, t2_1, t1_2, t2_2, *torque1, *torque2;
* This subroutine should do near optimal calculations of the inverse
* dynamics after the initialization routine has been called. This
* contains 17 adds and subtracts, and 30 multiplies and divides. The
* routine expects to be passed the current robot state variables, and
* will return the required joint torques to produce those values.
* This routine was originally in Fortran, and was converted to ’C’.
* This routine has been written to use degrees, thus an additional 8
* multiplies have been added to convert from degrees to radians in the
* Variables: t1_0, t2_0: The joint angles (degrees)
* t1_1, t2_1: the angular velocities
* t1_2, t2_2: The angular Accelerations
* Return: torque1, torque2: the required joint torques (N/m)
static double b1, b2, b3, b4, b5,
torque1[0] = ((t2_2)*(_r6 + pfx11*pfx7) + (t1_2)*_r2: pfx1*
_r1 + (-b5: b4)*t2_1*pfx8*t1_1 + (-b3: b1)*t2_1*t2_1*
pfx8 + b2*pfx8 + (pfx3 + (b5 + b4)*t1_2)*pfx7)/4.0;
torque2[0] = (t1_2*_r6 + t2_2*_r6 + pfx8*(pfx11*t1_1*t1_1 + b2) +
(pfx3 + pfx11*t1_2)*pfx7)/4.0;
void dynamics(t_step, t1_0_o, t2_0_o, t1_1_o, t2_1_o, tor_1, tor_2,
t1_0, t2_0, t1_1, t2_1, t1_2, t2_2)
double t_step, t1_0_o, t2_0_o, t1_1_o, t2_1_o, tor_1, tor_2,
*t1_0, *t2_0, *t1_1, *t2_1, *t1_2, *t2_2;
* This routine will take an initial system state, and then do a time
* step, and report the dynamic state at the end of the time step. This
* routine uses the Runge-Kutta technique for ODE integration. Note that
* the choice of time step becomes critical when it is too large for the
* motion of the system. Also keep in mind that this routine is using a
* variable time step Runge-Kutta.
* The Runge-Kutta integrator also calls the function rk_derivs().
* Variables: t_step: The time over which the system is to be integrated
* t1_0_o, t2_0_o: The start robot joint coordinates (degrees)
* t1_1_o, t2_1_o: The start robot joint velocities
* t1_2_o, t2_2_o: The start robot joint accelerations
* tor_1, tor_2: The constant robot joint torques (N/m)
* Returns: t1_0, t2_0: The final robot joint coordinates (degrees)
* t1_1, t2_1: The final robot joint velocities
* t1_2, t2_2: The final robot joint accelerations
static double ystart[20], /* State variable array */
h1, /* suggested first time step */
hmin, /* Minimum time step size */
time, /* start current time step */
tend, /* end current time step */
static int i, /* utility variable */
nvar; /* Number of state variables */
n = nvar = 4; /* Number of variables in state array */
dyn1_tor = tor_1; /* Remember torque globally */
step = 0.2*t_step; /* A nominal gross time step */
ystart[0] = t1_1_o; /* Set up state variable array */
n2 = n/2; /* location of position variables in array */
eps = 0.00001; /* Pick a tolerance limit */
h1 = t_step*0.05; /* An initial step size */
hmin = t_step*0.00000001; /* Minimum step size */
/* Loop for gross time steps of integration */
for(time = start; time <= end; time = time + step){
tend = time + step; /* Find end of current time step */
rk_odeint(ystart, nvar, time, tend, eps, h1, hmin,
&nok, &nbad); /* Do the Runge-Kutta magic */
t1_1[0] = ystart[0]; /* Recover new state variables */
forward_dynamics(t1_0[0], t2_0[0], t1_1[0], t2_1[0], tor_1, tor_2,
t1_2, t2_2); /* Find Joint accelerations */
* UPDATE STATE DERIVATIVE VARIABLE ARRAY FOR RUNGE-KUTTA
* The state variable derivative array for Runge-Kutta routines is updated
* here. This uses the forward dynamic equations.
* Variables: x: independant variable (not used here)
* y: state variables array (degrees)
* Returns: dydx: array of state derivatives
dydx[2] = y[0]; /* Update velocity */
forward_dynamics(y[2], y[3], y[0], y[1], /* Find Accelerations */
dyn1_tor, dyn2_tor, &dydx[0], &dydx[1]);
void forward_dynamics(t1_0, t2_0, t1_1, t2_1, tor_1, tor_2, t1_2, t2_2)
double t1_0, t2_0, t1_1, t2_1, tor_1, tor_2, *t1_2, *t2_2;
* These equations will use the position, velocity and torque, and produce
* the instantaneous accelerations. This routine is not as optimized as the
* previous inverse dynamics routine. The routine contains 33 adds/subtracts
* and 57 multiplies/divides. 4 trigonometric functions are also included.
* This routine also uses 10 multiplies/divides to convert degrees/radians.
* Variables: t1_0, t2_0: Joint positions (degrees)
* t1_1, t2_1: Joint velocities
* tor_1, tor_2: joint torques (N/m)
* Returns: t1_2, t2_2: joint accelerations
static double b1, b2, b3, b4, b5, b6, b7, b8, b9,
static double pfx1, pfx2, pfx3, pfx4, pfx5, pfx6,
pfx7, pfx8, pfx9, pfx10, pfx11, pfx12;
pfx1 = cos(t2_0*PI_FACT); /* Find common factors */
pfx4 = 1.0/(_r6*_r6 + pfx1*(2.0*_r5: _r4)*_r6: _r2*_r6 + pfx2*pfx3);
/* Get joint 1 acceleration */
t1_2[0] = pfx4*((b9 + b6)*tor_2: b6*tor_1 + pfx10*(pfx1*(-b2*pfx9 -
pfx2*pfx8): b4*pfx8) + pfx6: b12*b8: b4*b5 + b1*b2);
b11 = pfx8*_r5; /* Find more common terms */
/* Get joint 2 acceleration */
t2_2[0] = -pfx4*((4.0*pfx1*_r4 + 4.0*_r2)*tor_2 + (-b9: b6)*tor_1 +
pfx1*pfx5*(-_r1*_r5 + b14: b10) + pfx10*(pfx1*((b2: b13)*
pfx9: b11*_r4): b11*_r2 + (b10: b14)*pfx9) + pfx6 + b5*
(-pfx1*pfx2: b4) + (-b3*b7: b12)*b8 + b1*(b13: b2));