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.
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.
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.
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.
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
#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
* Functions contained within this module
void path_calc(), /* Functions Contained within this module */
typedef struct { /* Stores motion end points */
typedef struct { /* Defines Bezier Spline params */
endpoints path; /* A storage variable */
int _dominant_joint; /* indicator of joint which */
* This routine will call the test function to demonstrate the optimization
* routines for paths. Removed when used in other programs.
* 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.
static parameters spline; /* Store Spline Parameters here */
static int i; /* Work Variables */
t1_start = -90.0; /* set path endpoints */
init_inverse_dynamics(); /* Set up dynamics */
opt_path(&spline, t1_start, t1_end, t2_start, t2_end, TRUE);
path_calc(spline, N_PTS); /* make a file of optimal values */
* This method will create a file of results from the optimization
* subroutines. This routine is made for testing purposes, and
* Variables: spline: contains the table of values to construct the spline
* N: The number of points on the path
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 */
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 */
* 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
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 */
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).
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];
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;
slope1 = 3.0*(p3: p2); /* Find slope at end of segment */
p1 = (time_2/time_1)*slope2/3.0 + curve[0].y2[i];
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;
time_1 = time_2; /* Set time for previous segment */
time_2 = curve[0].t[N-1] /* Find Time for last step */
* 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];
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 */
p1 = (time_2/time_1)*slope2/3.0 + curve[0].y2[N-2];
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;
void get_value(N, spline, time, der1_0, der1_1, der1_2, der2_0, der2_1, der2_2)
double time, *der1_0, *der1_1, *der1_2, *der2_0, *der2_1, *der2_2;
* 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
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 */
/* set angles to start value */
/* or set angles to end values */
/* or find relevant spline segment */
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
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
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)
double t1_s, t1_e, t2_s, t2_e;
* 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
static double x[5*N_PTS], /* Work variables for optimization */
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 */
/* Choose dominant joint for motion */
if(fabs(t1_s-t1_e) >= fabs(t2_s-t2_e)){
get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,
* 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,
/* Move points to sline data struct */
if(spline[0].t[N_PTS-1] < best_t){
best_t = spline[0].t[N_PTS-1];
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,
/* Move points to spline data struct */
if(spline[0].t[N_PTS-1] < best_t){
best_t = spline[0].t[N_PTS-1];
printf("#2 best time = %f\n", best_t);
/* Find the best path again */
get_guess(xstrt, rmax, rmin, n, ncons, phi, best_2,
/* Display best guess path results */
answer(&u, xstrt, phi, psi, n, ncons, nequs);
/* Put points in spline data struct */
/* Perform optimization if flagged */
guess_start((.3/N_PTS), xstrt, rmin, rmax, n, 10000,
/* 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 */
seek(n, ncons, nequs, npenal, rmax, rmin, xstrt, x, &u, phi,
answer(&u, x, phi, psi, n, ncons, nequs);
/* move answer to first guess */
for(i = 0; i < n; i++) xstrt[i] = x[i];
guess_start((.03/N_PTS), xstrt, rmin, rmax, n, 6000,
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 */
/* Yet another Hookes and Jeeves */
seek(n, ncons, nequs, npenal, rmax, rmin, xstrt, x, &u, phi,
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,
/* Proudly display the answer */
answer(&u, xstrt, phi, psi, n, ncons, nequs);
/* Move answer into spline struct */
void opter_path(spline, t1_s, t1_e, t2_s, t2_e, opt_flag)
double t1_s, t1_e, t2_s, t2_e;
* 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
static double x[5*N_PTS], /* Work arrays for optimization */
couple, /* coupling factor work variable */
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 */
n = 5*N_PTS; /* Number of decision variables */
ncons = 3; /* number of equality constraints */
path.t1_start = t1_s; /* Path endpoints */
/* choose the dominant joint */
if(fabs(t1_s-t1_e) >= fabs(t2_s-t2_e)){
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,
/* copy path points to spline struct */
/* if time is better, keep path */
if(spline[0].t[N_PTS-1] < best_t){
best_t = spline[0].t[N_PTS-1];
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.
for(couple = best_c-0.1; couple <= best_c+0.1; couple += 0.02){
get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,
if(spline[0].t[N_PTS-1] < best_t){
best_t = spline[0].t[N_PTS-1];
printf("#2 best time = %f\n", best_t);
for(couple = best_2-0.02; couple <= best_2+0.02; couple += 0.004){
get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,
if(spline[0].t[N_PTS-1] < best_t){
best_t = spline[0].t[N_PTS-1];
printf("#3 best time = %f\n", best_t);
for(couple = best_3-0.004; couple <= best_3+0.004; couple += 0.0008){
get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,
if(spline[0].t[N_PTS-1] < best_t){
best_t = spline[0].t[N_PTS-1];
printf("#4 best time = %f at couple = %f\n",
/* Regenerate the best path */
get_guess(xstrt, rmax, rmin, n, ncons, phi, best_4,
answer(&u, xstrt, phi, psi, n, ncons, nequs);
/* copy path points to spline struct */
void get_path(spline, couple, t1_s, t1_e, t2_s, t2_e, opt_flag)
double t1_s, t1_e, t2_s, t2_e, couple;
* 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
static double x[5*N_PTS], /* Work Variables for Optimization */
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 */
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 */
/* Determine dominant joint */
if(fabs(t1_s-t1_e) >= fabs(t2_s-t2_e)){
get_guess(xstrt, rmax, rmin, n, ncons, phi, couple,
answer(&u, xstrt, phi, psi, n, ncons, nequs);
/* copy path points to spline struct */
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;
* 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
* 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
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 */
multy; /* step size reduction */
static int i, /* work variable counters */
count_1, /* counters for failures */
diff_1 = t1_e: t1_s; /* Distance to be moved for joints */
if(t1_s > t1_e){ /* Find maximum/minimum angles for */
min_1 = t1_e; /* each joint */
fact = 5.0*3.1415926/(double)n; /* Factor for time to angle */
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 */
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 */
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*/
* 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
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
spline[0].control1[i] = x[i*5 + 2];
spline[0].control2[i] = x[i*5 + 3];
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;
* The random walk search is performed here. This routine shares traits with
* the optimization methods of Siddall.
* Variables: xstrt: initial starting point guess
* 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
static int i, /* Work counter variables */
viols, /* number of constraint violations */
suc_flag, /* A flag to indicate success */
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 */
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);
/* 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 */ }
for(i = 0; i < max_guess; i++){ /* loop for max. number of guesses*/
frandn(aa, n, 1); /* get an array of random numbers*/
/* for first 500 guesses, use a*/
/* bias towards time decreases */
for(j = 0; j < n; j = j + 5){ /* only do times */
: 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 */
: 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(viols == 0){ /* if no constraint violations */
suc_flag++; /* increase count of successes */
for(j = 0; j < n; j++){ /* update work array */
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
printf("%d Solutions found with no violations\n", suc_flag);
nviols[0] = suc_flag; /* returns number of failures */
* This function is called by the optimization routines to obtain the value
* Variable: x: array of decision variables
* Returns: u: the value of the objective function
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] = 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] += spline.a2[i] * spline.a2[i];
* 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
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 */
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);
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
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]);
* This module exists only to satisfy the needs of Siddall’s routines. The
* values are set to indicate that there are no equality constraint
* Variables: x: array of decision variables
* nequs: number of equality constraints
* Returns: psi: array of equality constraint violations
for(i = 0; i < nequs; i++) psi[i] = 0.0; /* set constraints to zero */