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.
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’).
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.
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.
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
* Note that the endpoints are stored in a section refered to as the
* trajectory buffer. The Buffer is used to path endpoints in general
* Also note that in general terminolgy conventions are not strictly adhered
#include "/usr/people/hugh/thesis/src/test/tester.c"
void a_update(); /* the only local function */
* The buffer for the set of test (or training) trajectories is loaded from
* Variables: file = the file which contains the trajectories to be loaded
static int i; /* work variable */
static char worker[50]; /* Input string */
static double x_s, y_s, /* path start */
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 */
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
for(i = 0; i < end_of_buffer; i++){
fscanf(in, "%lf,%lf,%lf,%lf\n",
/* convert cartesian endpoints*/
/* to all four sets of joint */
x_s*rad, y_s*rad, ELBOW_DOWN);
x_s*rad, y_s*rad, ELBOW_DOWN);
x_e*rad, y_e*rad, ELBOW_DOWN);
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
/* load joint angles for path */
for(i = 0; i < end_of_buffer; i++){
fscanf(in, "%lf,%lf,%lf,%lf\n",
* close the file and point to start of trajectories
* The trajectory endpoints will be saved to an ASCII file with this
* Variables: file = the file which contains the trajectories to be saved
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 */
buffer[i].t1_start, buffer[i].t2_start,
buffer[i].t1_end, buffer[i].t2_end);
* 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
* 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 */
for(i = start; i < (start + n[0]); i++){
"%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.
* 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
static int i; /* work variable */
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
* This moves the pointer to the trajectory buffer, down the list. It will
* cause the visible list to be updated to the new item.
* increment pointer and check for end of buffer collision
if(buffer_point >= end_of_buffer) buffer_point = end_of_buffer -1;
* MOVES POINTER UP TRAJECTORY LIST
* The pointer will be moved to the top of the list. The display will be
* decrement pointer and check for start of buffer collision
if(buffer_point < 0) buffer_point = 0;
* REMOVES TRAJECTORY AT CURRENT POINTER
* The trajectory in the buffer (at the current pointer) will be deleted
* from the list. This is irreversible.
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 */
* 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.
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 */
* 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
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 */
void kin_forward(t1, t2, x, y)
* 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
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)
* The inverse kinematics will be done, with consideration of the elbow
* 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
static double r, /* for trig in problem */
fact; /* elbow up/down correction */
r = sqrt(x*x + y*y); /* end effector from origin */
if(conf == 1) fact = -1.0; /* elbow down */
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)
* 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
static double t1s, /* work variables to keep the different */
t2s, /* joint configurations for alternate */
static int keep[1]; /* a work variable for the path size */
* 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.
* 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
if((train_flag == 1) && (train_alt == 1) && (plan != NETWORK)
tra_1(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_2(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_3(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_4(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_5(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
* If the neural network controller was called for then test the
* controller with only the current configuration.
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.
nt_tracker(n_output, t1s, t2s, t1e, t2e, path, start, n);
void tra_1(t1s, t2s, t1e, t2e, path, start, n)
* 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
static int i, /* work variable */
steps; /* number of calculated steps in path*/
static double t1d, /* total displacement of joints */
t1_step, /* joint angle step per time step */
x_s, /* cartesian start and stop points */
* Find cartesian points in space
kin_forward(t1s, t2s, &x_s, &y_s);
kin_forward(t1e, t2e, &x_e, &y_e);
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
/* 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(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(steps_1 > steps_2) steps = steps_1;
* set initial conditions of control
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++){
* For the cases where one joint must stand
* still after motion, this will allow it.
path[i].t1_position = path[i-1].t1_position + t1_step;
path[i].t1_velocity = t1_step/T_STEP;
path[i].t1_position = path[i-1].t1_position;
path[i].t2_position = path[i-1].t2_position + t2_step;
path[i].t2_velocity = t2_step/T_STEP;
path[i].t2_position = path[i-1].t2_position;
path[i].t1_acceleration = 0.0;
path[i].t2_acceleration = 0.0;
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_acceleration = 0.0;
path[i].t2_acceleration = 0.0;
kin_forward(path[i].t1_position, path[i].t2_position,
&path[i].x_position, &path[i].y_position);
* Number of steps in controller path
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)
* 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
static int i, j, /* work variables */
steps_2; /* number of calculated steps in path*/
static double t1d, /* total displacement of joints */
t1_step, /* joint angle step per time step */
x_s, /* cartesian start and stop points */
* Find cartesian points in space
kin_forward(t1s, t2s, &x_s, &y_s);
kin_forward(t1e, t2e, &x_e, &y_e);
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)
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_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)));
t1_step=t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP));
t1_step = t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP)
+(steps_1 + 2) *T_STEP*T_STEP);
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);
t1_step = (t1d/T_STEP/T_STEP - AMAX_T1*
(steps_1*steps_1 + 2.0*steps_1))
t1_step = (t1d/T_STEP/T_STEP - AMAX_T1*
(steps_1*steps_1 + 3.0*steps_1))
t2_step = (t2d/T_STEP/T_STEP - AMAX_T2*
(steps_2*steps_2 + 2.0*steps_2))
t2_step = (t2d/T_STEP/T_STEP - AMAX_T2*
(steps_2*steps_2 + 3.0*steps_2))
* set initial conditions of control
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);
for(i = 1+start; i <= steps_1+start; i++){
a_update(path, i, mult_1 * AMAX_T1 * fact_1, 1);
a_update(path, i, mult_1 * t1_step * fact_1, 1);
a_update(path, i, mult_1 * 0.5*t1_step * fact_1, 1);
a_update(path, i, mult_1 * t1_step * fact_1, 1);
a_update(path, i+2-flag_e1, -0.5*t1_step * mult_1*fact_1, 1);
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++){
a_update(path, i, -AMAX_T1 * mult_1*fact_1, 1);
a_update(path, i, -t1_step * mult_1*fact_1, 1);
a_update(path, i, -t1_step * mult_1*fact_1, 1);
path[i+j].t1_acceleration = 0.0;
for(j = 7; j <= (7+step_2-step_1);j++){
path[i+j].t1_acceleration = 0.0;
for(i = 1+start; i <= steps_2+start; i++){
a_update(path, i, mult_2 * AMAX_T2 * fact_2, 2);
a_update(path, i, mult_2 * t2_step * fact_2, 2);
a_update(path, i, mult_2 * 0.5*t2_step*fact_2, 2);
a_update(path, i, mult_2 * t2_step*fact_2, 2);
a_update(path, i+2-flag_e2, -0.5*t2_step * mult_2*fact_2, 2);
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++){
a_update(path, i, -AMAX_T2 * mult_2*fact_2, 2);
a_update(path, i, -t2_step * mult_2*fact_2, 2);
a_update(path, i, -t2_step * mult_2*fact_2, 2);
path[i+j].t2_acceleration = 0.0;
for(j = 7; j <= (7+step_1-step_2);j++){
path[i+j].t2_acceleration = 0.0;
* Number of steps in controller path
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)
* 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
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;
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)
* 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
* 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
static int i, /* work variables */
static double time, /* work variable */
mag2, /* dynamics parameters */
couple = 0.0; /* default coupling factor */
FILE *temp; /* file to read coupling */
temp = fopen("couple.dat", "r"); /* factor from disk */
fscanf(temp, "%lf\n", &couple);
init_inverse_dynamics(); /* needed for dynamics calcs */
* A path is created from the optimization section, using only the
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 */
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);
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)
* 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
static int i, /* work variables */
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
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 */
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);
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)
* GENERATE TRAJECTORY (SUB-OPTIMAL, WITH NO JOINT REVERSE)
* Generates a path using the optimization functions which vary coupling
* 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
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);
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 */
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);
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
* 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
* Note that the endpoints are stored in a section refered to as the
* trajectory buffer. The Buffer is used to path endpoints in general
* Also note that in general terminolgy conventions are not strictly adhered
#include "/usr/people/hugh/thesis/src/test/tester.c"
void a_update(); /* the only local function */
* The buffer for the set of test (or training) trajectories is loaded from
* Variables: file = the file which contains the trajectories to be loaded
static int i; /* work variable */
static char worker[50]; /* Input string */
static double x_s, y_s, /* path start */
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 */
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
for(i = 0; i < end_of_buffer; i++){
fscanf(in, "%lf,%lf,%lf,%lf\n",
/* convert cartesian endpoints*/
/* to all four sets of joint */
x_s*rad, y_s*rad, ELBOW_DOWN);
x_s*rad, y_s*rad, ELBOW_DOWN);
x_e*rad, y_e*rad, ELBOW_DOWN);
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
/* load joint angles for path */
for(i = 0; i < end_of_buffer; i++){
fscanf(in, "%lf,%lf,%lf,%lf\n",
* close the file and point to start of trajectories
* The trajectory endpoints will be saved to an ASCII file with this
* Variables: file = the file which contains the trajectories to be saved
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 */
buffer[i].t1_start, buffer[i].t2_start,
buffer[i].t1_end, buffer[i].t2_end);
* 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
* 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 */
for(i = start; i < (start + n[0]); i++){
"%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.
* 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
static int i; /* work variable */
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
* This moves the pointer to the trajectory buffer, down the list. It will
* cause the visible list to be updated to the new item.
* increment pointer and check for end of buffer collision
if(buffer_point >= end_of_buffer) buffer_point = end_of_buffer -1;
* MOVES POINTER UP TRAJECTORY LIST
* The pointer will be moved to the top of the list. The display will be
* decrement pointer and check for start of buffer collision
if(buffer_point < 0) buffer_point = 0;
* REMOVES TRAJECTORY AT CURRENT POINTER
* The trajectory in the buffer (at the current pointer) will be deleted
* from the list. This is irreversible.
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 */
* 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.
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 */
* 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
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 */
void kin_forward(t1, t2, x, y)
* 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
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)
* The inverse kinematics will be done, with consideration of the elbow
* 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
static double r, /* for trig in problem */
fact; /* elbow up/down correction */
r = sqrt(x*x + y*y); /* end effector from origin */
if(conf == 1) fact = -1.0; /* elbow down */
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)
* 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
static double t1s, /* work variables to keep the different */
t2s, /* joint configurations for alternate */
static int keep[1]; /* a work variable for the path size */
* 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.
* 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
if((train_flag == 1) && (train_alt == 1) && (plan != NETWORK)
tra_1(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_2(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_3(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_4(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
tra_5(t1sa, t2sa, t1ea, t2ea, path, start+n[0], keep);
* If the neural network controller was called for then test the
* controller with only the current configuration.
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.
nt_tracker(n_output, t1s, t2s, t1e, t2e, path, start, n);
void tra_1(t1s, t2s, t1e, t2e, path, start, n)
* 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
static int i, /* work variable */
steps; /* number of calculated steps in path*/
static double t1d, /* total displacement of joints */
t1_step, /* joint angle step per time step */
x_s, /* cartesian start and stop points */
* Find cartesian points in space
kin_forward(t1s, t2s, &x_s, &y_s);
kin_forward(t1e, t2e, &x_e, &y_e);
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
/* 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(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(steps_1 > steps_2) steps = steps_1;
* set initial conditions of control
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++){
* For the cases where one joint must stand
* still after motion, this will allow it.
path[i].t1_position = path[i-1].t1_position + t1_step;
path[i].t1_velocity = t1_step/T_STEP;
path[i].t1_position = path[i-1].t1_position;
path[i].t2_position = path[i-1].t2_position + t2_step;
path[i].t2_velocity = t2_step/T_STEP;
path[i].t2_position = path[i-1].t2_position;
path[i].t1_acceleration = 0.0;
path[i].t2_acceleration = 0.0;
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_acceleration = 0.0;
path[i].t2_acceleration = 0.0;
kin_forward(path[i].t1_position, path[i].t2_position,
&path[i].x_position, &path[i].y_position);
* Number of steps in controller path
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)
* 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
static int i, j, /* work variables */
steps_2; /* number of calculated steps in path*/
static double t1d, /* total displacement of joints */
t1_step, /* joint angle step per time step */
x_s, /* cartesian start and stop points */
* Find cartesian points in space
kin_forward(t1s, t2s, &x_s, &y_s);
kin_forward(t1e, t2e, &x_e, &y_e);
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)
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_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)));
t1_step=t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP));
t1_step = t1d/(((steps_1+2)*T_STEP)*((steps_1+2)*T_STEP)
+(steps_1 + 2) *T_STEP*T_STEP);
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);
t1_step = (t1d/T_STEP/T_STEP - AMAX_T1*
(steps_1*steps_1 + 2.0*steps_1))
t1_step = (t1d/T_STEP/T_STEP - AMAX_T1*
(steps_1*steps_1 + 3.0*steps_1))
t2_step = (t2d/T_STEP/T_STEP - AMAX_T2*
(steps_2*steps_2 + 2.0*steps_2))
t2_step = (t2d/T_STEP/T_STEP - AMAX_T2*
(steps_2*steps_2 + 3.0*steps_2))
* set initial conditions of control
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);
for(i = 1+start; i <= steps_1+start; i++){
a_update(path, i, mult_1 * AMAX_T1 * fact_1, 1);
a_update(path, i, mult_1 * t1_step * fact_1, 1);
a_update(path, i, mult_1 * 0.5*t1_step * fact_1, 1);
a_update(path, i, mult_1 * t1_step * fact_1, 1);
a_update(path, i+2-flag_e1, -0.5*t1_step * mult_1*fact_1, 1);
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++){
a_update(path, i, -AMAX_T1 * mult_1*fact_1, 1);
a_update(path, i, -t1_step * mult_1*fact_1, 1);
a_update(path, i, -t1_step * mult_1*fact_1, 1);
path[i+j].t1_acceleration = 0.0;
for(j = 7; j <= (7+step_2-step_1);j++){
path[i+j].t1_acceleration = 0.0;
for(i = 1+start; i <= steps_2+start; i++){
a_update(path, i, mult_2 * AMAX_T2 * fact_2, 2);
a_update(path, i, mult_2 * t2_step * fact_2, 2);
a_update(path, i, mult_2 * 0.5*t2_step*fact_2, 2);
a_update(path, i, mult_2 * t2_step*fact_2, 2);
a_update(path, i+2-flag_e2, -0.5*t2_step * mult_2*fact_2, 2);
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++){
a_update(path, i, -AMAX_T2 * mult_2*fact_2, 2);
a_update(path, i, -t2_step * mult_2*fact_2, 2);
a_update(path, i, -t2_step * mult_2*fact_2, 2);
path[i+j].t2_acceleration = 0.0;
for(j = 7; j <= (7+step_1-step_2);j++){
path[i+j].t2_acceleration = 0.0;
* Number of steps in controller path
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)
* 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
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;
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)
* 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
* 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
static int i, /* work variables */
static double time, /* work variable */
mag2, /* dynamics parameters */
couple = 0.0; /* default coupling factor */
FILE *temp; /* file to read coupling */
temp = fopen("couple.dat", "r"); /* factor from disk */
fscanf(temp, "%lf\n", &couple);
init_inverse_dynamics(); /* needed for dynamics calcs */
* A path is created from the optimization section, using only the
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 */
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);
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)
* 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
static int i, /* work variables */
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
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 */
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);
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)
* GENERATE TRAJECTORY (SUB-OPTIMAL, WITH NO JOINT REVERSE)
* Generates a path using the optimization functions which vary coupling
* 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
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);
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 */
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);
path[start].t1_position, path[start].t2_position,
&path[start].x_position, &path[start].y_position);
i++; /* add more to end of list */