##
APPENDIX H - PATH OPTIMIZATION SUBROUTINES

H.1 Introduction:

The tasks involved in path planning were separated into their own separate module. This module includes calls to the optimization and the dynamics modules. This module will model paths with Bezier splines, do simple path generation, path optimization, and random walk searches.

A data structure is provided for Bezier spline storage. The spline data structure has a list of positions along the path. The same structure also has storage for the spline parameters. The spline parameter generation function is called, with the path points in the spline data structure. The function will return the same structure with the spline segment parameters. The spline data structure may be passed to a routine which will interpolate a value from the spline (as well as the first and second derivatives). This structure ensures an efficient handling of splines.

There are also a collection of routines for the generation of paths. The basic routine ‘get_guess()’, will produce a path guess, based on the coupling term. The path will then be refined by iteratively reducing time steps. In the optimization routines, a few different values of coupling factors will be tried, to find a better path time. In the primary optimization routine ‘opt_path()’ the optimization routines are also used to perform the Random Walk search, and the Hookes and Jeeves search.

Siddall’s optimization routines require that some servicing functions be supplied. These three functions are ureal(), const(), and equal(). These simply evaluation the cost, and constraints of the current set of decision variables. These routines are also used by the Random Walk Search. This search routine is also included, and set up to perform a semi generic form of the random walk search.

H.2 Example Subroutines:

path_test() - This is a sample subroutine which will generate an optimal path. The routine also saves points on the optimal path to disk using the path_calc() subroutine.

path_calc() - This function will save an optimal path to a file called ‘plot’. This is intended to show recovery of data from the spline structure, and act as a testing tool for debugging and development.

H.3 Spline Subroutines:

get_params() - When called, this function will use the points stored in the spline data structure. Calculations are performed to determine the coefficients for the spline segments, and these are returned in the data structure. This function must be called before a value can be interpolated for a spline.

get_value() - Once spline parameters have been calculated, a value may be interpolated for the spline. This function will determine which spline segment is relevant, and use its parameters to find the joint positions, velocities and accelerations at the indicated point in time.

H.4 Path Generation Subroutines:

opt_path() - This subroutine will accept the path endpoints, and generate a spline which should provide optimal motion. The method is multistage. First the path is approximated using the get_guess() function. Next, if the optimization flag is set, a combination of Hookes and Jeeves, and Random Walk searches are used to improve the solution.

opter_path() - A faster optimization is performed with this function. This search uses refinements of the coupling factor to improve the path. The paths are all generated with the get_guess() function.

get_path() - An unoptimized path may be desired. When this is the case, this function may be called. The function only requires path endpoints, and it will return a spline.

get_guess() - This function generates a path with a sinusoidal shape. The coupling factor is also used to compensate for non-linearities. After the path has been generated the path segment times are reduced until the path times are as small as possible without constraint violations.

H.5 Utility Subroutines:

cross() - The optimization routines require the path to be described in an array of decision variables. Since the spline routines have other data requirements (a structure), data must be converted between the two forms. This function is responsible for taking the list of decision variables and converting it to the spline structure form.

guess_start() - Random Walk Searches are performed by this routine. When the decision variable information is passed, this function will use a random walk technique to refine the results, and then pass the results back.

ureal() - This function is required by Siddall’s optimization routines. When called, the function will use the decision variables supplied to calculate a cost of the function. The intention is that Siddall’s routines will attempt to reduce the cost by altering the decision variables.

const() - When attempting to optimize a solution, the algorithms will have to alter the decision variables. This routine will report when a constraint has been violated. The routines indicate problems by putting negative values into a constraint array.

equal() - This routine only satisfies Siddall’s routines, which require that equality constraints be considered. In this case arrays of zero value are returned, because there are no equality constraints to be violated.

/*

* PATH DESCRIPTOR AND OPTIMIZATION ROUTINES

*

* This set of routines has been formulated to allow the description of

* paths with Bezier splines, and their optimization with optimization

* routines. These routines are based upon the theory described in the

* thesis body.

*

* November 29th, 1990.

*/

#include <stdio.h>

#include <math.h>

#include "/usr/people/hugh/thesis/src/optimize/seek.c" /* Optimization Subs.*/

/* #include "/usr/people/hugh/thesis/src/rt/rt_def.c" */ /* For Stand Alone */

/* #include "/usr/people/hugh/thesis/src/rt/rt_dyn.c" */ /* For Stand Alone */

/*

* Number of points on the spline (note that two of these are used for

* endpoints). The number of spline points must be >= 3

*/

#define N_PTS 4

/*

* Functions contained within this module

*/

void path_calc(), /* Functions Contained within this module */

path_test(),

opt_path(),

opter_path(),

cross(),

guess_start(),

get_guess(),

get_path(),

get_params(),

get_value();

typedef struct { /* Stores motion end points */

double t1_start;

double t1_end;

double t2_start;

double t2_end;

} endpoints;

typedef struct { /* Defines Bezier Spline params */

double t[N_PTS+1];

double y1[N_PTS+1];

double y2[N_PTS+1];

double control1[N_PTS+1];

double control2[N_PTS+1];

double a1[N_PTS+1];

double b1[N_PTS+1];

double c1[N_PTS+1];

double d1[N_PTS+1];

double a2[N_PTS+1];

double b2[N_PTS+1];

double c2[N_PTS+1];

double d2[N_PTS+1];

} parameters;

endpoints path; /* A storage variable */

int _dominant_joint; /* indicator of joint which */

/* dominates motion */

/*

* EXECUTIVE ROUTINE

*

* This routine will call the test function to demonstrate the optimization

* routines for paths. Removed when used in other programs.

*

* Novemeber, 29th, 1990.

*/

/*main()

{

path_test();

}*/

void path_test()

/*

* A PATH OPTIMZATION TESTING ROUTINE

*

* This routine will take an arbitrary path and optimize it. This

* may serve as a good example of how the routines operate.

*

* November 29th, 1990.

*/

{

static parameters spline; /* Store Spline Parameters here */

static double t1_start,

t1_end,

t2_start,

t2_end;

static int i; /* Work Variables */

t1_start = -90.0; /* set path endpoints */

t1_end = 90.0;

t2_start = 0.0;

t2_end = 0.0;

init_inverse_dynamics(); /* Set up dynamics */

/* Optimize the path */

opt_path(&spline, t1_start, t1_end, t2_start, t2_end, TRUE);

path_calc(spline, N_PTS); /* make a file of optimal values */

}

void path_calc(spline, N)

int N;

parameters spline;

/*

* FILE PATH DATA

*

* This method will create a file of results from the optimization

* subroutines. This routine is made for testing purposes, and

* as an example.

*

* Variables: spline - contains the table of values to construct the spline

* N - The number of points on the path

*

* November 29th, 1990.

*/

{

static double time, /* A work variable for saving paths */

mag, mag2, /* Variables of interest for path */

a2, a3, a4, a5; /* Dummy variables to be discarded */

FILE *fptr; /* Output file */

fptr = fopen("plot", "w"); /* File to dump path into */

get_params(&spline, N); /* Turns the table of values into */

/* a bezier spline representation */

/* Recover data points from spline */

/* representation and dump into file */

for(time = -.2; time <= spline.t[N-1]+0.2; time += spline.t[N-1]/50.0){

get_value(N, spline, time, &mag, &a2, &a3, &mag2, &a4, &a5);

fprintf(fptr, "%14.4f,%14.4f,%14.4f\n", time, mag, mag2);

}

fclose(fptr); /* Close Data File */

}

void get_params(curve, N)

int N;

parameters *curve;

/*

* CONVERT LIST OF POINTS TO SPLINE

*

* The list of endpoints and control points are converted to bezier spline

* segments. The structure will be returned with values for polynomial

* coefficients, which are used when values are extracted from the spline.

* Keep in mind that there are two splines being matched simultaneously,

* and the structure holds splines for each of the two robot joints.

*

* Variables: curve - a structure containing spline information

* N - Number of points on spline

*

* Returns: curve - with the spline segment coefficients

*

* November 29th, 1990.

*/

{

static int i; /* A work variable */

static double p0, p1, p2, p3, /* Four control points for segment */

slope1, slope2, /* Slope at end of segment */

time_1, time_2; /* Time for previous segment */

slope1 = 0.0; /* set zero slope to start */

slope2 = 0.0;

time_1 = 1.0; /* Init with non-zero value */

/*

* A loop to find the first and the internal spline segment parameters.

* This routine also allows smoothing of the transition between

* segments of the position and first derivative. The only segment

* not found here is the last (it has a slightly different

* requirement to match the last derivative).

*/

for(i = 0; i < N - 2; i++){

time_2 = curve[0].t[i+1] - curve[0].t[i]; /* Segment time */

p0 = curve[0].y1[i]; /* Get points on spline */

p1 = (time_2/time_1)*slope1/3.0 + curve[0].y1[i];

p2 = curve[0].control1[i];

p3 = curve[0].y1[i+1];

curve[0].a1[i] = -p0 + 3.0*p1 - 3.0*p2 + p3; /* Calc params.*/

curve[0].b1[i] = 3.0*p0 - 6.0*p1 + 3.0*p2;

curve[0].c1[i] = -3.0*p0 + 3.0*p1;

curve[0].d1[i] = p0;

slope1 = 3.0*(p3 - p2); /* Find slope at end of segment */

p0 = curve[0].y2[i];

p1 = (time_2/time_1)*slope2/3.0 + curve[0].y2[i];

p2 = curve[0].control2[i];

p3 = curve[0].y2[i+1];

curve[0].a2[i] = -p0 + 3.0*p1 - 3.0*p2 + p3;

curve[0].b2[i] = 3.0*p0 - 6.0*p1 + 3.0*p2;

curve[0].c2[i] = -3.0*p0 + 3.0*p1;

curve[0].d2[i] = p0;

slope2 = 3.0*(p3 - p2);

time_1 = time_2; /* Set time for previous segment */

}

time_2 = curve[0].t[N-1] /* Find Time for last step */

- curve[0].t[N-2];

/*

* These routines find the last segments for both splines. This is

* done separately because of the requirements of last spline seg.

*/

p0 = curve[0].y1[N-2]; /* Get points on spline */

p1 = (time_2/time_1)*slope1/3.0 + curve[0].y1[N-2];

p2 = curve[0].y1[N-1];

p3 = curve[0].y1[N-1];

curve[0].a1[N-2] = -p0 + 3.0*p1 - 3.0*p2 + p3; /* Calc Params */

curve[0].b1[N-2] = 3.0*p0 - 6.0*p1 + 3.0*p2;

curve[0].c1[N-2] = -3.0*p0 + 3.0*p1;

curve[0].d1[N-2] = p0; /* Find slope at end of segment */

p0 = curve[0].y2[N-2];

p1 = (time_2/time_1)*slope2/3.0 + curve[0].y2[N-2];

p2 = curve[0].y2[N-1];

p3 = curve[0].y2[N-1];

curve[0].a2[N-2] = -p0 + 3.0*p1 - 3.0*p2 + p3;

curve[0].b2[N-2] = 3.0*p0 - 6.0*p1 + 3.0*p2;

curve[0].c2[N-2] = -3.0*p0 + 3.0*p1;

curve[0].d2[N-2] = p0;

}

void get_value(N, spline, time, der1_0, der1_1, der1_2, der2_0, der2_1, der2_2)

parameters spline;

int N;

double time, *der1_0, *der1_1, *der1_2, *der2_0, *der2_1, *der2_2;

/*

* FIND POINT ON BEZIER SPLINE

*

* The parameters found for a Bezier spline will be used to interpolate a

* requested value. This function will not only return the function value,

* but also the first and second derivatives for both joints.

*

* Variables: N - the number of points on the path

* spline - a structure of spline parameters

* time - the time at which the spline should be interpreted

*

* Returns: der1_0, der2_0 - the value of the function for joints 1 & 2

* der1_1, der2_1 - the derivative for joints 1 and 2

* der1_2, der2_2 - the second derivatives for joints 1 & 2

*

* November 29th, 1990.

*/

{

static double diff, /* Fraction of segment covered */

duration; /* Duration of current segment */

static int i; /* Counter variable */

/*

* Find the segment which corresponds to the current segment.

*/

der1_1[0] = 0.0; /* initialize derivaties to zero */

der1_2[0] = 0.0;

der2_1[0] = 0.0;

der2_2[0] = 0.0;

/* set angles to start value */

if(spline.t[N-1] < time){

der1_0[0] = spline.y1[N-1];

der2_0[0] = spline.y2[N-1];

return;

}

/* or set angles to end values */

if(spline.t[0] > time){

der1_0[0] = spline.y1[0];

der2_0[0] = spline.y2[0];

return;

}

/* or find relevant spline segment */

for(i = N - 2; i > 0; i--)

if((time - spline.t[i]) >= 0.0) break;

/*

* Find the values for the segment determined.

*/

duration = spline.t[i+1]-spline.t[i]; /* find duration of segment */

diff = (time - spline.t[i])/duration; /* find fraction of segment */

/* Find position, velocity and acc. */

der1_0[0] = spline.a1[i]*diff*diff*diff + spline.b1[i]*diff*diff

+ spline.c1[i]*diff + spline.d1[i];

der1_1[0] = (3.0*spline.a1[i]*diff*diff + 2.0*spline.b1[i]*diff

+ spline.c1[i])/duration;

der1_2[0] = (6.0*spline.a1[i]*diff +2.0*spline.b1[i])/duration/duration;

der2_0[0] = spline.a2[i]*diff*diff*diff + spline.b2[i]*diff*diff

+ spline.c2[i]*diff + spline.d2[i];

der2_1[0] = (3.0*spline.a2[i]*diff*diff + 2.0*spline.b2[i]*diff

+ spline.c2[i])/duration;

der2_2[0] = (6.0*spline.a2[i]*diff +2.0*spline.b2[i])/duration/duration;

}

void opt_path(spline, t1_s, t1_e, t2_s, t2_e, opt_flag)

parameters *spline;

double t1_s, t1_e, t2_s, t2_e;

int opt_flag;

/*

* MOTION OPTIMIZATION

*

* This is the fancy routine to optimize the paths of the robot. This routine

* calls all of the optimization routines to generate a first guess, and then

* refine with the random walk and Hookes and Jeeves searches.

*

* Variables: t1_s, t2_s - Start values for path

* t1_e, t2_e - End values for path

* opt_flag - If set the path will be optimized (i.e. = TRUE)

*

* Returns: spline - the spline parameters for the path

*

* November 29th, 1990.

*/

{

static double x[5*N_PTS], /* Work variables for optimization */

phi[3],

psi[1],

rmax[5*N_PTS],

rmin[5*N_PTS],

xstrt[5*N_PTS],

w[500],

u, /* storage for cost function */

couple, /* Coupling factor for first guess */

best_c, /* Track best coupling factors */

best_2, /* Get refined coupling factor */

best_t; /* Best Time for coupled path */

static int n, /* number of decision variables */

ncons, /* Number of inequality constraints */

nequs, /* Number of equality constraints */

nviol, /* Number of constraint violations */

npenal; /* penalty function numbers */

static int i; /* work variables */

/*

* Parameters for optimization using seek

*/

n = 5*N_PTS; /* Number of decision variables */

ncons = 3; /* Number of inequality constraints */

nequs = 0; /* Number of equality constraints */

o_iprint = 1; /* Turn off status report printing */

o_idata = 1; /* Turn off report for data printing */

npenal = 2; /* Choose penalty function number */

path.t1_start = t1_s; /* set global values for endpoints */

path.t1_end = t1_e;

path.t2_start = t2_s;

path.t2_end = t2_e;

/* Choose dominant joint for motion */

if(fabs(t1_s-t1_e) >= fabs(t2_s-t2_e)){

_dominant_joint = 1;

} else {

_dominant_joint = 2;

}

couple = 0.0;

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

/*

* Find a better guess based on coupling factor

*/

best_t = 1000.0; /* set path time high */

/* Check over large coupling range */

for(couple = -0.5; couple <= 0.5; couple += 0.1){

/* Get guess based on couple factor */

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

/* Move points to sline data struct */

cross(xstrt, spline);

/* Keep if best path time */

if(spline[0].t[N_PTS-1] < best_t){

best_t = spline[0].t[N_PTS-1];

best_c = couple;

printf("#1 best time = %f\n", best_t);

}

}

/*

* Refine first guess even more on coupling factor

*/

best_t = 1000.0; /* set path time high */

/* Check over large coupling range */

for(couple = best_c-0.1; couple <= best_c+0.1; couple += 0.02){

/* Get guess based on couple factor */

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

/* Move points to spline data struct */

cross(xstrt, spline);

/* Keep if best path time */

if(spline[0].t[N_PTS-1] < best_t){

best_t = spline[0].t[N_PTS-1];

best_2 = couple;

printf("#2 best time = %f\n", best_t);

}

}

/* Find the best path again */

get_guess(xstrt, rmax, rmin, n, ncons, phi, best_2,

t1_s, t1_e, t2_s, t2_e);

/* Display best guess path results */

answer(&u, xstrt, phi, psi, n, ncons, nequs);

/* Put points in spline data struct */

cross(xstrt, spline);

/* Perform optimization if flagged */

if(opt_flag == TRUE){

/* Do random walk */

guess_start((.3/N_PTS), xstrt, rmin, rmax, n, 10000,

ncons, phi, &nviol);

/* move answer to first guess slot */

for(i = 0; i < n; i++) x[i] = xstrt[i];

/* Set up Hookes and Jeeves Search */

o_maxm = 10; /* Maximum number of seraches */

o_tol = 0.01; /* Tolerance for each search */

o_zero = 0.000001; /* Tolerance for zero value */

o_f = 0.2; /* Fract. of range to use for search */

o_g = 0.001; /* Multiple to reduce step by */

o_r = 1.0; /* for reduction of step */

o_reduce = o_g; /* for reduction of steps in search */

/* Call Hookes and Jeeves */

seek(n, ncons, nequs, npenal, rmax, rmin, xstrt, x, &u, phi,

psi, &nviol, w);

/* Print out answer */

answer(&u, x, phi, psi, n, ncons, nequs);

/* move answer to first guess */

for(i = 0; i < n; i++) xstrt[i] = x[i];

/* Do a random walk again */

guess_start((.03/N_PTS), xstrt, rmin, rmax, n, 6000,

ncons, phi, &nviol);

/* Print results of search */

answer(&u, xstrt, phi, psi, n, ncons, nequs);

/* move answer to first guess slot */

for(i = 0; i < n; i++) x[i] = xstrt[i];

o_maxm = 10; /* set up search, as above */

o_tol = 0.01;

o_zero = 0.000001;

o_f = 0.2;

o_g = 0.001;

o_r = 1.0;

o_reduce = o_g;

/* Yet another Hookes and Jeeves */

seek(n, ncons, nequs, npenal, rmax, rmin, xstrt, x, &u, phi,

psi, &nviol, w);

/* Print the answer */

answer(&u, x, phi, psi, n, ncons, nequs);

/* copy answer into first guess slot */

for(i = 0; i < n; i++) xstrt[i] = x[i];

/* The final random walk search */

guess_start((.001/N_PTS), xstrt, rmin, rmax, n, 50000,

ncons, phi, &nviol);

/* Proudly display the answer */

answer(&u, xstrt, phi, psi, n, ncons, nequs);

/* Move answer into spline struct */

cross(xstrt, spline);

}

}

void opter_path(spline, t1_s, t1_e, t2_s, t2_e, opt_flag)

parameters *spline;

double t1_s, t1_e, t2_s, t2_e;

int opt_flag;

/*

* OPTIMIZE PATH WITH COUPLING FACTOR ONLY

*

* This routine was written hastily for the purpose of providing an

* example of a path generated with coupling factors only. The results

* are as seen below, with an excessive refinement of coupling factor.

*

* Variables: t1_s, t2_s - start point for path

* t1_e, t2_e - end point for path

* opt_flag - if set the solution is highly optimized (ie = TRUE)

*

* Returns: spline - an optimized set of points on a spline

*

* November 29th, 1990.

*/

{

static double x[5*N_PTS], /* Work arrays for optimization */

phi[3], psi[1],

rmax[5*N_PTS],

rmin[5*N_PTS],

xstrt[5*N_PTS],

w[500],

u, /* Cost function */

couple, /* coupling factor work variable */

best_t, /* Best path time */

best_c, /* Best coupling factor */

best_2, /* Coupling factor from round 2 */

best_3, /* Coupling factor from round 3 */

best_4; /* Coupling factor from round 4 */

static int n, /* number of decision variables */

ncons, /* number of inequality constraints */

nequs, /* number of equality constraints */

nviol, /* number of constraint violations */

npenal; /* penalty function number */

static int i; /* Wrok variable */

/* optimization using seek */

n = 5*N_PTS; /* Number of decision variables */

ncons = 3; /* number of equality constraints */

nequs = 0;

path.t1_start = t1_s; /* Path endpoints */

path.t1_end = t1_e;

path.t2_start = t2_s;

path.t2_end = t2_e;

/* choose the dominant joint */

if(fabs(t1_s-t1_e) >= fabs(t2_s-t2_e)){

_dominant_joint = 1;

} else {

_dominant_joint = 2;

}

best_t = 1000.0; /* Set min. time very high */

/* cycle thruogh coupling factors */

for(couple = -0.5; couple <= 0.5; couple += 0.1){

/* Find a good guess for couple fact.*/

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

/* copy path points to spline struct */

cross(xstrt, spline);

/* if time is better, keep path */

if(spline[0].t[N_PTS-1] < best_t){

best_t = spline[0].t[N_PTS-1];

best_c = couple;

printf("#1 best time = %f\n", best_t);

}

}

/*

* The next three loops perform like the last one, except that they

* take smaller steps in the coupling term, to refine the solution.

*/

best_t = 1000.0;

for(couple = best_c-0.1; couple <= best_c+0.1; couple += 0.02){

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

cross(xstrt, spline);

if(spline[0].t[N_PTS-1] < best_t){

best_t = spline[0].t[N_PTS-1];

best_2 = couple;

printf("#2 best time = %f\n", best_t);

}

}

best_t = 1000.0;

for(couple = best_2-0.02; couple <= best_2+0.02; couple += 0.004){

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

cross(xstrt, spline);

if(spline[0].t[N_PTS-1] < best_t){

best_t = spline[0].t[N_PTS-1];

best_3 = couple;

printf("#3 best time = %f\n", best_t);

}

}

best_t = 1000.0;

for(couple = best_3-0.004; couple <= best_3+0.004; couple += 0.0008){

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

cross(xstrt, spline);

if(spline[0].t[N_PTS-1] < best_t){

best_t = spline[0].t[N_PTS-1];

best_4 = couple;

printf("#4 best time = %f at couple = %f\n",

best_t, best_4);

}

}

/* Regenerate the best path */

get_guess(xstrt, rmax, rmin, n, ncons, phi, best_4,

t1_s, t1_e, t2_s, t2_e);

/* Print out the answer */

answer(&u, xstrt, phi, psi, n, ncons, nequs);

/* copy path points to spline struct */

cross(xstrt, spline);

}

void get_path(spline, couple, t1_s, t1_e, t2_s, t2_e, opt_flag)

parameters *spline;

double t1_s, t1_e, t2_s, t2_e, couple;

int opt_flag;

/*

* GET AN UNOPTIMIZED PATH

*

* This returns the simplest path possible.

*

* Variables: t1_s, t2_s - start point for path

* t1_e, t2_e - end point for path

* opt_flag - if set the solution is highly optimized (ie = TRUE)

*

* Returns: spline - an optimized set of points on a spline

*

* November 29th, 1990.

*/

{

static double x[5*N_PTS], /* Work Variables for Optimization */

phi[3],

psi[1],

rmax[5*N_PTS],

rmin[5*N_PTS],

xstrt[5*N_PTS],

w[500],

u; /* Cost variable storage */

static int n, /* Number of decision variables */

ncons, /* Number of Equality Constraints */

nequs, /* Number of Equality constraints */

nviol, /* Number of constraint violations */

npenal; /* Penalty function number */

static int i; /* Work Variable */

/* optimization using seek */

n = 5*N_PTS; /* Number of decision variables */

ncons = 3; /* number of inequality constraints */

nequs = 0; /* number of equality constraints */

path.t1_start = t1_s; /* set up path endpoints */

path.t1_end = t1_e;

path.t2_start = t2_s;

path.t2_end = t2_e;

/* Determine dominant joint */

if(fabs(t1_s-t1_e) >= fabs(t2_s-t2_e)){

_dominant_joint = 1;

} else {

_dominant_joint = 2;

}

/* get path guess */

get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,

t1_s, t1_e, t2_s, t2_e);

/* print out answer */

answer(&u, xstrt, phi, psi, n, ncons, nequs);

/* copy path points to spline struct */

cross(xstrt, spline);

}

void get_guess(xstrt, rmax, rmin, n, ncons, phi, couple, t1_s, t1_e, t2_s, t2_e)

double *xstrt, *rmax, *rmin, t1_s, t1_e, t2_s, t2_e, couple, *phi;

int n, ncons;

/*

* DETERMINE A SET OF POINTS ON A PATH

*

* This algorithm will find a set of points between start and stop angles

* of the robot. The algorithm uses cosine shapes, and utlizes the

* coupling factor.

*

* Variables: t1_s, t2_s - start angles for manipulator path

* t1_e, t2_e - end angles for manipulator path

* couple - the coupling factor to be used when generating path

* n - number of decision variables

* ncons - number of equality constraints

*

* Returns: xstrt - The initial path guess points

* rmax - maximum values for parameters

* rmin - minimum values for params

* phi - array of inequality constraint violations

*

* Novemeber 29th, 1990.

*/

{

static double diff_1, /* distance for joint 1 to move */

diff_2, /* distance for joint 2 to move */

max_1, /* maximum angle for joint 1 */

min_1, /* minimum angle for joint 1 */

max_2, /* maximum angle for joint 2 */

min_2, /* minimum angle for joint 2 */

offset, /* work variable for path calc. */

offset_c, /* work variable for control points */

fact, /* for path time reduction */

temp, /* work variable */

multy; /* step size reduction */

static int i, /* work variable counters */

j,

k,

count_1, /* counters for failures */

count_2,

count_3;

diff_1 = t1_e - t1_s; /* Distance to be moved for joints */

diff_2 = t2_e - t2_s;

if(t1_s > t1_e){ /* Find maximum/minimum angles for */

min_1 = t1_e; /* each joint */

max_1 = t1_s;

} else {

min_1 = t1_s;

max_1 = t1_e;

}

if(t2_s > t2_e){

min_2 = t2_e;

max_2 = t2_s;

} else {

min_2 = t2_s;

max_2 = t2_e;

}

fact = 5.0*3.1415926/(double)n; /* Factor for time to angle */

/* Loop to generate points */

for(i = 0; i < n; i = i + 5){

/* get angle from count */

offset = fact * (double)( (int)(i/5));

offset_c = fact * ((double)( (int)(i/5)) + 0.6667);

/*

* Calculate points for each joint, and also estimates

* of max/min for optimization routines. The coupling

* terms are obviously applied here.

*/

xstrt[i] = t1_s + diff_1*(0.5 - 0.5*cos(offset))

+ couple*(sin(offset))*(diff_2 - diff_1);

rmax[i] = max_1 + fabs(diff_1 - diff_2)*couple;

rmin[i] = min_1 - fabs(diff_1 - diff_2)*couple;

xstrt[i+1] = t2_s + diff_2*(0.5 - 0.5*cos(offset))

+ couple*(sin(offset))*(diff_1 - diff_2);

rmax[i+1] = max_2 + fabs(diff_1 - diff_2)*couple;

rmin[i+1] = min_2 - fabs(diff_1 - diff_2)*couple;

xstrt[i+2] = t1_s + diff_1*(0.5 - 0.5*cos(offset_c))

+ couple*(sin(offset_c))*(diff_2 - diff_1);

rmax[i+2] = max_1 + fabs(diff_1 - diff_2)*couple;

rmin[i+2] = min_1 - fabs(diff_1 - diff_2)*couple;

xstrt[i+3] = t2_s + diff_2*(0.5 - 0.5*cos(offset_c))

+ couple*(sin(offset_c))*(diff_1 - diff_2);

rmax[i+3] = max_2 + fabs(diff_1 - diff_2)*couple;

rmin[i+3] = min_2 - fabs(diff_1 - diff_2)*couple;

rmax[i+4] = 4.0; /* Time step size */

rmin[i+4] = 0.1;

xstrt[i+4] = 5.0*25.0/(double)n;

}

xstrt[4] = 5.0*50.0/(double)n; /* Set final time step */

xstrt[n-1-5] = 5.0*50.0/(double)n;

/*

* This small optimization method was discussed in the thesis. The

* path above was quite slow, thus the time points will be slowly

* squeezed into a tighter path time.

*/

multy = .97; /* Reduction of step size */

for(i = 0; i < 200; i++){ /* Loop for reduction */

count_2 = 0; /* initialize counters for */

count_3 = 0; /* checking failures */

for(j = 4; j < n-5; j = j + 5){ /* only examine time steps */

temp = xstrt[j];/* store value temporarily */

xstrt[j] *= multy; /* update current time step */

const(xstrt, ncons, phi);/* check for const. viols. */

count_1 = 0; /* reset constriants counter */

count_2++; /* update loop counter */

/* if violation, then undo change */

for(k = 0; k < ncons; k++){

if(phi[k] < 0.0){

xstrt[j] = temp;

count_1++; /* count failure */

}

}

if(count_1 > 0) count_3++; /* add to failure counter */

}

/* if no changes made in last run */

/* then reduce step size and continue*/

if(count_2 == count_3){

multy *= 1.003;

if(multy > .9999) return;

}

}

}

void cross(x, spline)

double *x;

parameters *spline;

/*

* COPY PATH POINTS TO SPLINES

*

* This is a utility to save space by putting all of the copying routines

* in a single function. The routine takes points on the path, as used by

* the optimization routines, and copies them into the spline structure.

* The spline parameters must be calculated separately.

*

* Variables: x - the list of points in the path

*

* Returns: spline - the strucutre used for Bezier spline segments

*

* November 29th, 1990.

*/

{

static int i; /* A counter work variable */

static double time; /* cummulative path time */

time = 0.0; /* set sum to zero */

/*

* Loop through each point and copy it

*/

for(i = 0; i < N_PTS; i++){

spline[0].y1[i] = x[i*5];

spline[0].y2[i] = x[i*5 + 1];

spline[0].control1[i] = x[i*5 + 2];

spline[0].control2[i] = x[i*5 + 3];

spline[0].t[i] = time;

time += x[i*5 + 4]; /* add time to array */

}

spline[0].y1[0] = path.t1_start; /* Add start/stop points to spline */

spline[0].y1[N_PTS-1] = path.t1_end;

spline[0].y2[0] = path.t2_start;

spline[0].y2[N_PTS-1] = path.t2_end;

}

void guess_start(rr, xstrt, rmin, rmax, n, max_guess, ncons, phi, nviols)

double *xstrt, *rmin, *rmax, *phi, rr;

int n, max_guess, ncons, *nviols;

/*

* RANDOM WALK SEARCH

*

* The random walk search is performed here. This routine shares traits with

* the optimization methods of Siddall.

*

* Variables: xstrt - initial starting point guess

* rr - initial step size

* rmin - suggested minimums for decision variables

* rmax - suggested maximum for decision variables

* n - number of decision variables

* max_guess - maximum number of guesses allowed

* ncons - number of inequality constraints

*

* Returns: xstrt - improved guess for path

* phi - inequality constraint array

* nviols - number of constraint violations

*

* Novemeber 29th, 1990.

*/

{

static int i, /* Work counter variables */

j,

k,

viols, /* number of constraint violations */

suc_flag, /* A flag to indicate success */

e_count, /* error count */

s_count, /* step count */

alt; /* */

static double aa[N_PTS*5], /* Random number work array */

work[N_PTS*5], /* work array */

work2[N_PTS*5], /* another work array */

umin = 1.0e50, /* current cost threshold */

umin2 = 1.0e50, /* another cost storage */

uu, /* cost function storage */

uu2, /* another cost function var. */

psi[1]; /* array of inequality constraints */

suc_flag = 0; /* indicate no success */

e_count = 0; /* indicate no errors */

s_count = 0; /* indicate no successes */

alt = 0; /* */

const(xstrt, ncons, phi); /* check constraints for first guess */

for(i = 0; i < ncons; i++){ /* loop through all constraints */

if(phi[i] < 0.0){ /* check for violation */

printf("Constraint %d Violation in first guess\n", i);

e_count++;

/* indicate error */ }

}

/* Copy first guess into storage */

for(i = 0; i < n; i++) work[i] = xstrt[i];

if(e_count == 0){ /* Check for good first guess */

ureal(xstrt, &umin); /* get cost of solution */

suc_flag++; /* increment number of successes */

printf("First guess was good with u=%f\n", umin);

} else { /* if a bad first guess */

rr = 0.1; /* initialize step new size */ }

/*

* The main search loop

*/

for(i = 0; i < max_guess; i++){ /* loop for max. number of guesses*/

frandn(aa, n, 1); /* get an array of random numbers*/

if(i < 500){

/* for first 500 guesses, use a*/

/* bias towards time decreases */

for(j = 0; j < n; j = j + 5){ /* only do times */

xstrt[j+4] = rr*(rmax[j+4]

- rmin[j+4])*(aa[j+4]-0.75) + work[j+4];

}

} else { /* Normal unbiased steps */

for(j = 0; j < n; j++){ /* loop for all decisions */

xstrt[j] = rr*(rmax[j]

- rmin[j])*(aa[j]-0.5) + work[j];

}

}

ureal(xstrt, &uu); /* get cost of path */

s_count++; /* get count of steps */

if(s_count > 1000){ /* if 1000 steps */

rr = rr * 0.5; /* decrease step size */

if(rr < .0001) rr = .01; /* limit step size */

s_count = 0; /* reset number of steps */

}

if((i % 100) == 0) printf("Run %d \n", i); /* quick report */

if(uu < umin){ /* see if cost has improved */

const(xstrt, ncons, phi); /* check for violations */

viols = 0; /* reset error counter */

for(j = 0; j < ncons; j++){ /* check all constraints */

if(phi[j] < 0.0) viols++;

}

if(viols == 0){ /* if no constraint violations */

suc_flag++; /* increase count of successes */

for(j = 0; j < n; j++){ /* update work array */

work[j] = xstrt[j];

}

umin = uu; /* set new min cost */

printf("Run %d was good u= %lf\n", i, uu);

}

}

}

/*

* update array with improved value, and report success/failure

*/

if(suc_flag == 0){

printf("failed\n");

for(i = 0; i < n; i++){

xstrt[i] = work[i];

}

} else {

printf("%d Solutions found with no violations\n", suc_flag);

for(i = 0; i < n; i++){

xstrt[i] = work[i];

}

}

nviols[0] = suc_flag; /* returns number of failures */

}

void ureal(x, u)

double *x, *u;

/*

* OBJECTIVE FUNCTION

*

* This function is called by the optimization routines to obtain the value

* of the objective function.

*

* Variable: x - array of decision variables

*

* Returns: u - the value of the objective function

*

* November 30th, 1990.

*/

{

static int i; /* a work variable */

static parameters spline; /* Structure to store path info in */

u[0] = 0.0; /* set objective to 0 to start */

/*

* This objective only considers time of the path

*/

for(i = 0; i < 5*N_PTS-4; i = i + 5){ /* look at time of steps */

u[0] += fabs(x[i+4]);

}

u[0] = u[0]*u[0]*u[0]*u[0]*u[0]*u[0]; /* put value to sixth power */

cross(x, &spline); /* put path points in spline struct */

get_params(&spline, N_PTS); /* get spline parameters for path */

for(i = 0; i < N_PTS-1; i++){ /* look at each segment */

if(_dominant_joint == 1){ /* consider acceleration and vel. */

u[0] += spline.a1[i] * spline.a1[i];

u[0] -= fabs(spline.b1[i]);

} else {

u[0] += spline.a2[i] * spline.a2[i];

u[0] -= fabs(spline.b2[i]);

}

}

}

void const(x, ncons, phi)

double *x, *phi;

int ncons;

/*

* CONSTRAINT FUNCTION

*

* The constraints of the function are considered in this subroutine.

* All details are described in the body of the thesis.

*

* Variables: x - array of decision variables

* ncons - number of constraints

*

* Returns: phi - array of constraint violations

*

* November 30th, 1990.

*/

{

static int i; /* work variable */

static parameters spline; /* spline struct */

static double time, /* time counter */

pos_1, pos_2, /* joint positions */

vel_1, vel_2, /* joint velocities */

acc_1, acc_2, /* joint accelerations */

tor_1, tor_2, /* joint torques */

maxx, /* time for path */

sum1, sum2; /* torque below limits */

for(i = 0; i < ncons; i++) phi[i] = 0.000; /* empty all constraints */

cross(x, &spline); /* path into spline struct */

get_params(&spline, N_PTS); /* fit spline to points */

maxx = spline.t[N_PTS-1]; /* get path time */

sum1 = sum2 = 0.0; /* zero torque slackness */

/*

* Take small time steps along path

*/

for(time = -1.0; time < maxx+1.0; time += 0.1){

get_value(N_PTS, spline, time, /* get point on spline */

&pos_1, &vel_1, &acc_1, &pos_2, &vel_2, &acc_2);

/* get torques at position */

inverse_dynamics(pos_1, pos_2, vel_1, vel_2,

acc_1, acc_2, &tor_1, &tor_2);

sum1 += TMAX_T1 - fabs(tor_1); /* find diff between joint */

sum2 += TMAX_T2 - fabs(tor_2); /* torques and the limits */

/*

* If torques above limits, then indicate violation

*/

if(fabs(tor_1) > TMAX_T1) phi[0] += TMAX_T1 - fabs(tor_1);

if(fabs(tor_2) > TMAX_T2) phi[0] += TMAX_T2 - fabs(tor_2);

}

/*

* Check all spline segments for time violations, or position

* violations.

*/

for(i = 0; i < (5*N_PTS-5); i = i + 5){

if(x[i + 4] < 0.0001) phi[1] += -fabs(x[i + 4]);

if(fabs(x[i+0]) > 180.0) phi[1] += -fabs(x[i+0]);

if(fabs(x[i+1]) > 180.0) phi[1] += -fabs(x[i+1]);

if(fabs(x[i+2]) > 180.0) phi[1] += -fabs(x[i+2]);

if(fabs(x[i+3]) > 180.0) phi[1] += -fabs(x[i+3]);

}

}

void equal(x, psi, nequs)

double *x, *psi;

int nequs;

/*

* EQUALITY CONSTRAINTS

*

* This module exists only to satisfy the needs of Siddall’s routines. The

* values are set to indicate that there are no equality constraint

* violations.

*

* Variables: x - array of decision variables

* nequs - number of equality constraints

*

* Returns: psi - array of equality constraint violations

*

* November 30th, 1990.

*/

{

static int i;

for(i = 0; i < nequs; i++) psi[i] = 0.0; /* set constraints to zero */

}