33. Appendix J: Path Planning Function Module

33.1 J.1 Introduction:

There are a set of functions which are high level, but still below the user interface. These functions were collected into a single module to ease interface to applications. These functions include maintenance of basic data structures for training data, and for path endpoints. The routines also serve to perform basic functions of path generation, training, storing/loading data, etc. The intention of this structure is that the program can be run in two modes. The first will be with the user interface, to allow interactive control of functions and operations. The second is in batch mode. In this case the program may be set up to run entirely in the background of the computer. This module, and the routines it calls should be machine independent, the user interface is not.

There are two important data lists stored for path planning. The first is for storing path endpoints. The endpoints are stored as joint angles. There are two pointers associated with this list. One points to the last position, the other points into the list. Together, these pointers form the basis for a full range of editing, and manipulation routines.

The second important data array is an array of structures which keep trajectory information along the path. These trajectories include joint positions, velocities, accelerations, and torques (as well as other information). This array of structures is used for both short term, and temporary storage of sample paths. This is where all of the path generation, analysis, and display routines look for data.

Path generation is an important feature of this module. The paths are generated by different methods, and placed in the trajectory array. This section has been set up with terminal calling routine to increase the modularity of the design.

33.2 J.2 Endpoint Buffer Subroutines:

b_load(): When a list of path endpoints has been saved in an ASCII file, they may be recovered with this routine. The data structure may be determined by examining the function itself. If the number of points in the file is proceeded by the letter ‘C’ then the input values will be treated as points in cartesian space (otherwise the joint angles at the endpoints are saved). As a result, the points will be converted to joint angles, to produce a total of four point for each set of endpoints (i.e. elbow up to elbow up, elbow up to elbow down, elbow down to elbow down, elbow down to elbow up).

b_save(): The path endpoints (as joint angles) will be saved in an ASCII file. The file format may be determined by examining the function itself.

b_down(): This function will move a pointer through the path endpoint list. This pointer is used for operations involving additions, removals, and edits to this list. The pointer is only moved down by one position.

b_up(): This pointer does the opposite of b_down(). The pointer will be moved toward the top (or first) of the list of path endpoints. This only moves the pointer by one position.

b_goto(): The current pointer to the list of path endpoints can be set with this function. As long as the new list location is valid, the results will be updated.

b_delete(): The pointer to the current position in the buffer is used to refer to an element to be deleted. All of the path endpoints after the current position are moved forward to cover it over, and decrease the size of the list by one.

b_insert(): The current pointer is used as the insertion point for a new set of path endpoints. All endpoints from the pointer on will be moved back by one position. The empty location will be filled by values from the current endpoint variables (stored in the structure ‘current’).

33.3 J.3 Training Data Subroutines:

b_tload(): Points during motion of the manipulator are stored in the training data structure. When these values are to be stored, this subroutine may be used to keep them in an ASCII file. The file structure may be seen by examining the function.

b_tsave(): This routine will load points during motion which include joint positions, velocities, accelerations, and torques. The values will be stored directly in the training data array. The file structure may be determined from examining the function.

33.4 J.4 Robot Specific Subroutines:

kin_forward(): When the joint angles are passed to this routine, the forward kinematics are found to give the position in space of the end effector. The position in space is returned.

kin_inverse(): Given a position in space, the inverse kinematics will be performed to find the joint angles. The particular inverse kinematics solution found is determined by a flag for an elbow up, or elbow down solution.

33.5 J.5 Path Planning Subroutines:

get_trajectory(): This subroutine serves as a main terminal point for path planning subroutines. The subroutine accepts the path endpoints (as joint angles), the array to place the motion samples into, and the path planner to be used. The appropriate path planning routine is then called to generate the path, and the results are returned. The routines called include all algorithms for kinematic path planning, torque path planners (all levels of optimization), and neural network control. The subroutine will also produce two paths for each set of endpoints if a particular flag is set. These paths are both in opposite directions between the given endpoints.

tra_1(): This routine will fill the provided trajectory list with the motion between arbitrary endpoints, based upon a maximum velocity control scheme. Setting the ‘train_opt’ flag will enforce the joint limits of angles. A few steps will be added to the nd of every path to ensure that the robot may be seen to stand still.

tra_2(): Maximum Acceleration kinematic path planning is available with this subroutine. The path endpoints are used to generate a motion which is stored in the provided trajectory structure. If the ‘train_opt’ flag is set the joint position limits will be enforced. A few steps are added to the end of every motion to ensure that an example of the manipulator standing still.

a_update(): This is a utility function used by the path planners to simplify the update of joint motions, based on kinematic constraints only. The function will use the starting conditions of the step, and the acceleration over the step, to update the joint positions at the end of the time step. This function only deals with a single joint for every call to ensure modularity.

tra_3(): A simple path based upon a single coupling factor may be generated with this routine. This was a prototyping routine, thus the coupling factor is loaded from an ASCII file. The path for that coupling factor is then determined. The path has only had the time steps reduced, and has not had the positions along the path shifted.

tra_4(): Maximum torque limits are used along with a combination of all the optimization techniques to obtain a near-optimal path. This path planner was the one used for generating most of the maximum torque path plans.

tra_5(): Coupling factors are used to generate a sub-optimal motion. This method calls routines which will used different coupling factors to estimate better paths. If the ‘train_opt’ flag is set then the coupling factor will be refined four times, if not the coupling factor will be refined two times.

 

/*

* ROBOT TRAJECTORY TRAINING PROGRAM

*

* This code contains a number of functions which are necessary, but

* not needed by the User Interface. This includes robot specific,

* and trajectory maintenance routines. Trajectories are also generated

* here.

*

* Note that the endpoints are stored in a section refered to as the

* trajectory buffer. The Buffer is used to path endpoints in general

* here.

*

* Also note that in general terminolgy conventions are not strictly adhered

* to in this module of code.

*

* December 2nd, 1990.

*/

 

 

#include "/usr/people/hugh/thesis/src/test/tester.c"

 

 

void a_update(); /* the only local function */

 

 

 

 

 

void b_load(file)

char *file;

/*

* LOAD TRAJECTORIES FROM FILE

*

* The buffer for the set of test (or training) trajectories is loaded from

* an ASCII file.

*

* Variables: file = the file which contains the trajectories to be loaded

*

* December 2nd, 1990.

*/

{

FILE *in; /* Input file */

static int i; /* work variable */

static char worker[50]; /* Input string */

static double x_s, y_s, /* path start */

x_e, y_e, /* path end */

rad; /* maximum arm raduis */

 

rad = LENGTH_1 + LENGTH_2; /* set arm limit */

 

/*

* Open the file named and get the number of trajectories in it

*/

in = fopen(file, "r"); /* read only */

if(in != NULL){ /* check for file exists */

fscanf(in, "%s\n", worker); /* read info line */

if(worker[0] == ’C’){ /* check for correct format */

/* in this case cartesian */

end_of_buffer = atoi(&worker[1]); /* how many in file */

/*

* load in all the trajectories from the file, into

* the buffer and convert them from x-y to the four

* joint equivalents.

*/

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

/* get endpoints from file */

fscanf(in, "%lf,%lf,%lf,%lf\n",

&x_s, &y_s, &x_e, &y_e);

/* convert cartesian endpoints*/

/* to all four sets of joint */

/* angles */

kin_inverse(

&buffer[i*4+0].t1_start,

&buffer[i*4+0].t2_start,

x_s*rad, y_s*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+1].t1_start,

&buffer[i*4+1].t2_start,

x_s*rad, y_s*rad, ELBOW_DOWN);

kin_inverse(

&buffer[i*4+2].t1_start,

&buffer[i*4+2].t2_start,

x_s*rad, y_s*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+3].t1_start,

&buffer[i*4+3].t2_start,

x_s*rad, y_s*rad, ELBOW_DOWN);

kin_inverse(

&buffer[i*4+0].t1_end,

&buffer[i*4+0].t2_end,

x_e*rad, y_e*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+1].t1_end,

&buffer[i*4+1].t2_end,

x_e*rad, y_e*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+2].t1_end,

&buffer[i*4+2].t2_end,

x_e*rad, y_e*rad, ELBOW_DOWN);

kin_inverse(

&buffer[i*4+3].t1_end,

&buffer[i*4+3].t2_end,

x_e*rad, y_e*rad, ELBOW_DOWN);

}

end_of_buffer *= 4; /* there are four time more */

} else { /* load joint angles */

end_of_buffer = atoi(worker); /* number of endpoints */

/*

* load in all the trajectories from the file, into

* the buffer.

*/

/* load joint angles for path */

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

fscanf(in, "%lf,%lf,%lf,%lf\n",

&buffer[i].t1_start,

&buffer[i].t2_start,

&buffer[i].t1_end,

&buffer[i].t2_end);

}

}

}

/*

* close the file and point to start of trajectories

*/

fclose(in);

buffer_point = 0;

}

 

 

 

 

 

void b_save(file)

char *file;

/*

* SAVE TRAJECTORY INFORMATION

*

* The trajectory endpoints will be saved to an ASCII file with this

* subroutine.

*

* Variables: file = the file which contains the trajectories to be saved

*

* March 8th, 1990.

*/

{

FILE *out; /* The output file */

static int i; /* A work variable */

 

/*

* Open file and write the number of trajectories to be saved.

*/

out = fopen(file, "w"); /* write only */

fprintf(out, "%d\n", end_of_buffer); /* save number of endpoints */

/*

* write then trajectories to the file

*/

for(i = 0; i < end_of_buffer; i++){ /* save all endpoints */

fprintf(out, "%f,%f,%f,%f\n",

buffer[i].t1_start, buffer[i].t2_start,

buffer[i].t1_end, buffer[i].t2_end);

}

/*

* close file

*/

fclose(out);

}

 

 

 

 

 

void b_tload(file, start, n)

char *file;

int *n,

start;

/*

* LOAD TRAJECTORIES FROM FILE

*

* The buffer for the set of test (or training) trajectories is loaded from

* a file. These points are actual points during the motion of the arm.

*

* Variables: file: the file which contains the trajectories to be loaded

* start: the location to start loading to

*

* Returns: n: number of trajectories loaded

*

* March 8th, 1990.

*/

{

FILE *in;

static int i;

 

/*

* Open the file named and get the number of trajectories in it

*/

in = fopen(file, "r"); /* read only */

/*

* load in all the trajectories from the file, into the buffer

*/

n[0] = 0; /* default to zero loaded */

if(in != NULL){ /* if file exists */

fscanf(in, "%d\n", n); /* get number of points */

/* loop to read in variables */

/* sorry about format line */

for(i = start; i < (start + n[0]); i++){

fscanf(in,

"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",

&t_set[i].x_target, &t_set[i].y_target,

&t_set[i].x_position, &t_set[i].y_position,

&t_set[i].x_velocity, &t_set[i].y_velocity,

&t_set[i].x_acceleration, &t_set[i].y_acceleration,

&t_set[i].t1_target, &t_set[i].t2_target,

&t_set[i].t1_position, &t_set[i].t2_position,

&t_set[i].t1_velocity, &t_set[i].t2_velocity,

&t_set[i].t1_acceleration, &t_set[i].t2_acceleration,

&t_set[i].t1_torque, &t_set[i].t2_torque);

}

}

/*

* close the file and point to start of trajectories.

*/

fclose(in);

}

 

 

 

 

void b_tsave(file)

char *file;

/*

* SAVE TRAJECTORY INFORMATION

*

* The trajectories will be saved to an ASCII file with this subroutine. These

* trajectories are the actual points during a motion.

*

* Variables: file = the file which contains the trajectories to be saved

*

* March 8th, 1990.

*/

{

FILE *out; /* output file */

static int i; /* work variable */

 

/*

* Open file

*/

out = fopen(file, "w"); /* write only */

/*

* write the training set to the file

*/

fprintf(out, "%d\n", end_train); /* save the number of points */

for(i = 0; i < end_train; i++){ /* loop through entire set */

fprintf(out, /* another painful format */

"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",

t_set[i].x_target, t_set[i].y_target,

t_set[i].x_position, t_set[i].y_position,

t_set[i].x_velocity, t_set[i].y_velocity,

t_set[i].x_acceleration, t_set[i].y_acceleration,

t_set[i].t1_target, t_set[i].t2_target,

t_set[i].t1_position, t_set[i].t2_position,

t_set[i].t1_velocity, t_set[i].t2_velocity,

t_set[i].t1_acceleration, t_set[i].t2_acceleration,

t_set[i].t1_torque, t_set[i].t2_torque);

}

/*

* write last line and close file

*/

fclose(out);

}

 

 

 

 

 

void b_down()

/*

* MOVE DOWN TRAJECTORY LIST

*

* This moves the pointer to the trajectory buffer, down the list. It will

* cause the visible list to be updated to the new item.

*

* March 8th, 1990.

*/

{

/*

* increment pointer and check for end of buffer collision

*/

buffer_point++;

if(buffer_point >= end_of_buffer) buffer_point = end_of_buffer -1;

}

 

 

 

 

 

void b_up()

/*

* MOVES POINTER UP TRAJECTORY LIST

*

* The pointer will be moved to the top of the list. The display will be

* updated before return.

*

* March 8th, 1990.

*/

{

/*

* decrement pointer and check for start of buffer collision

*/

buffer_point--;

if(buffer_point < 0) buffer_point = 0;

}

 

 

 

 

 

void b_delete()

/*

* REMOVES TRAJECTORY AT CURRENT POINTER

*

* The trajectory in the buffer (at the current pointer) will be deleted

* from the list. This is irreversible.

*

* March 8th, 1990.

*/

{

static int i, /* a work variable */

flag; /* a success flag for delete */

/*

* Check for trajectory to delete

*/

if(end_of_buffer > 0){ /* if buffer not empty */

/*

* clear flag to indicate nothing deleted

*/

flag = 0; /* set no change flag */

/*

* Slide trajectories in file back to cover deleted traj.

*/

for(i = buffer_point+1; i <= end_of_buffer; i++){

buffer[i-1].t1_start = buffer[i].t1_start;

buffer[i-1].t2_start = buffer[i].t2_start;

buffer[i-1].t1_end = buffer[i].t1_end;

buffer[i-1].t2_end = buffer[i].t2_end;

flag = 1; /* flag change made */

}

/*

* Update end of buffer and current buffer pointer values.

*/

if(flag > 0){ /* if delete done */

end_of_buffer--; /* remove last element *

if((end_of_buffer == buffer_point)&&(end_of_buffer > 0))

buffer_point--; /* move location in list */

}

}

}

 

 

 

 

 

void b_insert()

/*

* INSERT DISPLAYED TRAJECTORY AT CURRENT LOCATION

*

* The points indicated as start and stop on the screen will be entered

* into the trajectory list, at the current pointer position. The other

* trajectories will be bumped down.

*

* March 8th, 1990.

*/

{

static int i; /* a work variable */

/*

* Slide everything in the buffer down one place to make space for new

*/

for(i = end_of_buffer; i > buffer_point; i--){

buffer[i].t1_start = buffer[i-1].t1_start;

buffer[i].t2_start = buffer[i-1].t2_start;

buffer[i].t1_end = buffer[i-1].t1_end;

buffer[i].t2_end = buffer[i-1].t2_end;

}

/*

* stick current start and stop in new empty spot.

*/

buffer[buffer_point].t1_start = current.t1_start;

buffer[buffer_point].t2_start = current.t2_start;

buffer[buffer_point].t1_end = current.t1_end;

buffer[buffer_point].t2_end = current.t2_end;

end_of_buffer++; /* add one to buffer size */

}

 

 

 

 

 

 

int b_goto(point)

int point;

/*

* GOTO POINTER

*

* This routine will set the pointer value to the indicated value.

* The routine will return a no_error state if sucessful.

*

* Variables: point = the new desired location for the buffer pointer

*

* Returns: error = returns NO_ERROR if sucessful

*

* March 11th, 1990.

*/

{

static int error; /* the error state */

 

error = ERROR; /* default is error */

/*

* If possible then change the current pointer to the desired value

*/

if((point >= 0) && (point < end_of_buffer) && (end_of_buffer > 0)){

buffer_point = point; /* update current line */

error = NO_ERROR; /* clear error */

}

return(error);

}

 

 

 

 

 

void kin_forward(t1, t2, x, y)

double t1, t2, *x, *y;

/*

* FORWARD KINEMATICS

*

* The forward kinematics will be calculated when this routine is called.

*

* Variables: t1, t2 = angles of manipulator joints

*

* Returns: x, y = position of manipulator end effector

*

* March 8th, 1990.

*/

{

x[0] = LENGTH_1*cos(PI*t1/180.0) + LENGTH_2*cos(PI*(t1+t2)/180.0);

y[0] = LENGTH_1*sin(PI*t1/180.0) + LENGTH_2*sin(PI*(t1+t2)/180.0);

}

 

 

 

 

 

void kin_inverse(t1, t2, x, y, conf)

double *t1, *t2, x, y;

int conf;

/*

* INVERSE KINEMATICS

*

* The inverse kinematics will be done, with consideration of the elbow

* up and down solutions.

*

* Variables: x, y = the cartesian location of the end effector

* conf = the elbow up/down solution (0 = elbow up)

*

* Returns: t1, t2 = the joint configuration of the manipulator

*

* March 26th, 1990.

*/

{

static double r, /* for trig in problem */

fact; /* elbow up/down correction */

 

r = sqrt(x*x + y*y); /* end effector from origin */

fact = 1.0; /* elbow up */

if(conf == 1) fact = -1.0; /* elbow down */

/* if solution is valid */

if(((double)(LENGTH_1-LENGTH_2) <= r) && (r > 0.0)

&& ((double)(LENGTH_1+LENGTH_2) >= r)){

t2[0]=fact*(180.0*acos((LENGTH_1*LENGTH_1+LENGTH_2*LENGTH_2-r*r)

/2.0/LENGTH_1/LENGTH_2)/PI: 180.0);

t1[0]=180.0*(fact*acos((LENGTH_1*LENGTH_1+r*r-LENGTH_2*LENGTH_2)

/2.0/LENGTH_1/r) + atan2(y, x))/PI;

}

}

 

 

 

 

 

void get_trajectory(plan, t1_s, t2_s, t1_e, t2_e, path, start, n)

int plan,

start,

*n;

double t1_s, t2_s,

t1_e, t2_e;

train_data *path;

/*

* TRAJECTORY GENERATION PROGRAM

*

* The different trajectory algorithms are available through this sub-routine.

*

* Variables: plan: the trajectory planner to be used

* x_s, y_s: the start point for the trajectory

* x_e, y_e: the end point for the trajectory

* path: the structure array to put the path into

* start: the start point for the path in the array

*

* Return: n: the number of path segments in the file

*

* March 8th, 1990.

*/

{

static double t1s, /* work variables to keep the different */

t2s, /* joint configurations for alternate */

t1e, /* training methods */

t2e,

t1sa,

t2sa,

t1ea,

t2ea;

static int keep[1]; /* a work variable for the path size */

n[0] = 0;

 

/*

* Find the different possible configurations for the path. This

* includes i) elbow down to elbow down, ii) elbow up to elbow up,

* iii) elbow down to elbow up, and iv) elbow up to elbow down.

*/

t1s = t1_s;

t2s = t2_s;

t1e = t1_e;

t2e = t2_e;

t1sa = t1_e;

t2sa = t2_e;

t1ea = t1_s;

t2ea = t2_s;

/*

* Apply the main controller cases if applicable.

*/

if(plan == CASE_1) tra_1(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_2) tra_2(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_3) tra_3(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_4) tra_4(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_5) tra_5(t1s, t2s, t1e, t2e, path, start, n);

 

/*

* If the training set it to be generated the reverse of paths may

* be generated here

*/

if((train_flag == 1) && (train_alt == 1) && (plan != NETWORK)

&& (plan != SET_FILE)){

if(plan == CASE_1)

tra_1(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_2)

tra_2(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_3)

tra_3(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_4)

tra_4(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_5)

tra_5(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

n[0] += keep[0];

}

 

/*

* If the neural network controller was called for then test the

* controller with only the current configuration.

*/

if(plan == NETWORK){

nt_trajectory(n_output, t1s, t2s, t1e, t2e, path, start, n);

}

 

/*

* If the neural network controller was called for, then test the

* controller with only the current configuration.

*/

if(plan == SET_FILE){

b_tload(t_file, start, n);

}

if(plan == NN_TRACK){

nt_tracker(n_output, t1s, t2s, t1e, t2e, path, start, n);

}

}

 

 

 

 

 

void tra_1(t1s, t2s, t1e, t2e, path, start, n)

int start, *n;

double t1s, t2s, t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (MAXIMUM VELOCITY)

*

* A Trajectory will be devised, based upon the maximum velocity constraint.

* This section is still in need of work.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* March 8th, 1990.

*/

{

static int i, /* work variable */

steps_1,

steps_2,

steps; /* number of calculated steps in path*/

static double t1d, /* total displacement of joints */

t2d,

t1_step, /* joint angle step per time step */

t2_step,

x_s, /* cartesian start and stop points */

y_s,

x_e,

y_e;

 

/*

* Find cartesian points in space

*/

kin_forward(t1s, t2s, &x_s, &y_s);

kin_forward(t1e, t2e, &x_e, &y_e);

if(train_up == 1){

if(t1s > 180.0) t1s = 360.0-t1s;

if(t2e > 180.0) t2e = 360.0-t2e;

if(t1s < -180.0) t1s = 360.0+t1s;

if(t2e < -180.0) t2e = 360.0+t2e;

}

/*

* find joint angle displacements

*/

t1d = t1e: t1s;

t2d = t2e: t2s;

/* It was discovered at this point that the */

/* correction of joint angles lead to a problem */

/* To solve this problem, both joint angles */

/* must be changed simultaneously. In effect */

/* this advocates a rule based control strategy */

/* Note that this may lead to a less than */

/* optimal path for the arm, but the results */

/* will not contain a discontinuity as */

/* introduced by a hard rule. */

if(train_opt != 0){

if(t1d > 180.0) t1d = t1d: 360.0;

if(t2d > 180.0) t2d = t2d: 360.0;

if(t1d < -180.0) t1d = 360.0 + t1d;

if(t2d < -180.0) t2d = 360.0 + t2d;

}

 

/*

* find minimum number of controller time steps needed

*/

steps_1 = (int)fabs(t1d/VMAX_T1/T_STEP);

steps_2 = (int)fabs(t2d/VMAX_T2/T_STEP);

if(train_sim == 1){

if(steps_1 > steps_2){

steps_2 = steps_1;

} else {

steps_1 = steps_2;

}

}

steps = steps_2;

if(steps_1 > steps_2) steps = steps_1;

/*

* Find joint step size

*/

t1_step = t1d/steps_1;

t2_step = t2d/steps_2;

/*

* set initial conditions of control

*/

path[start].x_target = x_e;

path[start].x_target = y_e;

path[start].x_position = x_s;

path[start].x_position = y_s;

path[start].t1_target = t1e;

path[start].t2_target = t2e;

path[start].t1_position = t1s;

path[start].t2_position = t2s;

path[start].t1_velocity = t1_step/T_STEP;

path[start].t2_velocity = t2_step/T_STEP;

path[start].t1_acceleration = t1_step*9999999.0/fabs(t1_step);

path[start].t2_acceleration = t2_step*9999999.0/fabs(t2_step);

/*

* follow path for required number of steps

*/

for(i = 1+start; i <= steps+start; i++){

/*

* Find next state of control

*/

path[i].t1_target = t1e;

path[i].t2_target = t2e;

/*

* For the cases where one joint must stand

* still after motion, this will allow it.

*/

if((steps_1+start) >= i){

path[i].t1_position = path[i-1].t1_position + t1_step;

path[i].t1_velocity = t1_step/T_STEP;

} else{

path[i].t1_position = path[i-1].t1_position;

path[i].t1_velocity = 0.0;

}

if((steps_2+start) >= i){

path[i].t2_position = path[i-1].t2_position + t2_step;

path[i].t2_velocity = t2_step/T_STEP;

} else{

path[i].t2_position = path[i-1].t2_position;

path[i].t2_velocity = 0.0;

}

path[i].t1_acceleration = 0.0;

path[i].t2_acceleration = 0.0;

path[i].x_target = x_e;

path[i].y_target = y_e;

kin_forward(path[i].t1_position, path[i].t2_position,

&path[i].x_position, &path[i].y_position);

}

/*

* Set Acceleration for last points.

*/

path[start+steps_1].t1_acceleration = -path[start].t1_acceleration;

path[start+steps_2].t2_acceleration = -path[start].t2_acceleration;

/*

* follow path for required number of steps

*/

for(i = 1+steps+start; i <= 5+steps+start; i++){

/*

* Make standing still coordinates

*/

path[i].t1_target = t1e;

path[i].t2_target = t2e;

path[i].t1_position = t1e;

path[i].t2_position = t2e;

path[i].t1_velocity = 0.0;

path[i].t2_velocity = 0.0;

path[i].t1_acceleration = 0.0;

path[i].t2_acceleration = 0.0;

path[i].x_target = x_e;

path[i].y_target = y_e;

kin_forward(path[i].t1_position, path[i].t2_position,

&path[i].x_position, &path[i].y_position);

}

/*

* Number of steps in controller path

*/

n[0] = steps + 1 + 5;

for(i = start; i <= start+n[0]; i++){

inverse_dynamics(path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

}

}

 

 

 

 

 

void tra_2(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (MAXIMUM ACCELERATION)

*

* A Trajectory will be devised, based upon the maximum acceleration

* constraint. This section is still in need of work.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* April 5th, 1990.

*/

{

static int i, j, /* work variables */

flag_e1,

flag_e2,

steps_e1,

steps_e2,

steps_o1,

steps_o2,

step_1,

step_2,

stepper,

steps_1,

steps_2; /* number of calculated steps in path*/

static double t1d, /* total displacement of joints */

t2d,

mult_1,

mult_2,

fact_1,

fact_2,

t1_step, /* joint angle step per time step */

t2_step,

x_s, /* cartesian start and stop points */

y_s,

x_e,

y_e;

 

/*

* Find cartesian points in space

*/

kin_forward(t1s, t2s, &x_s, &y_s);

kin_forward(t1e, t2e, &x_e, &y_e);

if(train_up == 1){

if(t1s > 180.0) t1s = 360.0-t1s;

if(t2e > 180.0) t2e = 360.0-t2e;

if(t1s < -180.0) t1s = 360.0+t1s;

if(t2e < -180.0) t2e = 360.0+t2e;

}

/*

* find joint angle displacements (minimum)

*/

t1d = t1e: t1s;

t2d = t2e: t2s;

if(train_opt != 0){

if(t1d > 180.0) t1d = t1d: 360.0;

if(t2d > 180.0) t2d = t2d: 360.0;

if(t1d < -180.0) t1d = 360.0 + t1d;

if(t2d < -180.0) t2d = 360.0 + t2d;

}

 

mult_1 = 1.0;

mult_2 = 1.0;

if(t1d < 0.0) mult_1 = -1.0;

if(t2d < 0.0) mult_2 = -1.0;

t1d *= mult_1;

t2d *= mult_2;

 

/*

* find minimum number of controller time steps needed

*/

steps_o1 = (int)(-1.5 + sqrt(2.25 + (t1d/T_STEP/T_STEP/AMAX_T1)));

steps_o2 = (int)(-1.5 + sqrt(2.25 + (t2d/T_STEP/T_STEP/AMAX_T2)));

steps_e1 = (int)(-1.0 + sqrt(1.0 + (t1d/T_STEP/T_STEP/AMAX_T1)));

steps_e2 = (int)(-1.0 + sqrt(1.0 + (t2d/T_STEP/T_STEP/AMAX_T2)));

 

steps_1 = steps_o1;

steps_2 = steps_o2;

flag_e1 = 0;

flag_e2 = 0;

 

if(steps_e1 <= steps_o1){

steps_1 = steps_e1;

flag_e1 = 1;

}

if(steps_e2 <= steps_o2){

steps_2 = steps_e2;

flag_e2 = 1;

}

 

if(train_sim == 1){

if(steps_1 > steps_2){

steps_2 = steps_1;

flag_e2 = flag_e1;

} else{

steps_1 = steps_2;

flag_e1 = flag_e2;

}

/*

* Find joint step size

*/

if(flag_e1 == 1){

t1_step=t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP));

} else {

t1_step = t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP)

+(steps_1 + 2) *T_STEP*T_STEP);

}

if(flag_e2 == 1){

t2_step=t2d/(((steps_2+2)*T_STEP)*((steps_2+2)*T_STEP)); } else {

t2_step = t2d/(((steps_2+2)*T_STEP)*((steps_2+2)*T_STEP)

+(steps_2 + 2) *T_STEP*T_STEP);

}

} else{

/*

* Find joint step size

*/

if(flag_e1 == 1){

t1_step = (t1d/T_STEP/T_STEP: AMAX_T1*

(steps_1*steps_1 + 2.0*steps_1))

/(3.5 + 2.0*steps_1);

} else {

t1_step = (t1d/T_STEP/T_STEP: AMAX_T1*

(steps_1*steps_1 + 3.0*steps_1))

/(6.0 + 2.0*steps_1);

}

if(flag_e2 == 1){

t2_step = (t2d/T_STEP/T_STEP: AMAX_T2*

(steps_2*steps_2 + 2.0*steps_2))

/(3.5 + 2.0*steps_2);

} else {

t2_step = (t2d/T_STEP/T_STEP: AMAX_T2*

(steps_2*steps_2 + 3.0*steps_2))

/(6.0 + 2.0*steps_2);

}

}

/*

* set initial conditions of control

*/

fact_1 = 1.0;

fact_2 = 1.0;

path[start].t1_target = t1e;

path[start].t2_target = t2e;

path[start].t1_position = t1s;

path[start].t2_position = t2s;

path[start].t1_velocity = 0.0;

path[start].t2_velocity = 0.0;

path[start].t1_acceleration = mult_1 * t1_step * fact_1;

path[start].t2_acceleration = mult_2 * t2_step * fact_2;

 

step_1 = 5 + 2 * steps_1 + (1: flag_e1);

step_2 = 5 + 2 * steps_2 + (1: flag_e2);

 

/*

* follow path for joint 1

*/

for(i = 1+start; i <= steps_1+start; i++){

if(train_sim == 0){

a_update(path, i, mult_1 * AMAX_T1 * fact_1, 1);

} else{

a_update(path, i, mult_1 * t1_step * fact_1, 1);

}

}

 

if(flag_e1 == 1){

a_update(path, i, mult_1 * 0.5*t1_step * fact_1, 1);

} else {

a_update(path, i, mult_1 * t1_step * fact_1, 1);

}

 

if(flag_e1 == 0){

a_update(path, i+1, 0.0, 1);

}

 

if(flag_e1 == 1){

a_update(path, i+2-flag_e1, -0.5*t1_step * mult_1*fact_1, 1);

} else {

a_update(path, i+2-flag_e1, -t1_step * mult_1*fact_1, 1);

}

 

for(i = start+steps_1+4-flag_e1; i <= 2*steps_1 +start+3-flag_e1; i++){

if(train_sim == 0){

a_update(path, i, -AMAX_T1 * mult_1*fact_1, 1);

} else{

a_update(path, i, -t1_step * mult_1*fact_1, 1);

}

}

 

a_update(path, i, -t1_step * mult_1*fact_1, 1);

 

for(j = 1; j <= 6; j++){

path[i+j].t1_target = t1e;

path[i+j].t1_position = t1e;

path[i+j].t1_velocity = 0.0;

path[i+j].t1_acceleration = 0.0;

}

 

for(j = 7; j <= (7+step_2-step_1);j++){

path[i+j].t1_target = t1e;

path[i+j].t1_position = t1e;

path[i+j].t1_velocity = 0.0;

path[i+j].t1_acceleration = 0.0;

}

 

/*

* follow path for joint 2

*/

for(i = 1+start; i <= steps_2+start; i++){

if(train_sim == 0){

a_update(path, i, mult_2 * AMAX_T2 * fact_2, 2);

} else{

a_update(path, i, mult_2 * t2_step * fact_2, 2);

}

}

 

if(flag_e2 == 1){

a_update(path, i, mult_2 * 0.5*t2_step*fact_2, 2);

} else {

a_update(path, i, mult_2 * t2_step*fact_2, 2);

}

 

if(flag_e2 == 0){

a_update(path, i+1, 0.0, 2);

}

 

if(flag_e2 == 1){

a_update(path, i+2-flag_e2, -0.5*t2_step * mult_2*fact_2, 2);

} else {

a_update(path, i+2-flag_e2, -t2_step * mult_2*fact_2, 2);

}

 

for(i = start+steps_2+4-flag_e2; i <= 2*steps_2 +start+3-flag_e2; i++){

if(train_sim == 0){

a_update(path, i, -AMAX_T2 * mult_2*fact_2, 2);

} else{

a_update(path, i, -t2_step * mult_2*fact_2, 2);

}

}

 

a_update(path, i, -t2_step * mult_2*fact_2, 2);

 

for(j = 1; j <= 6; j++){

path[i+j].t2_target = t2e;

path[i+j].t2_position = t2e;

path[i+j].t2_velocity = 0.0;

path[i+j].t2_acceleration = 0.0;

}

 

for(j = 7; j <= (7+step_1-step_2);j++){

path[i+j].t2_target = t2e;

path[i+j].t2_position = t2e;

path[i+j].t2_velocity = 0.0;

path[i+j].t2_acceleration = 0.0;

}

 

/*

* Number of steps in controller path

*/

n[0] = step_1+5;

if(step_2 > step_1) n[0] = step_2+5;

for(i = start; i <= start+n[0]; i++){

inverse_dynamics(path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

}

}

 

 

 

 

 

void a_update(path, location, accel, joint)

int location,

joint;

double accel;

train_data *path;

/*

* POSITION UPDATE

*

* This is a routine written to avoid repretitive calculation which was done

* by a number of subroutines. Assuming that acceleration is given, as well

* as position and velocity, the new position and velocity are calculated

* after some time step. Only does one joint to simplify things.

*

* Variables: path: The list of points on the path (including last)

* location: current position in ’path’

* joint: the joint number of interest

* accel: the acceleration for the current step

*

* Returns: path: the system state at the end of the step

*

* December 2nd, 1990.

*/

{

if(joint == 1){

path[location].t1_target = path[location-1].t1_target;

path[location].t1_position = path[location-1].t1_position

+ path[location-1].t1_velocity*T_STEP

+ 0.5*path[location-1].t1_acceleration*T_STEP*T_STEP;

path[location].t1_velocity = path[location-1].t1_velocity +

path[location-1].t1_acceleration*T_STEP;

path[location].t1_acceleration = accel;

}

if(joint == 2){

path[location].t2_target = path[location-1].t2_target;

path[location].t2_position = path[location-1].t2_position

+ path[location-1].t2_velocity*T_STEP

+ 0.5*path[location-1].t2_acceleration*T_STEP*T_STEP;

path[location].t2_velocity = path[location-1].t2_velocity +

path[location-1].t2_acceleration*T_STEP;

path[location].t2_acceleration = accel;

}

 

}

 

 

 

 

 

void tra_3(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (SIMPLE PATH WITH COUPLING FACTOR)

*

* The routine will build a path using the coupling term stored in a file

* this is implemented for demonstrative purposes only. The coupling factor

* was read from disk to allow fast prototyping, without cluttering interface

* even more.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* December 2nd, 1990.

*/

{

static int i, /* work variables */

j;

static double time, /* work variable */

mag, /* variables to store */

mag2, /* dynamics parameters */

a1,

a2,

a3,

a4,

a5,

couple = 0.0; /* default coupling factor */

static parameters spline;

static int mode;

 

FILE *temp; /* file to read coupling */

temp = fopen("couple.dat", "r"); /* factor from disk */

fscanf(temp, "%lf\n", &couple);

fclose(temp);

 

init_inverse_dynamics(); /* needed for dynamics calcs */

/*

* A path is created from the optimization section, using only the

* coupling factor.

*/

get_path(&spline, couple, t1s, t1e, t2s, t2e, FALSE);

 

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

i = start; /* list is filled here */

for(time = 0.0; time <= spline.t[N_PTS-1]+1.0; time += T_STEP){

get_value(N_PTS, spline, time, /* get pos,vel,acc at time */

&mag, &a2, &a3, &mag2, &a4, &a5);

path[i].t1_target = t1e; /* update training set */

path[i].t2_target = t2e;

path[i].t1_position = mag;

path[i].t2_position = mag2;

path[i].t1_velocity = a2;

path[i].t2_velocity = a4;

path[i].t1_acceleration = a3;

path[i].t2_acceleration = a5;

inverse_dynamics( /* get joint torques */

path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

kin_forward(

path[start].t1_position, path[start].t2_position,

&path[start].x_position, &path[start].y_position);

i++; /* pointer to current element*/

}

/*

* Number of steps in controller path

*/

n[0] = i: start; /* number of points added */

}

 

 

 

 

 

void tra_4(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (OPTIMAL TORQUE)

*

* A Trajectory will be devised, based upon some constraint, and objective

* as defined in the optimal motion planning section.

*

* Variables: t1s, t2s: the start configuration for the trajectory

* t1e, t2e: the end configuration for the trajectory

* path: the structure array to put the path into

* start: the start point for the path in the array

*

* Return: n: the number of path segments in the file

*

* April 9th, 1990.

*/

{

static int i, /* work variables */

j;

static double time, /* work variable */

mag, mag2, /* temporary variables for */

a1, a2, a3, a4, a5; /* dynamics configurations */

static parameters spline; /* spline storage structure */

 

init_inverse_dynamics(); /* set up dynamics routines */

 

opt_path(&spline, t1s, t1e, t2s, t2e, TRUE); /* get optimal path */

 

get_params(&spline, N_PTS); /* Fit splines to path points*/

i = start; /* point to end of list */

/*

* Loop through the path and obtain robot pos,vel,acc, & tor at

* time steps of system.

*/

for(time = 0.0; time <= spline.t[N_PTS-1]+1.0; time += T_STEP){

get_value(N_PTS, spline, time, /* get pos,vel,acc at time */

&mag, &a2, &a3, &mag2, &a4, &a5);

path[i].t1_target = t1e; /* put values in list */

path[i].t2_target = t2e;

path[i].t1_position = mag;

path[i].t2_position = mag2;

path[i].t1_velocity = a2;

path[i].t2_velocity = a4;

path[i].t1_acceleration = a3;

path[i].t2_acceleration = a5;

inverse_dynamics( /* get torques from state */

path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

kin_forward(

path[start].t1_position, path[start].t2_position,

&path[start].x_position, &path[start].y_position);

i++; /* move to next location */

}

/*

* Number of steps in controller path

*/

n[0] = i: start; /* get number of points added*/

}

 

 

 

 

 

void tra_5(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (SUB-OPTIMAL, WITH NO JOINT REVERSE)

*

* Generates a path using the optimization functions which vary coupling

* factors only.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* April 9th, 1990.

*/

{

static int i, j; /* work variables */

static double time, /*time work variable */

mag, mag2, /* temporary variables*/

a1, a2, a3, a4, a5; /* for dynamics */

static parameters spline; /* struct for spline*/

 

init_inverse_dynamics(); /* set up dynamics */

 

if(train_opt != 0){ /* how much optimzation */

opter_path(&spline, t1s, t1e, t2s, t2e, FALSE);

} else {

opt_path(&spline, t1s, t1e, t2s, t2e, FALSE);

}

 

get_params(&spline, N_PTS); /* get spline from path points*/

i = start; /* point to end of list */

/*

* generate list of points along path for pos, vel, acc, and torque.

*/

for(time = 0.0; time <= spline.t[N_PTS-1]+1.0; time += T_STEP){

get_value(N_PTS, spline, time, /* get pos,vel,acc from spline*/

&mag, &a2, &a3, &mag2, &a4, &a5);

path[i].t1_target = t1e; /* put pos,vel,acc in list */

path[i].t2_target = t2e;

path[i].t1_position = mag;

path[i].t2_position = mag2;

path[i].t1_velocity = a2;

path[i].t2_velocity = a4;

path[i].t1_acceleration = a3;

path[i].t2_acceleration = a5;

inverse_dynamics( /* get joint torques */

path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

kin_forward(

path[start].t1_position, path[start].t2_position,

&path[start].x_position, &path[start].y_position);

i++; /* add more to end of list */

}

/*

* Number of steps in controller path

*/

n[0] = i: start;

}

 

/*

* ROBOT TRAJECTORY TRAINING PROGRAM

*

* This code contains a number of functions which are necessary, but

* not needed by the User Interface. This includes robot specific,

* and trajectory maintenance routines. Trajectories are also generated

* here.

*

* Note that the endpoints are stored in a section refered to as the

* trajectory buffer. The Buffer is used to path endpoints in general

* here.

*

* Also note that in general terminolgy conventions are not strictly adhered

* to in this module of code.

*

* December 2nd, 1990.

*/

 

 

#include "/usr/people/hugh/thesis/src/test/tester.c"

 

 

void a_update(); /* the only local function */

 

 

 

 

 

void b_load(file)

char *file;

/*

* LOAD TRAJECTORIES FROM FILE

*

* The buffer for the set of test (or training) trajectories is loaded from

* an ASCII file.

*

* Variables: file = the file which contains the trajectories to be loaded

*

* December 2nd, 1990.

*/

{

FILE *in; /* Input file */

static int i; /* work variable */

static char worker[50]; /* Input string */

static double x_s, y_s, /* path start */

x_e, y_e, /* path end */

rad; /* maximum arm raduis */

 

rad = LENGTH_1 + LENGTH_2; /* set arm limit */

 

/*

* Open the file named and get the number of trajectories in it

*/

in = fopen(file, "r"); /* read only */

if(in != NULL){ /* check for file exists */

fscanf(in, "%s\n", worker); /* read info line */

if(worker[0] == ’C’){ /* check for correct format */

/* in this case cartesian */

end_of_buffer = atoi(&worker[1]); /* how many in file */

/*

* load in all the trajectories from the file, into

* the buffer and convert them from x-y to the four

* joint equivalents.

*/

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

/* get endpoints from file */

fscanf(in, "%lf,%lf,%lf,%lf\n",

&x_s, &y_s, &x_e, &y_e);

/* convert cartesian endpoints*/

/* to all four sets of joint */

/* angles */

kin_inverse(

&buffer[i*4+0].t1_start,

&buffer[i*4+0].t2_start,

x_s*rad, y_s*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+1].t1_start,

&buffer[i*4+1].t2_start,

x_s*rad, y_s*rad, ELBOW_DOWN);

kin_inverse(

&buffer[i*4+2].t1_start,

&buffer[i*4+2].t2_start,

x_s*rad, y_s*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+3].t1_start,

&buffer[i*4+3].t2_start,

x_s*rad, y_s*rad, ELBOW_DOWN);

kin_inverse(

&buffer[i*4+0].t1_end,

&buffer[i*4+0].t2_end,

x_e*rad, y_e*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+1].t1_end,

&buffer[i*4+1].t2_end,

x_e*rad, y_e*rad, ELBOW_UP);

kin_inverse(

&buffer[i*4+2].t1_end,

&buffer[i*4+2].t2_end,

x_e*rad, y_e*rad, ELBOW_DOWN);

kin_inverse(

&buffer[i*4+3].t1_end,

&buffer[i*4+3].t2_end,

x_e*rad, y_e*rad, ELBOW_DOWN);

}

end_of_buffer *= 4; /* there are four time more */

} else { /* load joint angles */

end_of_buffer = atoi(worker); /* number of endpoints */

/*

* load in all the trajectories from the file, into

* the buffer.

*/

/* load joint angles for path */

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

fscanf(in, "%lf,%lf,%lf,%lf\n",

&buffer[i].t1_start,

&buffer[i].t2_start,

&buffer[i].t1_end,

&buffer[i].t2_end);

}

}

}

/*

* close the file and point to start of trajectories

*/

fclose(in);

buffer_point = 0;

}

 

 

 

 

 

void b_save(file)

char *file;

/*

* SAVE TRAJECTORY INFORMATION

*

* The trajectory endpoints will be saved to an ASCII file with this

* subroutine.

*

* Variables: file = the file which contains the trajectories to be saved

*

* March 8th, 1990.

*/

{

FILE *out; /* The output file */

static int i; /* A work variable */

 

/*

* Open file and write the number of trajectories to be saved.

*/

out = fopen(file, "w"); /* write only */

fprintf(out, "%d\n", end_of_buffer); /* save number of endpoints */

/*

* write then trajectories to the file

*/

for(i = 0; i < end_of_buffer; i++){ /* save all endpoints */

fprintf(out, "%f,%f,%f,%f\n",

buffer[i].t1_start, buffer[i].t2_start,

buffer[i].t1_end, buffer[i].t2_end);

}

/*

* close file

*/

fclose(out);

}

 

 

 

 

 

void b_tload(file, start, n)

char *file;

int *n,

start;

/*

* LOAD TRAJECTORIES FROM FILE

*

* The buffer for the set of test (or training) trajectories is loaded from

* a file. These points are actual points during the motion of the arm.

*

* Variables: file: the file which contains the trajectories to be loaded

* start: the location to start loading to

*

* Returns: n: number of trajectories loaded

*

* March 8th, 1990.

*/

{

FILE *in;

static int i;

 

/*

* Open the file named and get the number of trajectories in it

*/

in = fopen(file, "r"); /* read only */

/*

* load in all the trajectories from the file, into the buffer

*/

n[0] = 0; /* default to zero loaded */

if(in != NULL){ /* if file exists */

fscanf(in, "%d\n", n); /* get number of points */

/* loop to read in variables */

/* sorry about format line */

for(i = start; i < (start + n[0]); i++){

fscanf(in,

"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",

&t_set[i].x_target, &t_set[i].y_target,

&t_set[i].x_position, &t_set[i].y_position,

&t_set[i].x_velocity, &t_set[i].y_velocity,

&t_set[i].x_acceleration, &t_set[i].y_acceleration,

&t_set[i].t1_target, &t_set[i].t2_target,

&t_set[i].t1_position, &t_set[i].t2_position,

&t_set[i].t1_velocity, &t_set[i].t2_velocity,

&t_set[i].t1_acceleration, &t_set[i].t2_acceleration,

&t_set[i].t1_torque, &t_set[i].t2_torque);

}

}

/*

* close the file and point to start of trajectories.

*/

fclose(in);

}

 

 

 

 

void b_tsave(file)

char *file;

/*

* SAVE TRAJECTORY INFORMATION

*

* The trajectories will be saved to an ASCII file with this subroutine. These

* trajectories are the actual points during a motion.

*

* Variables: file = the file which contains the trajectories to be saved

*

* March 8th, 1990.

*/

{

FILE *out; /* output file */

static int i; /* work variable */

 

/*

* Open file

*/

out = fopen(file, "w"); /* write only */

/*

* write the training set to the file

*/

fprintf(out, "%d\n", end_train); /* save the number of points */

for(i = 0; i < end_train; i++){ /* loop through entire set */

fprintf(out, /* another painful format */

"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",

t_set[i].x_target, t_set[i].y_target,

t_set[i].x_position, t_set[i].y_position,

t_set[i].x_velocity, t_set[i].y_velocity,

t_set[i].x_acceleration, t_set[i].y_acceleration,

t_set[i].t1_target, t_set[i].t2_target,

t_set[i].t1_position, t_set[i].t2_position,

t_set[i].t1_velocity, t_set[i].t2_velocity,

t_set[i].t1_acceleration, t_set[i].t2_acceleration,

t_set[i].t1_torque, t_set[i].t2_torque);

}

/*

* write last line and close file

*/

fclose(out);

}

 

 

 

 

 

void b_down()

/*

* MOVE DOWN TRAJECTORY LIST

*

* This moves the pointer to the trajectory buffer, down the list. It will

* cause the visible list to be updated to the new item.

*

* March 8th, 1990.

*/

{

/*

* increment pointer and check for end of buffer collision

*/

buffer_point++;

if(buffer_point >= end_of_buffer) buffer_point = end_of_buffer -1;

}

 

 

 

 

 

void b_up()

/*

* MOVES POINTER UP TRAJECTORY LIST

*

* The pointer will be moved to the top of the list. The display will be

* updated before return.

*

* March 8th, 1990.

*/

{

/*

* decrement pointer and check for start of buffer collision

*/

buffer_point--;

if(buffer_point < 0) buffer_point = 0;

}

 

 

 

 

 

void b_delete()

/*

* REMOVES TRAJECTORY AT CURRENT POINTER

*

* The trajectory in the buffer (at the current pointer) will be deleted

* from the list. This is irreversible.

*

* March 8th, 1990.

*/

{

static int i, /* a work variable */

flag; /* a success flag for delete */

/*

* Check for trajectory to delete

*/

if(end_of_buffer > 0){ /* if buffer not empty */

/*

* clear flag to indicate nothing deleted

*/

flag = 0; /* set no change flag */

/*

* Slide trajectories in file back to cover deleted traj.

*/

for(i = buffer_point+1; i <= end_of_buffer; i++){

buffer[i-1].t1_start = buffer[i].t1_start;

buffer[i-1].t2_start = buffer[i].t2_start;

buffer[i-1].t1_end = buffer[i].t1_end;

buffer[i-1].t2_end = buffer[i].t2_end;

flag = 1; /* flag change made */

}

/*

* Update end of buffer and current buffer pointer values.

*/

if(flag > 0){ /* if delete done */

end_of_buffer--; /* remove last element *

if((end_of_buffer == buffer_point)&&(end_of_buffer > 0))

buffer_point--; /* move location in list */

}

}

}

 

 

 

 

 

void b_insert()

/*

* INSERT DISPLAYED TRAJECTORY AT CURRENT LOCATION

*

* The points indicated as start and stop on the screen will be entered

* into the trajectory list, at the current pointer position. The other

* trajectories will be bumped down.

*

* March 8th, 1990.

*/

{

static int i; /* a work variable */

/*

* Slide everything in the buffer down one place to make space for new

*/

for(i = end_of_buffer; i > buffer_point; i--){

buffer[i].t1_start = buffer[i-1].t1_start;

buffer[i].t2_start = buffer[i-1].t2_start;

buffer[i].t1_end = buffer[i-1].t1_end;

buffer[i].t2_end = buffer[i-1].t2_end;

}

/*

* stick current start and stop in new empty spot.

*/

buffer[buffer_point].t1_start = current.t1_start;

buffer[buffer_point].t2_start = current.t2_start;

buffer[buffer_point].t1_end = current.t1_end;

buffer[buffer_point].t2_end = current.t2_end;

end_of_buffer++; /* add one to buffer size */

}

 

 

 

 

 

 

int b_goto(point)

int point;

/*

* GOTO POINTER

*

* This routine will set the pointer value to the indicated value.

* The routine will return a no_error state if sucessful.

*

* Variables: point = the new desired location for the buffer pointer

*

* Returns: error = returns NO_ERROR if sucessful

*

* March 11th, 1990.

*/

{

static int error; /* the error state */

 

error = ERROR; /* default is error */

/*

* If possible then change the current pointer to the desired value

*/

if((point >= 0) && (point < end_of_buffer) && (end_of_buffer > 0)){

buffer_point = point; /* update current line */

error = NO_ERROR; /* clear error */

}

return(error);

}

 

 

 

 

 

void kin_forward(t1, t2, x, y)

double t1, t2, *x, *y;

/*

* FORWARD KINEMATICS

*

* The forward kinematics will be calculated when this routine is called.

*

* Variables: t1, t2 = angles of manipulator joints

*

* Returns: x, y = position of manipulator end effector

*

* March 8th, 1990.

*/

{

x[0] = LENGTH_1*cos(PI*t1/180.0) + LENGTH_2*cos(PI*(t1+t2)/180.0);

y[0] = LENGTH_1*sin(PI*t1/180.0) + LENGTH_2*sin(PI*(t1+t2)/180.0);

}

 

 

 

 

 

void kin_inverse(t1, t2, x, y, conf)

double *t1, *t2, x, y;

int conf;

/*

* INVERSE KINEMATICS

*

* The inverse kinematics will be done, with consideration of the elbow

* up and down solutions.

*

* Variables: x, y = the cartesian location of the end effector

* conf = the elbow up/down solution (0 = elbow up)

*

* Returns: t1, t2 = the joint configuration of the manipulator

*

* March 26th, 1990.

*/

{

static double r, /* for trig in problem */

fact; /* elbow up/down correction */

 

r = sqrt(x*x + y*y); /* end effector from origin */

fact = 1.0; /* elbow up */

if(conf == 1) fact = -1.0; /* elbow down */

/* if solution is valid */

if(((double)(LENGTH_1-LENGTH_2) <= r) && (r > 0.0)

&& ((double)(LENGTH_1+LENGTH_2) >= r)){

t2[0]=fact*(180.0*acos((LENGTH_1*LENGTH_1+LENGTH_2*LENGTH_2-r*r)

/2.0/LENGTH_1/LENGTH_2)/PI: 180.0);

t1[0]=180.0*(fact*acos((LENGTH_1*LENGTH_1+r*r-LENGTH_2*LENGTH_2)

/2.0/LENGTH_1/r) + atan2(y, x))/PI;

}

}

 

 

 

 

 

void get_trajectory(plan, t1_s, t2_s, t1_e, t2_e, path, start, n)

int plan,

start,

*n;

double t1_s, t2_s,

t1_e, t2_e;

train_data *path;

/*

* TRAJECTORY GENERATION PROGRAM

*

* The different trajectory algorithms are available through this sub-routine.

*

* Variables: plan: the trajectory planner to be used

* x_s, y_s: the start point for the trajectory

* x_e, y_e: the end point for the trajectory

* path: the structure array to put the path into

* start: the start point for the path in the array

*

* Return: n: the number of path segments in the file

*

* March 8th, 1990.

*/

{

static double t1s, /* work variables to keep the different */

t2s, /* joint configurations for alternate */

t1e, /* training methods */

t2e,

t1sa,

t2sa,

t1ea,

t2ea;

static int keep[1]; /* a work variable for the path size */

n[0] = 0;

 

/*

* Find the different possible configurations for the path. This

* includes i) elbow down to elbow down, ii) elbow up to elbow up,

* iii) elbow down to elbow up, and iv) elbow up to elbow down.

*/

t1s = t1_s;

t2s = t2_s;

t1e = t1_e;

t2e = t2_e;

t1sa = t1_e;

t2sa = t2_e;

t1ea = t1_s;

t2ea = t2_s;

/*

* Apply the main controller cases if applicable.

*/

if(plan == CASE_1) tra_1(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_2) tra_2(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_3) tra_3(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_4) tra_4(t1s, t2s, t1e, t2e, path, start, n);

if(plan == CASE_5) tra_5(t1s, t2s, t1e, t2e, path, start, n);

 

/*

* If the training set it to be generated the reverse of paths may

* be generated here

*/

if((train_flag == 1) && (train_alt == 1) && (plan != NETWORK)

&& (plan != SET_FILE)){

if(plan == CASE_1)

tra_1(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_2)

tra_2(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_3)

tra_3(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_4)

tra_4(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

if(plan == CASE_5)

tra_5(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);

n[0] += keep[0];

}

 

/*

* If the neural network controller was called for then test the

* controller with only the current configuration.

*/

if(plan == NETWORK){

nt_trajectory(n_output, t1s, t2s, t1e, t2e, path, start, n);

}

 

/*

* If the neural network controller was called for, then test the

* controller with only the current configuration.

*/

if(plan == SET_FILE){

b_tload(t_file, start, n);

}

if(plan == NN_TRACK){

nt_tracker(n_output, t1s, t2s, t1e, t2e, path, start, n);

}

}

 

 

 

 

 

void tra_1(t1s, t2s, t1e, t2e, path, start, n)

int start, *n;

double t1s, t2s, t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (MAXIMUM VELOCITY)

*

* A Trajectory will be devised, based upon the maximum velocity constraint.

* This section is still in need of work.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* March 8th, 1990.

*/

{

static int i, /* work variable */

steps_1,

steps_2,

steps; /* number of calculated steps in path*/

static double t1d, /* total displacement of joints */

t2d,

t1_step, /* joint angle step per time step */

t2_step,

x_s, /* cartesian start and stop points */

y_s,

x_e,

y_e;

 

/*

* Find cartesian points in space

*/

kin_forward(t1s, t2s, &x_s, &y_s);

kin_forward(t1e, t2e, &x_e, &y_e);

if(train_up == 1){

if(t1s > 180.0) t1s = 360.0-t1s;

if(t2e > 180.0) t2e = 360.0-t2e;

if(t1s < -180.0) t1s = 360.0+t1s;

if(t2e < -180.0) t2e = 360.0+t2e;

}

/*

* find joint angle displacements

*/

t1d = t1e: t1s;

t2d = t2e: t2s;

/* It was discovered at this point that the */

/* correction of joint angles lead to a problem */

/* To solve this problem, both joint angles */

/* must be changed simultaneously. In effect */

/* this advocates a rule based control strategy */

/* Note that this may lead to a less than */

/* optimal path for the arm, but the results */

/* will not contain a discontinuity as */

/* introduced by a hard rule. */

if(train_opt != 0){

if(t1d > 180.0) t1d = t1d: 360.0;

if(t2d > 180.0) t2d = t2d: 360.0;

if(t1d < -180.0) t1d = 360.0 + t1d;

if(t2d < -180.0) t2d = 360.0 + t2d;

}

 

/*

* find minimum number of controller time steps needed

*/

steps_1 = (int)fabs(t1d/VMAX_T1/T_STEP);

steps_2 = (int)fabs(t2d/VMAX_T2/T_STEP);

if(train_sim == 1){

if(steps_1 > steps_2){

steps_2 = steps_1;

} else {

steps_1 = steps_2;

}

}

steps = steps_2;

if(steps_1 > steps_2) steps = steps_1;

/*

* Find joint step size

*/

t1_step = t1d/steps_1;

t2_step = t2d/steps_2;

/*

* set initial conditions of control

*/

path[start].x_target = x_e;

path[start].x_target = y_e;

path[start].x_position = x_s;

path[start].x_position = y_s;

path[start].t1_target = t1e;

path[start].t2_target = t2e;

path[start].t1_position = t1s;

path[start].t2_position = t2s;

path[start].t1_velocity = t1_step/T_STEP;

path[start].t2_velocity = t2_step/T_STEP;

path[start].t1_acceleration = t1_step*9999999.0/fabs(t1_step);

path[start].t2_acceleration = t2_step*9999999.0/fabs(t2_step);

/*

* follow path for required number of steps

*/

for(i = 1+start; i <= steps+start; i++){

/*

* Find next state of control

*/

path[i].t1_target = t1e;

path[i].t2_target = t2e;

/*

* For the cases where one joint must stand

* still after motion, this will allow it.

*/

if((steps_1+start) >= i){

path[i].t1_position = path[i-1].t1_position + t1_step;

path[i].t1_velocity = t1_step/T_STEP;

} else{

path[i].t1_position = path[i-1].t1_position;

path[i].t1_velocity = 0.0;

}

if((steps_2+start) >= i){

path[i].t2_position = path[i-1].t2_position + t2_step;

path[i].t2_velocity = t2_step/T_STEP;

} else{

path[i].t2_position = path[i-1].t2_position;

path[i].t2_velocity = 0.0;

}

path[i].t1_acceleration = 0.0;

path[i].t2_acceleration = 0.0;

path[i].x_target = x_e;

path[i].y_target = y_e;

kin_forward(path[i].t1_position, path[i].t2_position,

&path[i].x_position, &path[i].y_position);

}

/*

* Set Acceleration for last points.

*/

path[start+steps_1].t1_acceleration = -path[start].t1_acceleration;

path[start+steps_2].t2_acceleration = -path[start].t2_acceleration;

/*

* follow path for required number of steps

*/

for(i = 1+steps+start; i <= 5+steps+start; i++){

/*

* Make standing still coordinates

*/

path[i].t1_target = t1e;

path[i].t2_target = t2e;

path[i].t1_position = t1e;

path[i].t2_position = t2e;

path[i].t1_velocity = 0.0;

path[i].t2_velocity = 0.0;

path[i].t1_acceleration = 0.0;

path[i].t2_acceleration = 0.0;

path[i].x_target = x_e;

path[i].y_target = y_e;

kin_forward(path[i].t1_position, path[i].t2_position,

&path[i].x_position, &path[i].y_position);

}

/*

* Number of steps in controller path

*/

n[0] = steps + 1 + 5;

for(i = start; i <= start+n[0]; i++){

inverse_dynamics(path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

}

}

 

 

 

 

 

void tra_2(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (MAXIMUM ACCELERATION)

*

* A Trajectory will be devised, based upon the maximum acceleration

* constraint. This section is still in need of work.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* April 5th, 1990.

*/

{

static int i, j, /* work variables */

flag_e1,

flag_e2,

steps_e1,

steps_e2,

steps_o1,

steps_o2,

step_1,

step_2,

stepper,

steps_1,

steps_2; /* number of calculated steps in path*/

static double t1d, /* total displacement of joints */

t2d,

mult_1,

mult_2,

fact_1,

fact_2,

t1_step, /* joint angle step per time step */

t2_step,

x_s, /* cartesian start and stop points */

y_s,

x_e,

y_e;

 

/*

* Find cartesian points in space

*/

kin_forward(t1s, t2s, &x_s, &y_s);

kin_forward(t1e, t2e, &x_e, &y_e);

if(train_up == 1){

if(t1s > 180.0) t1s = 360.0-t1s;

if(t2e > 180.0) t2e = 360.0-t2e;

if(t1s < -180.0) t1s = 360.0+t1s;

if(t2e < -180.0) t2e = 360.0+t2e;

}

/*

* find joint angle displacements (minimum)

*/

t1d = t1e: t1s;

t2d = t2e: t2s;

if(train_opt != 0){

if(t1d > 180.0) t1d = t1d: 360.0;

if(t2d > 180.0) t2d = t2d: 360.0;

if(t1d < -180.0) t1d = 360.0 + t1d;

if(t2d < -180.0) t2d = 360.0 + t2d;

}

 

mult_1 = 1.0;

mult_2 = 1.0;

if(t1d < 0.0) mult_1 = -1.0;

if(t2d < 0.0) mult_2 = -1.0;

t1d *= mult_1;

t2d *= mult_2;

 

/*

* find minimum number of controller time steps needed

*/

steps_o1 = (int)(-1.5 + sqrt(2.25 + (t1d/T_STEP/T_STEP/AMAX_T1)));

steps_o2 = (int)(-1.5 + sqrt(2.25 + (t2d/T_STEP/T_STEP/AMAX_T2)));

steps_e1 = (int)(-1.0 + sqrt(1.0 + (t1d/T_STEP/T_STEP/AMAX_T1)));

steps_e2 = (int)(-1.0 + sqrt(1.0 + (t2d/T_STEP/T_STEP/AMAX_T2)));

 

steps_1 = steps_o1;

steps_2 = steps_o2;

flag_e1 = 0;

flag_e2 = 0;

 

if(steps_e1 <= steps_o1){

steps_1 = steps_e1;

flag_e1 = 1;

}

if(steps_e2 <= steps_o2){

steps_2 = steps_e2;

flag_e2 = 1;

}

 

if(train_sim == 1){

if(steps_1 > steps_2){

steps_2 = steps_1;

flag_e2 = flag_e1;

} else{

steps_1 = steps_2;

flag_e1 = flag_e2;

}

/*

* Find joint step size

*/

if(flag_e1 == 1){

t1_step=t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP));

} else {

t1_step = t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP)

+(steps_1 + 2) *T_STEP*T_STEP);

}

if(flag_e2 == 1){

t2_step=t2d/(((steps_2+2)*T_STEP)*((steps_2+2)*T_STEP)); } else {

t2_step = t2d/(((steps_2+2)*T_STEP)*((steps_2+2)*T_STEP)

+(steps_2 + 2) *T_STEP*T_STEP);

}

} else{

/*

* Find joint step size

*/

if(flag_e1 == 1){

t1_step = (t1d/T_STEP/T_STEP: AMAX_T1*

(steps_1*steps_1 + 2.0*steps_1))

/(3.5 + 2.0*steps_1);

} else {

t1_step = (t1d/T_STEP/T_STEP: AMAX_T1*

(steps_1*steps_1 + 3.0*steps_1))

/(6.0 + 2.0*steps_1);

}

if(flag_e2 == 1){

t2_step = (t2d/T_STEP/T_STEP: AMAX_T2*

(steps_2*steps_2 + 2.0*steps_2))

/(3.5 + 2.0*steps_2);

} else {

t2_step = (t2d/T_STEP/T_STEP: AMAX_T2*

(steps_2*steps_2 + 3.0*steps_2))

/(6.0 + 2.0*steps_2);

}

}

/*

* set initial conditions of control

*/

fact_1 = 1.0;

fact_2 = 1.0;

path[start].t1_target = t1e;

path[start].t2_target = t2e;

path[start].t1_position = t1s;

path[start].t2_position = t2s;

path[start].t1_velocity = 0.0;

path[start].t2_velocity = 0.0;

path[start].t1_acceleration = mult_1 * t1_step * fact_1;

path[start].t2_acceleration = mult_2 * t2_step * fact_2;

 

step_1 = 5 + 2 * steps_1 + (1: flag_e1);

step_2 = 5 + 2 * steps_2 + (1: flag_e2);

 

/*

* follow path for joint 1

*/

for(i = 1+start; i <= steps_1+start; i++){

if(train_sim == 0){

a_update(path, i, mult_1 * AMAX_T1 * fact_1, 1);

} else{

a_update(path, i, mult_1 * t1_step * fact_1, 1);

}

}

 

if(flag_e1 == 1){

a_update(path, i, mult_1 * 0.5*t1_step * fact_1, 1);

} else {

a_update(path, i, mult_1 * t1_step * fact_1, 1);

}

 

if(flag_e1 == 0){

a_update(path, i+1, 0.0, 1);

}

 

if(flag_e1 == 1){

a_update(path, i+2-flag_e1, -0.5*t1_step * mult_1*fact_1, 1);

} else {

a_update(path, i+2-flag_e1, -t1_step * mult_1*fact_1, 1);

}

 

for(i = start+steps_1+4-flag_e1; i <= 2*steps_1 +start+3-flag_e1; i++){

if(train_sim == 0){

a_update(path, i, -AMAX_T1 * mult_1*fact_1, 1);

} else{

a_update(path, i, -t1_step * mult_1*fact_1, 1);

}

}

 

a_update(path, i, -t1_step * mult_1*fact_1, 1);

 

for(j = 1; j <= 6; j++){

path[i+j].t1_target = t1e;

path[i+j].t1_position = t1e;

path[i+j].t1_velocity = 0.0;

path[i+j].t1_acceleration = 0.0;

}

 

for(j = 7; j <= (7+step_2-step_1);j++){

path[i+j].t1_target = t1e;

path[i+j].t1_position = t1e;

path[i+j].t1_velocity = 0.0;

path[i+j].t1_acceleration = 0.0;

}

 

/*

* follow path for joint 2

*/

for(i = 1+start; i <= steps_2+start; i++){

if(train_sim == 0){

a_update(path, i, mult_2 * AMAX_T2 * fact_2, 2);

} else{

a_update(path, i, mult_2 * t2_step * fact_2, 2);

}

}

 

if(flag_e2 == 1){

a_update(path, i, mult_2 * 0.5*t2_step*fact_2, 2);

} else {

a_update(path, i, mult_2 * t2_step*fact_2, 2);

}

 

if(flag_e2 == 0){

a_update(path, i+1, 0.0, 2);

}

 

if(flag_e2 == 1){

a_update(path, i+2-flag_e2, -0.5*t2_step * mult_2*fact_2, 2);

} else {

a_update(path, i+2-flag_e2, -t2_step * mult_2*fact_2, 2);

}

 

for(i = start+steps_2+4-flag_e2; i <= 2*steps_2 +start+3-flag_e2; i++){

if(train_sim == 0){

a_update(path, i, -AMAX_T2 * mult_2*fact_2, 2);

} else{

a_update(path, i, -t2_step * mult_2*fact_2, 2);

}

}

 

a_update(path, i, -t2_step * mult_2*fact_2, 2);

 

for(j = 1; j <= 6; j++){

path[i+j].t2_target = t2e;

path[i+j].t2_position = t2e;

path[i+j].t2_velocity = 0.0;

path[i+j].t2_acceleration = 0.0;

}

 

for(j = 7; j <= (7+step_1-step_2);j++){

path[i+j].t2_target = t2e;

path[i+j].t2_position = t2e;

path[i+j].t2_velocity = 0.0;

path[i+j].t2_acceleration = 0.0;

}

 

/*

* Number of steps in controller path

*/

n[0] = step_1+5;

if(step_2 > step_1) n[0] = step_2+5;

for(i = start; i <= start+n[0]; i++){

inverse_dynamics(path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

}

}

 

 

 

 

 

void a_update(path, location, accel, joint)

int location,

joint;

double accel;

train_data *path;

/*

* POSITION UPDATE

*

* This is a routine written to avoid repretitive calculation which was done

* by a number of subroutines. Assuming that acceleration is given, as well

* as position and velocity, the new position and velocity are calculated

* after some time step. Only does one joint to simplify things.

*

* Variables: path: The list of points on the path (including last)

* location: current position in ’path’

* joint: the joint number of interest

* accel: the acceleration for the current step

*

* Returns: path: the system state at the end of the step

*

* December 2nd, 1990.

*/

{

if(joint == 1){

path[location].t1_target = path[location-1].t1_target;

path[location].t1_position = path[location-1].t1_position

+ path[location-1].t1_velocity*T_STEP

+ 0.5*path[location-1].t1_acceleration*T_STEP*T_STEP;

path[location].t1_velocity = path[location-1].t1_velocity +

path[location-1].t1_acceleration*T_STEP;

path[location].t1_acceleration = accel;

}

if(joint == 2){

path[location].t2_target = path[location-1].t2_target;

path[location].t2_position = path[location-1].t2_position

+ path[location-1].t2_velocity*T_STEP

+ 0.5*path[location-1].t2_acceleration*T_STEP*T_STEP;

path[location].t2_velocity = path[location-1].t2_velocity +

path[location-1].t2_acceleration*T_STEP;

path[location].t2_acceleration = accel;

}

 

}

 

 

 

 

 

void tra_3(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (SIMPLE PATH WITH COUPLING FACTOR)

*

* The routine will build a path using the coupling term stored in a file

* this is implemented for demonstrative purposes only. The coupling factor

* was read from disk to allow fast prototyping, without cluttering interface

* even more.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* December 2nd, 1990.

*/

{

static int i, /* work variables */

j;

static double time, /* work variable */

mag, /* variables to store */

mag2, /* dynamics parameters */

a1,

a2,

a3,

a4,

a5,

couple = 0.0; /* default coupling factor */

static parameters spline;

static int mode;

 

FILE *temp; /* file to read coupling */

temp = fopen("couple.dat", "r"); /* factor from disk */

fscanf(temp, "%lf\n", &couple);

fclose(temp);

 

init_inverse_dynamics(); /* needed for dynamics calcs */

/*

* A path is created from the optimization section, using only the

* coupling factor.

*/

get_path(&spline, couple, t1s, t1e, t2s, t2e, FALSE);

 

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

i = start; /* list is filled here */

for(time = 0.0; time <= spline.t[N_PTS-1]+1.0; time += T_STEP){

get_value(N_PTS, spline, time, /* get pos,vel,acc at time */

&mag, &a2, &a3, &mag2, &a4, &a5);

path[i].t1_target = t1e; /* update training set */

path[i].t2_target = t2e;

path[i].t1_position = mag;

path[i].t2_position = mag2;

path[i].t1_velocity = a2;

path[i].t2_velocity = a4;

path[i].t1_acceleration = a3;

path[i].t2_acceleration = a5;

inverse_dynamics( /* get joint torques */

path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

kin_forward(

path[start].t1_position, path[start].t2_position,

&path[start].x_position, &path[start].y_position);

i++; /* pointer to current element*/

}

/*

* Number of steps in controller path

*/

n[0] = i: start; /* number of points added */

}

 

 

 

 

 

void tra_4(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (OPTIMAL TORQUE)

*

* A Trajectory will be devised, based upon some constraint, and objective

* as defined in the optimal motion planning section.

*

* Variables: t1s, t2s: the start configuration for the trajectory

* t1e, t2e: the end configuration for the trajectory

* path: the structure array to put the path into

* start: the start point for the path in the array

*

* Return: n: the number of path segments in the file

*

* April 9th, 1990.

*/

{

static int i, /* work variables */

j;

static double time, /* work variable */

mag, mag2, /* temporary variables for */

a1, a2, a3, a4, a5; /* dynamics configurations */

static parameters spline; /* spline storage structure */

 

init_inverse_dynamics(); /* set up dynamics routines */

 

opt_path(&spline, t1s, t1e, t2s, t2e, TRUE); /* get optimal path */

 

get_params(&spline, N_PTS); /* Fit splines to path points*/

i = start; /* point to end of list */

/*

* Loop through the path and obtain robot pos,vel,acc, & tor at

* time steps of system.

*/

for(time = 0.0; time <= spline.t[N_PTS-1]+1.0; time += T_STEP){

get_value(N_PTS, spline, time, /* get pos,vel,acc at time */

&mag, &a2, &a3, &mag2, &a4, &a5);

path[i].t1_target = t1e; /* put values in list */

path[i].t2_target = t2e;

path[i].t1_position = mag;

path[i].t2_position = mag2;

path[i].t1_velocity = a2;

path[i].t2_velocity = a4;

path[i].t1_acceleration = a3;

path[i].t2_acceleration = a5;

inverse_dynamics( /* get torques from state */

path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

kin_forward(

path[start].t1_position, path[start].t2_position,

&path[start].x_position, &path[start].y_position);

i++; /* move to next location */

}

/*

* Number of steps in controller path

*/

n[0] = i: start; /* get number of points added*/

}

 

 

 

 

 

void tra_5(t1s, t2s, t1e, t2e, path, start, n)

int start,

*n;

double t1s, t2s,

t1e, t2e;

train_data *path;

/*

* GENERATE TRAJECTORY (SUB-OPTIMAL, WITH NO JOINT REVERSE)

*

* Generates a path using the optimization functions which vary coupling

* factors only.

*

* Variables: t1s, t2s = the start configuration for the trajectory

* t1e, t2e = the end configuration for the trajectory

* path = the structure array to put the path into

* start = the start point for the path in the array

*

* Return: n = the number of path segments in the file

*

* April 9th, 1990.

*/

{

static int i, j; /* work variables */

static double time, /*time work variable */

mag, mag2, /* temporary variables*/

a1, a2, a3, a4, a5; /* for dynamics */

static parameters spline; /* struct for spline*/

 

init_inverse_dynamics(); /* set up dynamics */

 

if(train_opt != 0){ /* how much optimzation */

opter_path(&spline, t1s, t1e, t2s, t2e, FALSE);

} else {

opt_path(&spline, t1s, t1e, t2s, t2e, FALSE);

}

 

get_params(&spline, N_PTS); /* get spline from path points*/

i = start; /* point to end of list */

/*

* generate list of points along path for pos, vel, acc, and torque.

*/

for(time = 0.0; time <= spline.t[N_PTS-1]+1.0; time += T_STEP){

get_value(N_PTS, spline, time, /* get pos,vel,acc from spline*/

&mag, &a2, &a3, &mag2, &a4, &a5);

path[i].t1_target = t1e; /* put pos,vel,acc in list */

path[i].t2_target = t2e;

path[i].t1_position = mag;

path[i].t2_position = mag2;

path[i].t1_velocity = a2;

path[i].t2_velocity = a4;

path[i].t1_acceleration = a3;

path[i].t2_acceleration = a5;

inverse_dynamics( /* get joint torques */

path[i].t1_position, path[i].t2_position,

path[i].t1_velocity, path[i].t2_velocity,

path[i].t1_acceleration, path[i].t2_acceleration,

&path[i].t1_torque, &path[i].t2_torque);

kin_forward(

path[start].t1_position, path[start].t2_position,

&path[start].x_position, &path[start].y_position);

i++; /* add more to end of list */

}

/*

* Number of steps in controller path

*/

n[0] = i: start;

}