/* This is file hd_model.c. It contains the simulation code used to solve a * system of differential equations which characterize the behavior of a * harmonic drive. Included in this file are two function which calculate * equations of motion for the two different joint configurations used for * the three harmonic-drive testing stations. The first two joints (shoudler * and elbow) use configuration1 and the third joint (wrist) uses the second * configuration. Additionally, code to calculate the equations for two * different harmonic-drive models is also included in this file. This code * is located in three separate functions which can be called arbitrarily * by any function which calculated the equations of motion of a dynamic * system. If desired, this program can also return the energy distribution * in the simulation and the stiffness of the given model. * * Copyright 1993 Massachusetts Institute of Technology * */ #include #include #include #include #include "hd_model.h" /* Declare and initialize the global variables. */ int debug = FALSE; int calc_energy = FALSE; int print_energy = FALSE; int stiffness_flag = FALSE; int stiffness_flag2 = FALSE; int ideal_flag = FALSE; int fast_plot; int plot = FALSE; int model; int joint = JOINT_TO_TEST; int num_selected_output_var = 0; int system_order = ORDER; int index[NUM_OF_OUTPUT_VAR]; float current_time; char graph_title[NUM_OF_OUTPUT_VAR][50]; char graph_units[NUM_OF_OUTPUT_VAR][50]; char output_datafile[NUM_OF_OUTPUT_VAR][100]; char plot_datafile[NUM_OF_OUTPUT_VAR][100]; /* Initialize some useful character strings. */ char gnuplot_filename[] = "plot_commands.gnu"; char *joint_name[NUM_OF_AXIS] = {"shld", "elb", "wrist"}; char outdatapath[] = "/com/ftp/pub/users/tut/hdsim/simulation_data/"; char indatapath[] = "/com/ftp/pub/users/tut/hdsim/simulation_code/"; char plotdatapath[] = "/com/ftp/pub/users/tut/hdsim/plot_data/"; /* Initialize the array of maximum velocity contraints on the non-linear * damping functions for each model. */ float max_friction_vel[HD_MODELS][NUM_OF_AXIS] = {{135.0, 300.0, 500.0}, {250.0, 550.0, 1000.0}}; /* Declare the functions which contain the equations of motion and the * Runge-Kutta solver function which is defined in the file rkqc.c. */ void derivs_config1(double, double *, double *); void derivs_config2(double, double *, double *); extern void rkqc(double *,double *,int,double *,double, double,double *,double *,double *,void (*)()); /* Declare the functions which calculate the energy for both of the two * joint configurations and the total system energy. */ void calculate_config_energy(double); void calculate_total_energy(double *, double *); /* Declare functions which calculate the total equations of motion for the * harmonic-drive system when the transmission is ideal. in this case, the * system reduced to a first-order system. */ void derivs_ideal_config1(double, double *, double *); void derivs_ideal_config2(double, double *, double *); /* Declare a pointer to a function that will hold the appropriate pointer to * the function which will calculate the equations of motion. */ void (*ptr_derivs)(double, double *, double *); /* Declare the individual functions which calculate the input parameters, dynamic * equations and energy values for the different harmonic-drive models. */ void calculate_rotary_params(void); void rotary_hd_model(double, double, double, double, double, double, double *, double *, double *); void rotary_hd_energy(double); void calculate_gear_tooth_params(void); void gear_tooth_hd_model(double, double, double, double, double, double, double *, double *, double *); void gear_tooth_hd_energy(double); /* Now initialize these functions into arrays of function pointers for * easy access. */ void (*calculate_params[HD_MODELS])(void) = {calculate_rotary_params, calculate_gear_tooth_params}; void (*hd_model[HD_MODELS])(double, double, double, double, double, double, double *, double *, double *) = {rotary_hd_model, gear_tooth_hd_model}; void (*hd_energy[HD_MODELS])(double) = {rotary_hd_energy, gear_tooth_hd_energy}; /* Declare the functions which (1) calculate stiffness data, (2) model the * saturation limits of the current amplifiers, and (3) determine the static * and dynamic friction values, respectively. */ void collect_stiffness_data(void); double iregulate(double, double, double); double calculate_friction(double, double, double, double, double, double, double, double, double, double); /* Declare the rest of the functions. */ void get_arguments(int argc, char **argv); void usage(void); void read_input_data(FILE *); void read_input_filenames(FILE *); void print_input_parameters(void); void print_variables(void); void export_vectors(FILE *, double *, double *, int, char *); void generate_gnuplot_file(char *); /***********************************************************************************/ /* */ /* Main Program */ /* */ /***********************************************************************************/ /* This main program opens and reads the input data files, initializes appropriate * variables and prints/plots the results. */ main(int argc, char **argv) { int h, i, j; int num_of_trials[NUM_OF_AXIS] = {SHLD_NUM_OF_DATA_RUNS, ELB_NUM_OF_DATA_RUNS, WRIST_NUM_OF_DATA_RUNS}; int good_steps[1], bad_steps[1]; float input_amps[NUM_OF_AXIS][SHLD_NUM_OF_DATA_RUNS] = {{1.6, 2.0, 2.4, 2.8, 3.2, 3.6, 4.0, 4.4, 5.0, 6.0}, {1.4, 1.8, 2.2, 2.6, 3.0, 3.4, 3.8, 4.5, 0.0, 0.0}, {0.28, 0.36, 0.40, 0.44, 0.48, 0.52, 0.56, 0.60, 0.0, 0.0}}; char unix_command[200]; char vector_name[100]; char *input_datafile[NUM_OF_AXIS] = {"hd_shld_input_data.dat", "hd_elb_input_data.dat", "hd_wrist_input_data.dat"}; FILE *infile; FILE *outfile[NUM_OF_OUTPUT_VAR]; extern int kount; /* Read and process the command-line arguments. */ get_arguments(argc, argv); /* Open the input data file for the given joint, read the simulation * parameters from the file, and then close the input file. */ open_file(&infile, indatapath, input_datafile[joint]); read_input_data(infile); close_file(infile); /* Calculate necessary parameters for the given model from the values read * from the input file. */ (*(calculate_params[model]))(); /* Print out the input data if in debugging mode. */ if (debug) print_input_parameters(); /* Set the pointer to the appropriate function containing the equations of * motion. */ if(joint == WRIST) { if(ideal_flag) ptr_derivs = derivs_ideal_config2; else ptr_derivs = derivs_config2; } else { if(ideal_flag) ptr_derivs = derivs_ideal_config1; else ptr_derivs = derivs_config1; } /* If stiffness data is to be collected, jump to the appropriate function. */ if(stiffness_flag) collect_stiffness_data(); /* Now read in the data from the other input file that provides information * about what data to output. */ open_file(&infile, indatapath, "hd_input_filenames.dat"); read_input_filenames(infile); close_file(infile); /* Now if the largest line number in the index[] vector is greater than * or equal to the first line of the energy variables in the second * input data file, then energy is required as an output, so that the * energy flag should be set so that energy is calculated. */ if(index[num_selected_output_var-1] >= TOTAL_ENERGY) calc_energy = TRUE; /* If the energy is to be calculated, increase the order of the system * to ORDER+1 in order to accomodate the additional total energy variable * which is to be integrated as a member of the state variable. */ if(calc_energy) system_order = ORDER + 1; /* If the -plot option is not selected, set the number of trials to 1. */ if(!plot) num_of_trials[joint] = 1; /* Now run the simulation num_of_trials-times by calling odeint(). Function * odeint() contains the Runga-Kutta numerical solver which calls the functions, * listed below which calculate the equations of motion for the selected dynamic * system. */ for(j = 0; j < num_of_trials[joint]; j++) { /* Change the value of the requested motor current if necessary. */ if(plot) irequested = input_amps[joint][j]; /* Restore or initialize the xstart[] values. */ for(i = 0; i < (ORDER+1); i++) xstart[i] = xstart_sav[i]; printf("\n Starting simulation at %f amps.... \n", irequested); odeint(xstart, system_order, initial_time, final_time, EPS, INITIAL_STEP, MINIMUM_STEP, good_steps, bad_steps, ptr_derivs, rkqc); printf("\n Simulation complete. \n"); if(debug) { printf("\n Number of good steps taken: %d", good_steps[0]); printf("\n Number of bad steps taken: %d\n", bad_steps[0]); } /* Initialize all output files, export the desired output data to them, * and close the files. If the -plot option is selected, then save * the specified data in the plot_data directory with the appropriate * filename. */ for(i = 0; i < num_selected_output_var; i++) { if(plot) { sprintf(plot_datafile[i], "%s.%4.2famps", output_datafile[i], input_amps[joint][j]); create_file(&(outfile[i]), plotdatapath, plot_datafile[i]); export_vectors(outfile[i], time, output_data[i+1], kount, output_datafile[i]); } else { create_file(&(outfile[i]), outdatapath, output_datafile[i]); export_vectors(outfile[i], time, output_data[i+1], kount, output_datafile[i]); } close_file(outfile[i]); } /* Now plot the results using xgraph, if slow printable plotting is desired, * or gnuplot, if the fast_plot option is selected. */ if(!plot) { if(fast_plot) { generate_gnuplot_file(gnuplot_filename); sprintf(unix_command, "gnuplot %s%s", outdatapath, gnuplot_filename); printf("\n Executing the unix command: %s\n", unix_command); system(unix_command); } else { for(i = 0; i < num_selected_output_var; i++) { sprintf(unix_command, "xgraph %s%s -t Harmonic_Drive_Simulation\ -y %s_%s -x Time_sec &", outdatapath, output_datafile[i], graph_title[i], graph_units[i]); printf("\n Executing the unix command: %s\n", unix_command); system(unix_command); } } } } } /***********************************************************************************/ /* */ /* Functions which describe the dynamics of the two joint configurations. */ /* */ /***********************************************************************************/ /* Function derivs_config1() can be used to solve the equations of motion for the * harmonic drive joint configuration in which: (1) the motor drives the wave- * generator and is mounted to the circular spline, (2) the flexspline is attached * to ground, and (3) the circular spline is the output port. */ void derivs_config1(t, x, dxdt) double t; double x[], dxdt[]; { /* Set the global time variable to the current time. */ current_time = t; /* Set the local variable definitions to the value of the state variable * determined on the previous step. */ v[POS_IN] = x[POS_IN_INDEX]; v[POS_OUT] = x[POS_OUT_INDEX]; v[VEL_IN] = x[VEL_IN_INDEX]; v[VEL_OUT] = x[VEL_OUT_INDEX]; /* From these positions and velocities, call the appropriate harmonic drive * model to calculate the resulting torques. */ (*(hd_model[model]))(v[POS_IN], 0.0, v[POS_OUT], v[VEL_IN], 0.0, v[VEL_OUT], &(v[TORQUE_WG]), &(v[TORQUE_FS]), &(v[TORQUE_CS])); /* Set the value of torque sensor to the torque read on the flexspline. */ v[TORQUE_SENSOR] = -v[TORQUE_FS]; /* Calculate the motor torque as a function of current: * Note that the current, iactual, is calculated by the function * iregulate() which takes into account the non-linear behavior of the * current amps. Note that v[CURRENT_SENSOR] must be initialized to zero. * Recall that the relative velocity of the motor is the absolute motor * velocity minus the absolute output velocity. */ v[CURRENT_SENSOR] = iregulate(v[CURRENT_SENSOR], irequested, (v[VEL_IN] - v[VEL_OUT])); v[TORQUE_MOTOR] = motor_kt * v[CURRENT_SENSOR]; /* Calculate the input and output damping. */ v[TORQUE_B_IN] = b_in * (v[VEL_IN] - v[VEL_OUT]); v[TORQUE_B_OUT] = b_out * v[VEL_OUT]; /* Finally, calculate the equations of motion. */ dxdt[POS_IN_INDEX] = v[VEL_IN]; dxdt[VEL_IN_INDEX] = (1.0 / (CONV * inertia_in)) * (v[TORQUE_MOTOR] - v[TORQUE_B_IN] - v[TORQUE_WG]); dxdt[POS_OUT_INDEX] = v[VEL_OUT]; dxdt[VEL_OUT_INDEX] = (1.0 / (CONV * inertia_out)) * (v[TORQUE_CS] - v[TORQUE_B_OUT] + v[TORQUE_B_IN]); /* Now calculate the values that the sensors actually read. */ v[INPUT_POSITION_SENSOR] = v[POS_IN] - v[POS_OUT]; v[INPUT_VELOCITY_SENSOR] = v[VEL_IN] - v[VEL_OUT]; v[OUTPUT_POSITION_SENSOR] = v[POS_OUT]; v[OUTPUT_VELOCITY_SENSOR] = v[VEL_OUT]; v[POSITION_ERROR] = (v[INPUT_POSITION_SENSOR] / N) - v[OUTPUT_POSITION_SENSOR]; /* If the total energy is selected as an output variable, call * the function that calculates the total power and integrates * it using the runga-kutta solver. */ if(calc_energy) calculate_total_energy(x, dxdt); /* If the debugging option is selected, call the function to print * out some variables. */ if(debug) print_variables(); } /* Function derivs_config2() can be used to solve the equations of motion for * the harmonic drive joint configuration in which: (1) the motor drives the * wave-generator and is mounted to the circular spline, (2) the flexspline is * attached to the output link, and (3) the circular spline attached to ground. */ void derivs_config2(t, x, dxdt) double t; double x[], dxdt[]; { /* Set the global time variable to the current time. */ current_time = t; /* Set the local variable definitions to the value of the state variable * determined on the previous step. */ v[POS_IN] = x[POS_IN_INDEX]; v[POS_OUT] = x[POS_OUT_INDEX]; v[VEL_IN] = x[VEL_IN_INDEX]; v[VEL_OUT] = x[VEL_OUT_INDEX]; /* From these positions and velocities, call the appropriate harmonic drive * model to calculate the resulting torques. */ (*(hd_model[model]))(v[POS_IN], v[POS_OUT], 0.0, v[VEL_IN], v[VEL_OUT], 0.0, &(v[TORQUE_WG]), &(v[TORQUE_FS]), &(v[TORQUE_CS])); /* Calculate the motor torque as a function of current: * Note that the current, iactual, is calculated by the function * iregulate() which takes into account the non-linear behavior of the * current amps. Note that v[CURRENT_SENSOR] must be initialized to zero. */ v[CURRENT_SENSOR] = iregulate(v[CURRENT_SENSOR], irequested, v[VEL_IN]); v[TORQUE_MOTOR] = motor_kt * v[CURRENT_SENSOR]; /* Calculate the input and output damping. */ v[TORQUE_B_IN] = b_in * v[VEL_IN]; v[TORQUE_B_OUT] = b_out * v[VEL_OUT]; /* The torque sensor is mounted between the motor and circular spline and * ground, and therefore sees the reaction torque from the motor, the motor * damping torque, and the circular spline torque. */ v[TORQUE_SENSOR] = v[TORQUE_CS] - v[TORQUE_MOTOR] + v[TORQUE_B_IN]; /* Finally, calculate the equations of motion. */ dxdt[POS_IN_INDEX] = v[VEL_IN]; dxdt[VEL_IN_INDEX] = (1.0 / (CONV * inertia_in)) * (v[TORQUE_MOTOR] - v[TORQUE_B_IN] - v[TORQUE_WG]); dxdt[POS_OUT_INDEX] = v[VEL_OUT]; dxdt[VEL_OUT_INDEX] = (1.0 / (CONV * inertia_out)) * (-v[TORQUE_FS] - v[TORQUE_B_OUT]); /* Now calculate the values that the sensors actually read. */ v[INPUT_POSITION_SENSOR] = v[POS_IN]; v[INPUT_VELOCITY_SENSOR] = v[VEL_IN]; v[OUTPUT_POSITION_SENSOR] = v[POS_OUT]; v[OUTPUT_VELOCITY_SENSOR] = v[VEL_OUT]; v[POSITION_ERROR] = (v[INPUT_POSITION_SENSOR] / N) + v[OUTPUT_POSITION_SENSOR]; /* If the total energy is selected as an output variable, call * the function that calculates the total power and integrates * it using the runga-kutta solver. */ if(calc_energy) calculate_total_energy(x, dxdt); /* If the debugging option is selected, call the function to print * out some variables. */ if(debug) print_variables(); } /***********************************************************************************/ /* */ /* Functions which describe the energy of the two joint configurations. */ /* */ /***********************************************************************************/ /* Function calculate_total_energy() calculates the total power in the system * of equations presented in derivs_config1() or derivs_config2(). This power * is determined on every time step and stored in the state vector which is * is solved by the Runge-Kutta solver. This can be used to verify that energy * is being conserved in both dynamic models. */ void calculate_total_energy(x, dxdt) double x[], dxdt[]; { double power_b_in, power_hd_out; /* First store the integrated power in the total energy variable. */ v[TOTAL_ENERGY]= x[ENERGY]; /* Now calculate the input damping power loss depending on the joint * configuration. Also calculate the appropriate torque on the output * port of the harmonic drive. */ if(joint == WRIST) { power_b_in = v[TORQUE_B_IN] * (v[VEL_IN]*CONV); power_hd_out = -v[TORQUE_FS] * (v[VEL_OUT]*CONV); } else { power_b_in = v[TORQUE_B_IN] * ((v[VEL_IN] - v[VEL_OUT])*CONV); power_hd_out = v[TORQUE_CS] * (v[VEL_OUT]*CONV); } /* Calculate the total instantaneous power in the system. */ dxdt[ENERGY] = ((v[TORQUE_MOTOR] * (v[VEL_IN]*CONV)) - (inertia_in * (v[VEL_IN]*CONV) * (dxdt[VEL_IN_INDEX]*CONV)) - (inertia_out * (v[VEL_OUT]*CONV) * (dxdt[VEL_OUT_INDEX]*CONV)) - (v[TORQUE_WG] * (v[VEL_IN]*CONV)) + (power_hd_out) - (power_b_in) - (v[TORQUE_B_OUT] * (v[VEL_OUT]*CONV))); } /* Function calculate_config_energy() is called by the function odeint() after * each complete time step. This function takes the current simulation time * step and uses a simple Euler integration scheme to determine the energy in * the system, excluding the harmonic drive. Since this function relies on * values calculated in the harmonic drive energy function, it should be called * after the specific harmonic-drive energy function is called. Due to the * similarities between the equations in derivs_config1() and derivs_config2(), * this energy function is valid for both models. */ void calculate_config_energy(delta_t) double delta_t; { double vel_in_rad, vel_out_rad; /* Be sure that all velocities and positions are in radians to ensure * proper unit-matching. */ vel_in_rad = v[VEL_IN] * CONV; vel_out_rad = v[VEL_OUT] * CONV; /* Calculate the energy input by the motor to the system by integrating * its power input over time. */ v[MOTOR_ENERGY] = v[MOTOR_ENERGY] + (delta_t * v[TORQUE_MOTOR] * vel_in_rad); /* Calculate the kinetic energy of the inertias from (I*(w^2)/2). */ v[KINETIC_ENERGY_IN] = (inertia_in * vel_in_rad * vel_in_rad) / 2.0; v[KINETIC_ENERGY_OUT] = (inertia_out * vel_out_rad * vel_out_rad) / 2.0; /* Calculate the damping energy by integrating the power dissipation over * time. */ if(joint == WRIST) v[INPUT_DAMPING_ENERGY] = (v[INPUT_DAMPING_ENERGY] + (delta_t * (v[TORQUE_B_IN] * vel_in_rad))); else v[INPUT_DAMPING_ENERGY] = (v[INPUT_DAMPING_ENERGY] + (delta_t * (v[TORQUE_B_IN] * (vel_in_rad - vel_out_rad)))); v[OUTPUT_DAMPING_ENERGY] = (v[OUTPUT_DAMPING_ENERGY] + (delta_t * (v[TORQUE_B_OUT] * vel_out_rad))); /* Now calculate some aggregate quantities. */ v[TOTAL_KINETIC_ENERGY] = v[KINETIC_ENERGY_IN] + v[KINETIC_ENERGY_OUT]; v[TOTAL_INPUT_ENERGY] = v[MOTOR_ENERGY]; v[TOTAL_LOST_ENERGY] = (v[INPUT_DAMPING_ENERGY] + v[WG_FRICTION_ENERGY] + v[TOOTH_TIP_FRICTION_ENERGY] + v[TOOTH_SURFACE_TOTAL_LOSS_ENERGY] + v[OUTPUT_DAMPING_ENERGY]); /* Now print out the energy values if the print_energy option is selected. */ if (print_energy) { printf("\n total energy: %25.20lf", v[TOTAL_ENERGY]); printf("\n total kinetic energy: %20.15lf", v[TOTAL_KINETIC_ENERGY]); printf("\n total potential energy: %20.15lf", v[TOTAL_POTENTIAL_ENERGY]); printf("\n total input energy: %20.15lf", v[TOTAL_INPUT_ENERGY]); printf("\n total lost energy: %20.15lf", v[TOTAL_LOST_ENERGY]); printf("\n motor energy: %20.15lf", v[MOTOR_ENERGY]); printf("\n kinetic energy in: %20.15lf", v[KINETIC_ENERGY_IN]); printf("\n kinetic energy out: %20.15lf", v[KINETIC_ENERGY_OUT]); printf("\n spring energy: %20.15lf", v[SPRING_ENERGY]); printf("\n cyclic torque energy: %20.15lf", v[CYCLIC_ENERGY]); printf("\n delta_t: %20.15lf", delta_t); printf("\n"); } } /***********************************************************************************/ /* */ /* Functions which describe behavior of system with ideal transmission. */ /* */ /***********************************************************************************/ /* Function derivs_ideal_config1() calculates the equations of motion for a * configuration-1 sytstem that has an ideal harmonic-drive transmission. */ void derivs_ideal_config1(t, x, dxdt) double t; double x[], dxdt[]; { /* Set the global time variable to the current time. */ current_time = t; /* Set the local variable definitions to the value of the state variable * determined on the previous step. */ v[POS_IN] = x[POS_IN_INDEX]; v[POS_OUT] = x[POS_OUT_INDEX]; v[VEL_IN] = x[VEL_IN_INDEX]; v[VEL_OUT] = x[VEL_OUT_INDEX]; /* Calculate the motor torque as a function of current. For the ideal * model, assume that the current amps function ideally as well. */ v[CURRENT_SENSOR] = irequested; v[TORQUE_MOTOR] = motor_kt * v[CURRENT_SENSOR]; /* Calculate the input and output damping. */ v[TORQUE_B_IN] = b_in * (v[VEL_IN] - v[VEL_OUT]); v[TORQUE_B_OUT] = b_out * v[VEL_OUT]; /* Finally, calculate the equations of motion. */ dxdt[POS_IN_INDEX] = v[VEL_IN]; dxdt[VEL_IN_INDEX] = ((1.0 / (CONV * (inertia_in + (inertia_out/((N+1.0)*(N+1.0)))))) * (v[TORQUE_MOTOR] - v[TORQUE_B_IN] - ((v[TORQUE_B_OUT] - v[TORQUE_B_IN])/(N+1.0)))); dxdt[POS_OUT_INDEX] = dxdt[POS_IN_INDEX]/(N+1.0); dxdt[VEL_OUT_INDEX] = dxdt[VEL_IN_INDEX]/(N+1.0); /* Now calculate the values that the sensors actually read. */ v[INPUT_POSITION_SENSOR] = v[POS_IN] - v[POS_OUT]; v[INPUT_VELOCITY_SENSOR] = v[VEL_IN] - v[VEL_OUT]; v[OUTPUT_POSITION_SENSOR] = v[POS_OUT]; v[OUTPUT_VELOCITY_SENSOR] = v[VEL_OUT]; v[POSITION_ERROR] = (v[INPUT_POSITION_SENSOR] / N) - v[OUTPUT_POSITION_SENSOR]; v[TORQUE_FS] = (-N/(N+1.0)) * (((CONV * inertia_in) * (dxdt[VEL_IN_INDEX]/(N+1.0))) + v[TORQUE_B_OUT] - v[TORQUE_B_IN]); v[TORQUE_SENSOR] = -v[TORQUE_FS]; /* Calculate the torques on the remaining harmonic-drive ports. */ v[TORQUE_WG] = (-1.0/N) * v[TORQUE_FS]; v[TORQUE_CS] = (-(N+1.0)/N) * v[TORQUE_FS]; /* If the total energy is selected as an output variable, call * the function that calculates the total power and integrates * it using the runga-kutta solver. */ if(calc_energy) calculate_total_energy(x, dxdt); /* If the debugging option is selected, call the function to print * out some variables. */ if(debug) print_variables(); } /* Function derivs_ideal_config2() calculates the equations of motion for a * configuration-2 sytstem that has an ideal harmonic-drive transmission. */ void derivs_ideal_config2(t, x, dxdt) double t; double x[], dxdt[]; { /* Set the global time variable to the current time. */ current_time = t; /* Set the local variable definitions to the value of the state variable * determined on the previous step. */ v[POS_IN] = x[POS_IN_INDEX]; v[POS_OUT] = x[POS_OUT_INDEX]; v[VEL_IN] = x[VEL_IN_INDEX]; v[VEL_OUT] = x[VEL_OUT_INDEX]; /* Calculate the motor torque as a function of current. For the ideal * model, assume that the current amps function ideally as well. */ v[CURRENT_SENSOR] = irequested; v[TORQUE_MOTOR] = motor_kt * v[CURRENT_SENSOR]; /* Calculate the input and output damping. */ v[TORQUE_B_IN] = b_in * v[VEL_IN]; v[TORQUE_B_OUT] = b_out * v[VEL_OUT]; /* Finally, calculate the equations of motion. */ dxdt[POS_IN_INDEX] = v[VEL_IN]; dxdt[VEL_IN_INDEX] = ((1.0 / (CONV * (inertia_in - (inertia_out/(N*N))))) * (v[TORQUE_MOTOR] - v[TORQUE_B_IN] - (v[TORQUE_B_OUT]/N))); dxdt[POS_OUT_INDEX] = dxdt[POS_IN_INDEX] / (-N); dxdt[VEL_OUT_INDEX] = dxdt[VEL_IN_INDEX] / (-N); /* Now calculate the values that the sensors actually read. */ v[INPUT_POSITION_SENSOR] = v[POS_IN] - v[POS_OUT]; v[INPUT_VELOCITY_SENSOR] = v[VEL_IN] - v[VEL_OUT]; v[OUTPUT_POSITION_SENSOR] = v[POS_OUT]; v[OUTPUT_VELOCITY_SENSOR] = v[VEL_OUT]; v[POSITION_ERROR] = (v[INPUT_POSITION_SENSOR] / N) + v[OUTPUT_POSITION_SENSOR]; v[TORQUE_FS] = -(((CONV * inertia_out) * (dxdt[VEL_IN_INDEX]/(-N))) + v[TORQUE_B_OUT]); /* Calculate the torques on the remaining harmonic-drive ports. */ v[TORQUE_WG] = (-1.0/N) * v[TORQUE_FS]; v[TORQUE_CS] = (-(N+1.0)/N) * v[TORQUE_FS]; /* The torque sensor is mounted between the motor and circular spline and * ground, and therefore sees the reaction torque from the motor, the motor * damping torque, and the circular spline torque. */ v[TORQUE_SENSOR] = v[TORQUE_CS] - v[TORQUE_MOTOR] + v[TORQUE_B_IN]; /* If the total energy is selected as an output variable, call * the function that calculates the total power and integrates * it using the runga-kutta solver. */ if(calc_energy) calculate_total_energy(x, dxdt); /* If the debugging option is selected, call the function to print * out some variables. */ if(debug) print_variables(); } /***********************************************************************************/ /* */ /* Functions determining equations for two different harmonic-drive models. */ /* */ /***********************************************************************************/ /* Function rotary_hd_model() calculates the three-port harmonic-drive * equations for a non-ideal rotational model. */ void rotary_hd_model(pos_wg, pos_fs, pos_cs, vel_wg, vel_fs, vel_cs, torque_wg, torque_fs, torque_cs) double pos_wg, pos_fs, pos_cs; double vel_wg, vel_fs, vel_cs; double *torque_wg, *torque_fs, *torque_cs; { /* Store the velocities and positions and the fs, cs, and wg ports * in the global data vector to begin calculations. */ v[POS_WG] = pos_wg; v[POS_FS] = pos_fs; v[POS_CS] = pos_cs; v[VEL_WG] = vel_wg; v[VEL_FS] = vel_fs; v[VEL_CS] = vel_cs; /* Calculate the continuity constraint of the cyclic torque. */ v[POS_K] = v[POS_WG]; v[VEL_K] = v[VEL_WG]; /* Calculate some other continuity constraints. */ v[POS_N_FS] = v[POS_FS]; v[VEL_N_FS] = v[VEL_FS]; v[POS_N_CS] = v[POS_CS]; v[VEL_N_CS] = v[VEL_CS]; /* Determine the kinematic error function and its derivative such that * (d/dt)v[ERFN] = v[VEL_WG] v[DERF]. */ v[ERFN] = ((error_amplitude0 * Sin(((1.0 * v[POS_WG]) + error_phase0) * CONV)) + (error_amplitude1 * Sin(((2.0 * v[POS_WG]) + error_phase1) * CONV)) + (error_amplitude2 * Sin(((4.0 * v[POS_WG]) + error_phase2) * CONV))); v[DERF] = ((1.0 * CONV * error_amplitude0 * Cos(((1.0 * v[POS_WG]) + error_phase0) * CONV)) + (2.0 * CONV * error_amplitude1 * Cos(((2.0 * v[POS_WG]) + error_phase1) * CONV)) + (4.0 * CONV * error_amplitude2 * Cos(((4.0 * v[POS_WG]) + error_phase2) * CONV))); /* Now calculate the position and velocity equations imposed by the * three-port hamonic-drive gear-reduction with position error included. */ v[POS_N_WG] = (((N + 1.0) * v[POS_N_CS]) - (N * v[POS_N_FS]) + (N * v[ERFN])); v[VEL_N_WG] = (((N + 1.0) * v[VEL_N_CS]) - (N * v[VEL_N_FS]) + (N * v[VEL_WG] * v[DERF])); /* Calculate the static, dynamic, and cyclic friction which acts between the * circular spline and flexspline. */ v[VEL_HD_FRICTION] = v[VEL_N_CS] - v[VEL_N_FS]; v[TORQUE_HD_FRICTION] = calculate_friction(v[VEL_HD_FRICTION], b_hd_constant, b_hd1, b_hd2, stiction_torque_hd, stiction_vel_hd, v[POS_OUT], cyclic_friction_amp_hd, cyclic_friction_phase_hd, (max_friction_vel[model][joint])); /* For the sake of continuity in the simulation, ramp the constant friction * in the harmonic drive from zero to the desired value if at the * beginning of the step-response trial. */ if(!(stiffness_flag || stiffness_flag2)) if(current_time < 0.05) v[TORQUE_HD_FRICTION] = ((((current_time - 0.05)/0.05) + 1.0) * v[TORQUE_HD_FRICTION]); /* Find the harmonic drive stiffness coefficients: */ v[K1] = (k1_constant + k1_amplitude * Sin(((2.0 * v[POS_WG]) + k1_phase) * CONV)); v[K2] = (k2_constant + k2_amplitude * Sin(((2.0 * v[POS_WG]) + k2_phase) * CONV)); /* Calculate the torque on the spring. */ v[TORQUE_K] = ((v[K1] * (v[POS_K] - v[POS_N_WG])) + (v[K2] * Power((v[POS_K] - v[POS_N_WG]), 3))); /* Calculate the compatibility, or torque-balance, requirement. */ v[TORQUE_N_WG] = v[TORQUE_K]; /* Calculate the cyclic torque. */ v[TORQUE_CYCLIC] = (cyclic_amplitude * Sin(((2.0 * v[POS_WG]) + cyclic_phase) * CONV)); /* Now apply the torque constraint imposed by the three-port gear-reduction * with position error included. Note that the torque on the flexspline * is defined to be positive in the negative rotation direction. */ v[TORQUE_N_FS] = -((-N / (1.0 - (N * v[DERF]))) * v[TORQUE_N_WG]); v[TORQUE_N_CS] = ((N + 1.0) / (1.0 - (N * v[DERF]))) * v[TORQUE_N_WG]; /* Finally, calculate the torque on all of the harmonic-drive ports. */ v[TORQUE_WG] = v[TORQUE_K] - v[TORQUE_CYCLIC]; v[TORQUE_FS] = v[TORQUE_N_FS] - v[TORQUE_HD_FRICTION]; v[TORQUE_CS] = v[TORQUE_N_CS] - v[TORQUE_HD_FRICTION]; /* Finally, return the torques on the three hd ports. */ *torque_wg = v[TORQUE_WG]; *torque_fs = v[TORQUE_FS]; *torque_cs = v[TORQUE_CS]; } /* Function gear_tooth_hd_model() calculates the three-port harmonic-drive * equations for the harmonic drive representation which uses inclined planes to * simulate the tooth-rubbing behavior inside the drive. */ void gear_tooth_hd_model(pos_wg, pos_fs, pos_cs, vel_wg, vel_fs, vel_cs, torque_wg, torque_fs, torque_cs) double pos_wg, pos_fs, pos_cs; double vel_wg, vel_fs, vel_cs; double *torque_wg, *torque_fs, *torque_cs; { /* Store the velocities and positions and the fs, cs, and wg ports * in the global data vector to begin calculations. */ v[POS_WG] = pos_wg; v[POS_FS] = pos_fs; v[POS_CS] = pos_cs; v[VEL_WG] = vel_wg; v[VEL_FS] = vel_fs; v[VEL_CS] = vel_cs; /* First, calculate the continuity (or kinematic) constraints. * * Find the velocity of the wave generator with respect to the flexspline. */ v[POS_WG_RELATIVE] = v[POS_WG] - v[POS_FS]; v[VEL_WG_RELATIVE] = v[VEL_WG] - v[VEL_FS]; /* Determine the kinematic error function and its derivative such that * (d/dt)v[ERFN] = v[VEL_WG_RELATIVE] v[DERF]. */ v[ERFN] = (N*tan1 * ((error_amplitude0 * Sin(((1.0 * v[POS_WG_RELATIVE]) + error_phase0) * CONV)) + (error_amplitude1 * Sin(((2.0 * v[POS_WG_RELATIVE]) + error_phase1) * CONV)) + (error_amplitude2 * Sin(((4.0 * v[POS_WG_RELATIVE]) + error_phase2) * CONV)))); v[DERF] = (N*tan1 * ((1.0 * CONV * error_amplitude0 * Cos(((1.0 * v[POS_WG_RELATIVE]) + error_phase0) * CONV)) + (2.0 * CONV * error_amplitude1 * Cos(((2.0 * v[POS_WG_RELATIVE]) + error_phase1) * CONV)) + (4.0 * CONV * error_amplitude2 * Cos(((4.0 * v[POS_WG_RELATIVE]) + error_phase2) * CONV)))); /* Find the radial (vertical) movement of the tooth base. */ v[POS_TOOTH_BASE] = tan1 * v[POS_WG_RELATIVE]; v[VEL_TOOTH_BASE] = tan1 * v[VEL_WG_RELATIVE]; /* Find the radial movement at the output of the position error element. */ v[POS_ERR] = v[POS_TOOTH_BASE] + v[ERFN]; v[VEL_ERR] = v[VEL_TOOTH_BASE] + (v[VEL_WG_RELATIVE] * v[DERF]); /* Find the radial movement of the tooth tip. */ v[POS_TOOTH_TIP] = (v[POS_CS] - v[POS_FS]) / tan2; v[VEL_TOOTH_TIP] = (v[VEL_CS] - v[VEL_FS]) / tan2; /* Find the velocity at the wave generator surface. */ v[POS_WG_SURFACE] = ((cos1 * v[POS_WG]) + (sin1 * v[POS_TOOTH_BASE]) - (cos1 * v[POS_FS])); v[VEL_WG_SURFACE] = ((cos1 * v[VEL_WG]) + (sin1 * v[VEL_TOOTH_BASE]) - (cos1 * v[VEL_FS])); /* Find the velocity at the gear-tooth surface. */ v[POS_TOOTH_SURFACE] = ((cos2 * v[POS_TOOTH_TIP]) + (sin2 * v[POS_CS]) - (sin2 * v[POS_FS])); v[VEL_TOOTH_SURFACE] = ((cos2 * v[VEL_TOOTH_TIP]) + (sin2 * v[VEL_CS]) - (sin2 * v[VEL_FS])); /* Now, calculate the Constitutive Relationships: * * Find the wave generator friction. */ v[TORQUE_WG_FRICTION] = calculate_friction(v[VEL_WG_SURFACE], b_wg_constant, b_wg1, b_wg2, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0); /* Find the friction at the tooth tip. */ v[TORQUE_TOOTH_TIP_FRICTION] = calculate_friction((v[VEL_TOOTH_BASE]- v[VEL_TOOTH_TIP]), b_tooth_tip_constant, b_tooth_tip1, b_tooth_tip2, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0); /* The total friction at the gear-tooth surface is due to the coulomb friction * (mu * N), the constant friction (b_tooth_surface_constant), the velocity- * dependent friction, and the cyclic friction. Calculate the component due * to everything except the coulomb friction. */ v[TORQUE_TOOTH_SURFACE_FRICTION] = calculate_friction(v[VEL_TOOTH_SURFACE], b_tooth_surface_constant, b_tooth_surface1, b_tooth_surface2, stiction_torque_tooth, stiction_vel_tooth, v[POS_OUT], cyclic_friction_amp_tooth, cyclic_friction_phase_tooth, (max_friction_vel[model][joint])); /* Find the harmonic drive stiffness coefficients: */ v[K1] = (k1_constant + k1_amplitude * Sin(((2.0 * v[POS_WG_RELATIVE]) + k1_phase) * CONV)); v[K2] = (k2_constant + k2_amplitude * Sin(((2.0 * v[POS_WG_RELATIVE]) + k2_phase) * CONV)); /* Calculate the torque on the spring. */ v[TORQUE_K] = ((v[K1] * (v[POS_ERR] - v[POS_TOOTH_TIP])) + (v[K2] * Power((v[POS_ERR] - v[POS_TOOTH_TIP]), 3))); /* Calculate the cyclic torque. */ v[TORQUE_CYCLIC] = (cyclic_amplitude * Sin(((2.0 * v[POS_WG_RELATIVE]) + cyclic_phase) * CONV)); /* From the power conservation constraint on the position error element * calculate the torque seen on the input side. */ v[TORQUE_ERR_IN] = (1.0 + (v[DERF] / tan1)) * v[TORQUE_K]; /* Set the sign of mu to control the direction of the coulomb friction force * along the gear-tooth surface. The sign of mu should be identical to the * sign of the surface rubbing-velocity. */ mu = copysign(mu_save, v[VEL_TOOTH_SURFACE]); /* If stiffness data is being collected, the friction acts in the opposite * direction as the joint is being loaded. */ if(stiffness_flag) mu = -mu_save; /* Now calculate the force/torque compatibility equations: * * Determine the output normal force. */ v[TORQUE_OUTPUT_NORMAL] = v[TORQUE_K] + v[TORQUE_TOOTH_TIP_FRICTION]; /* For the sake of continuity in the simulation, ramp the constant friction * on the gear-tooth surface from zero to the desired value if at the * beginning of the step-response trial. This is a quick fix that should * be implemented in more general terms for all of the constant friction * components. */ if(!(stiffness_flag || stiffness_flag2)) if(current_time < 0.05) v[TORQUE_TOOTH_SURFACE_FRICTION] = ((((current_time - 0.05)/0.05) + 1.0) * v[TORQUE_TOOTH_SURFACE_FRICTION]); /* Find the gear-tooth normal force. Note that this equation might need to * be calculated iteratively if the velocity is zero since the coulomb * friction becomes a different function of the normal force. For now, I * will start my simulations at a non-zero initial velocity so that the * velocity will never be zero. */ v[TORQUE_TOOTH_SURFACE_NORMAL] = ((v[TORQUE_OUTPUT_NORMAL] - (cos2 * v[TORQUE_TOOTH_SURFACE_FRICTION])) / (sin2 + (mu * cos2))); /* If the gear-tooth surface-normal is negative then the mechanism through * which the friction acts at the gear-tooth surface changes. In order to * describe this new mechanism, a different, but similar, model is needed. * Unfortunately, changing to the new model at this point in the simulation * poses significant numerical problems. To avoid this mess, when the normal * force becomes negative, I will set the coulomb friction to zero and * recalculate the tooth-surface normal force. */ if(v[TORQUE_TOOTH_SURFACE_NORMAL] < 0.0) { mu = 0.0; v[TORQUE_TOOTH_SURFACE_NORMAL] = ((v[TORQUE_OUTPUT_NORMAL] - (cos2 * v[TORQUE_TOOTH_SURFACE_FRICTION])) / (sin2 + (mu * cos2))); } /* Given this correct tooth-surface normal force, the coulomb friction and * total friction at the tooth interface can be calculated. */ v[TORQUE_TOOTH_SURFACE_COULOMB] = mu * v[TORQUE_TOOTH_SURFACE_NORMAL]; v[TORQUE_TOOTH_SURFACE_TOTAL_LOSS] = (v[TORQUE_TOOTH_SURFACE_COULOMB] + v[TORQUE_TOOTH_SURFACE_FRICTION]); /* Calculate the input normal force. */ v[TORQUE_INPUT_NORMAL] = v[TORQUE_ERR_IN] + v[TORQUE_TOOTH_TIP_FRICTION]; /* Now determine the normal force on the wave generator. */ v[TORQUE_WG_NORMAL] = (1.0 / cos1) * (v[TORQUE_INPUT_NORMAL] + (v[TORQUE_WG_FRICTION] * sin1)); /* Find the normal force on the tooth tip. */ v[TORQUE_TOOTH_TIP_NORMAL] = ((cos2 * v[TORQUE_TOOTH_SURFACE_NORMAL]) - (sin2 * v[TORQUE_TOOTH_SURFACE_TOTAL_LOSS])); /* Now the torques on the wave_generator, flexspline, and circular_spline can * be determined. */ v[TORQUE_WG] = ((v[TORQUE_CYCLIC]) + (v[TORQUE_WG_FRICTION] * cos1) + (v[TORQUE_WG_NORMAL] * sin1)); v[TORQUE_FS] = ((v[TORQUE_TOOTH_TIP_NORMAL]) - (v[TORQUE_WG_FRICTION] * cos1) - (v[TORQUE_WG_NORMAL] * sin1)); v[TORQUE_CS] = ((v[TORQUE_TOOTH_SURFACE_NORMAL] * cos2) - (v[TORQUE_TOOTH_SURFACE_TOTAL_LOSS] * sin2)); /* Finally, return the torques on the three hd ports. */ *torque_wg = v[TORQUE_WG]; *torque_fs = v[TORQUE_FS]; *torque_cs = v[TORQUE_CS]; } /***********************************************************************************/ /* */ /* Functions which describe the energy of each harmonic-drive model. */ /* */ /***********************************************************************************/ /* Function rotary_hd_energy() calculates the energy for the non-ideal, rotary * harmonic-drive model. It is called by the function odeint() after the completion * of each time step in order to integrate the individual energy components of the * harmonic drive model with tooth rubbing. This function takes the current time * step of the solver and integrates the energy over that time step using a simple * Euler integration scheme. As you might expect, these energy calculations can * sometimes have appreciable error, but they are useful, for the most part. */ void rotary_hd_energy(delta_t) double delta_t; { double vel_wg_rad, vel_fs_rad, vel_cs_rad; double vel_hd_friction_rad; static double old_pos_k = 0.0; /* Be sure that all velocities and positions are in radians to ensure * proper unit-matching. */ vel_wg_rad = v[VEL_WG] * CONV; vel_fs_rad = v[VEL_FS] * CONV; vel_cs_rad = v[VEL_CS] * CONV; vel_hd_friction_rad = v[VEL_HD_FRICTION] * CONV; /* Calculate the potential energy stored in the spring by integrating * the spring force over the spring displacement. */ v[SPRING_ENERGY] = (v[SPRING_ENERGY] + ((((v[POS_K] - v[POS_N_WG]) - old_pos_k) * CONV) * v[TORQUE_K])); /* Calculate the cyclic torque energy by integrating the power over time. */ v[CYCLIC_ENERGY] = (v[CYCLIC_ENERGY] + (delta_t * vel_wg_rad * (-v[TORQUE_CYCLIC]))); /* Calculate the energy lost to friction by integrating the power dissipation * over time. */ v[HD_FRICTION_ENERGY] = (v[HD_FRICTION_ENERGY] + (delta_t * (v[TORQUE_HD_FRICTION] * vel_hd_friction_rad))); /* Calculate total potential energy in the system assuming the cyclic torque * can be treated like a spring. */ v[TOTAL_POTENTIAL_ENERGY] = v[SPRING_ENERGY] + v[CYCLIC_ENERGY]; old_pos_k = (v[POS_K] - v[POS_N_WG]); } /* Function gear_tooth_hd_energy() calculates the energy for the harmonic-drive * model which includes gear-tooth-rubbing effects. It is called by the function * odeint() after the completion of each time step in order to integrate the * individual energy components of the harmonic drive model with tooth rubbing. * This function takes the current time step of the solver and integrates the * energy over that time step using a simple Euler integration scheme. As you * might expect, these energy calculations can sometimes have appreciable error, * but they are useful, for the most part. */ void gear_tooth_hd_energy(delta_t) double delta_t; { double vel_wg_rad, vel_fs_rad, vel_cs_rad; double vel_wg_surface_rad, vel_tooth_surface_rad; double vel_tooth_tip_rad, vel_tooth_base_rad; static double old_pos_k = 0.0; /* Be sure that all velocities and positions are in radians to ensure * proper unit-matching. */ vel_wg_rad = v[VEL_WG] * CONV; vel_fs_rad = v[VEL_FS] * CONV; vel_cs_rad = v[VEL_CS] * CONV; vel_wg_surface_rad = v[VEL_WG_SURFACE] * CONV; vel_tooth_surface_rad = v[VEL_TOOTH_SURFACE] * CONV; vel_tooth_tip_rad = v[VEL_TOOTH_TIP] * CONV; vel_tooth_base_rad = v[VEL_TOOTH_BASE] * CONV; /* Calculate the potential energy stored in the spring by integrating * the spring force over the spring displacement. */ v[SPRING_ENERGY] = (v[SPRING_ENERGY] + ((((v[POS_ERR] - v[POS_TOOTH_TIP]) - old_pos_k) * CONV) * v[TORQUE_K])); /* Calculate the cyclic torque energy by integrating the power over time. */ v[CYCLIC_ENERGY] = (v[CYCLIC_ENERGY] + (delta_t * vel_wg_rad * (-v[TORQUE_CYCLIC]))); /* Calculate the energy lost to friction by integrating the power dissipation * over time. */ v[WG_FRICTION_ENERGY] = (v[WG_FRICTION_ENERGY] + (delta_t * (v[TORQUE_WG_FRICTION] * vel_wg_surface_rad))); v[TOOTH_TIP_FRICTION_ENERGY] = (v[TOOTH_TIP_FRICTION_ENERGY] + (delta_t * (v[TORQUE_TOOTH_TIP_FRICTION] * (vel_tooth_base_rad - vel_tooth_tip_rad)))); v[TOOTH_SURFACE_FRICTION_ENERGY] = (v[TOOTH_SURFACE_FRICTION_ENERGY] + (delta_t * (v[TORQUE_TOOTH_SURFACE_FRICTION] * vel_tooth_surface_rad))); v[TOOTH_SURFACE_COULOMB_ENERGY] = (v[TOOTH_SURFACE_COULOMB_ENERGY] + (delta_t * (v[TORQUE_TOOTH_SURFACE_COULOMB] * vel_tooth_surface_rad))); v[TOOTH_SURFACE_TOTAL_LOSS_ENERGY] = (v[TOOTH_SURFACE_COULOMB_ENERGY] + v[TOOTH_SURFACE_FRICTION_ENERGY]); /* Calculate total potential energy in the system assuming the cyclic torque * can be treated like a spring. */ v[TOTAL_POTENTIAL_ENERGY] = v[SPRING_ENERGY] + v[CYCLIC_ENERGY]; old_pos_k = (v[POS_ERR] - v[POS_TOOTH_TIP]); } /***********************************************************************************/ /* */ /* Functions which calculate model parameters based on input values. */ /* */ /***********************************************************************************/ /* Function calculate_rotary_params() is used to take the input parameter * values and determine relevant parameters for the simple non-ideal rotary * harmonic-drive model. */ void calculate_rotary_params() { double reduction_factor; double frequency; double temp1, temp2; /* Reflect the non-linear stiffness coefficients as measured from the output * side of the transmission to the input side. */ k1_constant = (k1_output_constant / Power(N, 2)); k1_amplitude = (k1_output_amplitude / Power(N, 2)); k1_phase = k1_output_phase; k2_constant = (k2_output_constant / Power(N, 4)); k2_amplitude = (k2_output_amplitude / Power(N, 4)); k2_phase = k2_output_phase; /* Calculate the coulomb friction in the harmonic drive based on the width * of the hysteresis loop in the harmonic-drive stiffness profile. */ b_hd_constant = (hysteresis_width / 2.0); /* Calculate the reduction ratio between the input rotation and the velocity * seen by the friction element mounted between the flexspline and circular * spline ports. */ if(joint == WRIST) reduction_factor = N; else reduction_factor = (N + 1.0); /* Calculate the dynamic friction coefficients by subtracting the input * and output damping components from the total dynamic friction using * conservation of power. These coefficients should be positive. */ b_hd1 = ((Power(reduction_factor, 2)) * (b_total1 - b_in)) - b_out; b_hd2 = ((Power(reduction_factor, 4))) * b_total2; /* Convert the stiction and cyclic friction from input rotation units to * output rotation. Don't upset the signs of these values. */ stiction_torque_hd = reduction_factor * stiction_torque; stiction_vel_hd = (stiction_vel / reduction_factor); cyclic_friction_amp_hd = reduction_factor * cyclic_friction_amplitude; cyclic_friction_phase_hd = cyclic_friction_phase; /* Calculate the natural frequency of the non-rigid vibrational mode of the * two-mass system assuming linear stiffness. */ temp1 = ((k1_constant / CONV) * ((1.0 /inertia_in) + (1.0 / (inertia_out / (Power(reduction_factor, 2.0)))))); temp2 = (1.0 / (Power(2.0, 1.5) * PI)); frequency = (temp2 * Power((2.0 * temp1), 0.5)); printf("\n\n The natural frequency of the non-rigid vibrational mode\n"); printf(" for a two-mass linear system is: %f Hz.\n", frequency); printf(" Therefore, resonance vibration should appear at input rotational\n"); printf(" velocities of: %f input_deg/sec.\n", ((frequency * 360.0) / 4.0)); printf(" %f input_deg/sec.\n", ((frequency * 360.0) / 2.0)); printf(" %f input_deg/sec.\n", ((frequency * 360.0) / 1.0)); /* Print out these new parameters if in debugging mode. */ if(debug) { printf("\n b_hd_constant: %4.20lf", b_hd_constant); printf("\n b_hd1: %4.20lf", b_hd1); printf("\n b_hd2: %4.20lf", b_hd2); printf("\n k1_constant: %4.20lf", k1_constant); printf("\n k2_constant: %4.20lf\n", k2_constant); printf("\n"); } } /* Function calculate_gear_tooth_params() is used to take the input parameter * values and determine: (1) trigonometric and geometric relationships, (2) the * tooth surface friction coefficients, and (3) the ideal spring stiffness * coefficients. */ void calculate_gear_tooth_params() { double stiffness_factor_A; double stiffness_factor_B; double wg_to_tooth_factor; double damping_factor_A; double damping_factor_B; double damping_factor_C; double damping_factor_D; double inertia_in_eff, inertia_out_eff; double temp1, temp2, frequency; /* Initialize the coulomb friction coefficient. */ mu = mu_save; /* Calculate the gear-tooth trig values. */ tan2 = tan(CONV * tooth_angle); sin2 = sin(CONV * tooth_angle); cos2 = cos(CONV * tooth_angle); /* The wg_angle can be found from the gear ratio and the tooth_angle * using the relationship: (1/(N+1)) = tan1 tan2. */ wg_angle = atan(1.0 / (tan2 * (N + 1.0))); tan1 = tan(wg_angle); sin1 = sin(wg_angle); cos1 = cos(wg_angle); /* Now calculate the stiffness of the spring that deforms vertically * from the rotational stiffness provided. */ stiffness_factor_A = (cos2 + (mu * sin2)) / (sin2 - (mu * cos2)); stiffness_factor_B = -(1.0 / (tan1 - (1.0 / tan2))); k1_constant = (k1_output_constant * (stiffness_factor_B / (stiffness_factor_A - tan1))); k1_amplitude = (k1_output_amplitude * (stiffness_factor_B / (stiffness_factor_A - tan1))); k1_phase = k1_output_phase; k2_constant = (k2_output_constant * (Power(stiffness_factor_B, 3) / (stiffness_factor_A - tan1))); k2_amplitude = (k2_output_amplitude * (Power(stiffness_factor_B, 3) / (stiffness_factor_A - tan1))); k2_phase = k2_output_phase; /* From the hysteresis width on the stiffness curve and the values of the * other constant friction components in the harmonic drive, determine * the appropriate constant friction component on the gear tooth surface. */ b_tooth_surface_constant = ((1.0 / ((stiffness_factor_A * cos2) + sin2)) * ((hysteresis_width / 2.0) - ((stiffness_factor_A - tan1) * b_tooth_tip_constant) - (((sin1 * tan1) + cos1) * b_wg_constant))); /* Scale the stiction torque and cyclic friction torque from the input * side (wg) to the gear tooth surface. */ wg_to_tooth_factor = (1.0 / ((tan1 * cos2) + (tan1 * sin2 * tan2))); stiction_torque_tooth = wg_to_tooth_factor * stiction_torque; stiction_vel_tooth = (stiction_vel / wg_to_tooth_factor); cyclic_friction_amp_tooth = wg_to_tooth_factor * cyclic_friction_amplitude; cyclic_friction_phase_tooth = cyclic_friction_phase; /* By applying conservation of power to the damping components in the * model, the linear and cubic coefficients for the gear-tooth-surface * damping can be found by subtracting the known damping components from * the total joint damping and matching coefficients. This has to be * done differently for configuration 1 and configuration 2 since the * dampers outside the harmonic drive depend on different velocities. */ damping_factor_A = ((cos2 / tan2) + sin2); damping_factor_B = (cos1 + (sin1 * tan1)); damping_factor_C = (1.0 / (N + 1.0)); damping_factor_D = (1.0 + (1.0 / N)); if(joint == WRIST) { b_tooth_surface1 = ((b_total1 - b_in - (b_wg1 * (Power((damping_factor_B * damping_factor_D), 2))) - (b_out * (Power((-1.0 / N), 2)))) / (Power((damping_factor_A * (1.0 / N)), 2))); b_tooth_surface2 = ((b_total2 - (b_wg2 * (Power((damping_factor_B * damping_factor_D), 4)))) / (Power((damping_factor_A * (1.0 / N)), 4))); } else { b_tooth_surface1 = ((b_total1 - b_in - (b_wg1 * (Power((damping_factor_B), 2))) - (b_out * (Power((damping_factor_C), 2)))) / (Power((damping_factor_A * damping_factor_C), 2))); b_tooth_surface2 = ((b_total2 - (b_wg2 * (Power((damping_factor_B), 4)))) / (Power((damping_factor_A * damping_factor_C), 4))); } /* Calculate the natural frequency of the non-rigid vibrational mode of the * two-mass system assuming linear stiffness. First calculate the effective * input and output inertias seen by the internal spring, then use the * ideal linear stiffness value, with friction removed, to determine the * resulting natural frequency of the linear system. */ inertia_in_eff = (inertia_in / (tan1 * tan1)); inertia_out_eff = (inertia_out * tan2 * tan2); temp1 = ((k1_constant / CONV) * ((1.0 /inertia_in_eff) + (1.0 / inertia_out_eff))); temp2 = (1.0 / (Power(2.0, 1.5) * PI)); frequency = (temp2 * Power((2.0 * temp1), 0.5)); printf("\n\n The natural frequency of the non-rigid vibrational mode\n"); printf(" for a two-mass linear system is: %f Hz.\n", frequency); printf(" Therefore, resonance vibration should appear at input rotational\n"); printf(" velocities of: %f input_deg/sec.\n", ((frequency * 360.0) / 4.0)); printf(" %f input_deg/sec.\n", ((frequency * 360.0) / 2.0)); printf(" %f input_deg/sec.\n", ((frequency * 360.0) / 1.0)); /* Print out these new parameters if in debugging mode. */ if(debug) { printf("\n wg_angle: %4.20lf deg", (wg_angle / CONV)); printf("\n b_tooth_surface_constant: %4.20lf", b_tooth_surface_constant); printf("\n b_tooth_surface1: %4.20lf", b_tooth_surface1); printf("\n b_tooth_surface2: %4.20lf", b_tooth_surface2); printf("\n k1_constant: %4.20lf", k1_constant); printf("\n k2_constant: %4.20lf\n", k2_constant); printf("\n"); printf("\n tooth surface to input scaling factor: %4.20lf\n", (1.0 / (damping_factor_A * damping_factor_C))); } } /***********************************************************************************/ /* */ /* Functions which model the friction and amplifiers and collect stiffness data. */ /* */ /***********************************************************************************/ /* Function irequlate() takes as arguments the present amp current, the requested * amp current, and the current motor velocity and returns the actual current that * the amps can produce. */ double iregulate(ipresent, irequested, omega) double ipresent; double irequested; double omega; { double voltage_upper_limit; double voltage_lower_limit; double current_upper_limit; double current_lower_limit; double current1, current2; double ireturn; /* Make sure that the current requested is within the amp's operating range. */ if (irequested > IMAX) { fprintf(stderr, "\n WARNING: requested current is above allowed amp range!\n"); printf("\n Current Time: "); irequested = IMAX; } else if (irequested < -IMAX) { fprintf(stderr, "\n WARNING: requested current is below allowed amp range!\n"); printf("\n Current Time: "); irequested = -IMAX; } /* At the present operating current, ipresent, calculate the maximum * allowable voltage that the amps can provide as calculated from * the voltage-current curves for the amps in the Aerotech catalog. * Assume that, if the current is negative, the maximum voltage that the * amp can reach is the saturation voltage, VMAX. Also assume that the * minimum voltage for negative current is described by a curve similar * to the voltage-current for positive current and voltage. Lastly, the * minimum voltage for positive current is equal to the negative voltage * threshold of the amp, -VMAX. */ if(ipresent >= 0) { if(ipresent <= IMAX) voltage_upper_limit = VMAX + VI_SLOPE * ipresent; else voltage_upper_limit = VMAX + VI_SLOPE * IMAX; voltage_lower_limit = -VMAX; } else { if(ipresent >= -IMAX) voltage_lower_limit = -VMAX + VI_SLOPE * ipresent; else voltage_lower_limit = -VMAX + VI_SLOPE * (-IMAX); voltage_upper_limit = VMAX; } /* Now from this maximum or minimum allowed voltage at present current, calculate * the allowable current range that the requested current can achieve by assuming * that the amp voltage is equal to the voltage across the motor (= Kb * velocity * or back EMF) plus the voltage drop due to current flowing through the amp * resistance. */ current1 = (voltage_upper_limit - (motor_kb * omega))/amp_resistance; current2 = (voltage_lower_limit - (motor_kb * omega))/amp_resistance; if(current1 < current2) { current_upper_limit = current2; current_lower_limit = current1; } else { current_upper_limit = current1; current_lower_limit = current2; } /* Now, if the requested current, irequested, is between the upper and lower * current bounds, return the value, otherwise return the upper or lower * limit. */ if((irequested <= current_upper_limit) && (irequested >= current_lower_limit)) ireturn = irequested; else { if((fabs(current_lower_limit - irequested)) < (fabs(current_upper_limit - irequested))) ireturn = current_lower_limit; else ireturn = current_upper_limit; } /* Now add a relative damping factor to the current. Make it behave so that * the current can't change from the old to the new value instantaneously, but, * instead, a part of the new value, ireturn, is averaged with part of the old * value, ipresent, to create the actual new current somewhere between the two * values. This step is necessary to keep the amps current from fluctuating * rapidly between each subsequent step and possibly going unstable. The AMP_ * DAMPING factor is a number between 0 and 1.0 that represents the amount * of damping in the amps current response. Note that since step size can vary * significantly, and that this damping factor is calculated on every step, the * amount of damping will vary with the changing step size. */ ireturn = (AMP_DAMPING * ipresent) + ((1.0 - AMP_DAMPING) * ireturn); /* Give a run-time warning if the current exceeds the amp's allowed range. */ if ((ireturn > IMAX) || (ireturn < -IMAX)) { fprintf(stderr, "\n WARNING: allowable amp current range exceeded!\n"); printf("\n Current Time: "); } return(ireturn); } /* Function calculate_friction() takes 10 arguments: (1) the velocity at the * friction interface, (2,3, and 4) the constant, linear, and cubic velocity- * dependent friction coefficients, (5) the stiction torque, (6) the velocity * where the stiction is deactivated, (7) the reference position for the cyclic * friction, (8 and 9) the amplitude and phase of a sinusoidal friction function, * and (10) the velocity for which the damping curve begins to decrease. * This function returns the resulting value of the friction given these * parameters. Note that the friction_amplitude should not be larger than * b_constant or else the resulting friction can become negative. Additionally, * if the velocity is very small, this friction model is undefined, and a warning * message is given. Finally, if the velocity is too large, the cubic term of the * velocity-dependent damping may decrease and become negative. To prevent this, * the velocity dependent damping is calculated for omega_max at all velocities * greater than omega_max. */ double calculate_friction(omega, b_constant, b1, b2, stiction_torque, stiction_vel, theta, friction_amplitude, friction_phase, omega_max) double omega; double b_constant, b1, b2; double stiction_torque, stiction_vel; double theta; double friction_amplitude, friction_phase; double omega_max; { double friction, damping; /* Make sure that the velocity is not near zero. */ if (((fabs(omega)) < TINY_VEL) && ((stiction_torque + friction_amplitude + b_constant) != 0.0) && (!stiffness_flag) && (!stiffness_flag2)) { fprintf(stderr, "\n ERROR: Function calculate_friction()."); fprintf(stderr, "\n Velocity is near zero and the model is undefined!\n"); return(0.0); } /* If the stiffness data is being collected and the joint is being loaded, * return the velocity-independent friction as if the velocity were positive. * Note: do not use the stiction torque. */ if(stiffness_flag) { friction = (-b_constant + (-friction_amplitude * Sin((theta + friction_phase) * CONV))); return(friction); } /* If stiffness data is being collected and the joint is being unloaded, * return the velocity-independent friction as if the velocity were positive. */ if(stiffness_flag2) { friction = (b_constant + (friction_amplitude * Sin((theta + friction_phase) * CONV))); return(friction); } /* Make sure that the cylic friction amplitude is less than the constant friction * coefficient. */ if (friction_amplitude > b_constant) { fprintf(stderr, "\n ERROR: Function calculate_friction()."); fprintf(stderr, "\n Cyclic friction amplitude too large!\n"); } /* Calculate the static and constant friction as follows: * if the velocity is below the stiction_vel, calculate the friction based * on the linear fit between maximum stiction at zero velocity and b_constant * at stiction_vel. Otherwise the friction is equal to b_constant in the * direction opposing the velocity. Add the cylic friction accordingly. */ if (omega >= stiction_vel) friction = (b_constant + (friction_amplitude * Sin((theta + friction_phase) * CONV))); else if ((omega > 0.0) && (omega < stiction_vel)) friction = (stiction_torque + ((stiction_torque - b_constant)/(-stiction_vel)) * omega); else if (omega <= (-stiction_vel)) friction = ((-b_constant) + (-friction_amplitude * Sin((theta + friction_phase) * CONV))); else friction = ((-stiction_torque) + (((-b_constant) - (-stiction_torque))/(-stiction_vel)) * omega); /* Now calculate the velocity-dependent friction. */ if(fabs(omega) > omega_max) damping = ((b1 * copysign(omega_max, omega)) + (b2 * Power((copysign(omega_max, omega)), 3))); else damping = (b1 * omega) + (b2 * Power(omega, 3)); /* Return the aggregate friction value. */ return(friction + damping); } /* Function collect_stiffness_data() can be used to collect a set of output * position versus output torque data. This function uses the specified * harmonic drive function to calculate the resulting torques when a displacement * is applied to the joint output. The joint is configured with the wave * generator locked to the circular spline. While the joint is being loaded, * data is collected in position increments of pos_increment until the maximum * torque is reached. At that point, the harmonic drive model is switched since * the friction changes direction and the remaining data is collected. A positive * stiffness curve is returned for positive displacement. Assuming directional * symmetry in the drive, this curve can be flipped about the x- and y- axes to * determine the shape of curve for negative displacements. Be sure to load the * drive in the direction that ensures that the gear-tooth-surface normal force * remains positive. */ void collect_stiffness_data() { int i; double torque[STIFFNESS_POINTS]; double displacement[STIFFNESS_POINTS]; double pos_increment; double actual_disp, torque_wg, torque_fs, torque_cs; char unix_command[200]; int points = STIFFNESS_POINTS; static double max_disp[NUM_OF_AXIS] = {0.119, 0.252, 0.343}; /* static double max_disp[NUM_OF_AXIS] = {0.173, 0.37, 0.485}; */ static char *stiffness_filename[NUM_OF_AXIS] = {"shld_stiffness_data.dat", "elb_stiffness_data.dat", "wrist_stiffness_data.dat"}; FILE *outfile, *gnufile; /* Calculate the position increment. */ pos_increment = max_disp[joint] / ((double) (points/2.0)); /* Begin loading the output link of the joint in compression. */ for(i = 1; i <= (points/2); i++) { actual_disp = ((double) (i-1)) * pos_increment; if(joint == WRIST) { (*(hd_model[model]))(0.0, actual_disp, 0.0, 0.0, 0.0, 0.0, &torque_wg, &torque_fs, &torque_cs); torque[i] = torque_fs; } else { actual_disp = -actual_disp; (*(hd_model[model]))(actual_disp, 0.0, actual_disp, 0.0, 0.0, 0.0, &torque_wg, &torque_fs, &torque_cs); torque[i] = torque_cs; } displacement[i] = fabs(actual_disp); if(debug) print_variables(); } /* Adjust the flags to allow proper friction values to be computed. */ stiffness_flag = FALSE; stiffness_flag2 = TRUE; /* Collect data while unloading. Note the appropriate harmonic drive * model for the reverse-friction case is selected. */ for(i = ((points/2) + 1); i <= points; i++) { actual_disp = ((double) (points - i)) * pos_increment; if(joint == WRIST) { (*(hd_model[model]))(0.0, actual_disp, 0.0, 0.0, 0.0, 0.0, &torque_wg, &torque_fs, &torque_cs); torque[i] = torque_fs; } else { actual_disp = -actual_disp; (*(hd_model[model]))(actual_disp, 0.0, actual_disp, 0.0, 0.0, 0.0, &torque_wg, &torque_fs, &torque_cs); torque[i] = torque_cs; } displacement[i] = fabs(actual_disp); if(debug) print_variables(); } /* Now store and print the data. */ create_file(&outfile, outdatapath, stiffness_filename[joint]); export_vectors(outfile, torque, displacement, points, stiffness_filename[joint]); close_file(outfile); if(fast_plot) { create_file(&gnufile, outdatapath, gnuplot_filename); fprintf(gnufile, "cd '%s'\n", outdatapath); fprintf(gnufile, "set xlabel 'Torque (N*m)'\n"); fprintf(gnufile, "set ylabel 'Displacement (deg)'\n"); fprintf(gnufile, "set title 'Stiffness Curve'\n"); fprintf(gnufile, "plot '%s'\n", stiffness_filename[joint]); fprintf(gnufile, "pause -1\n"); close_file(gnufile); sprintf(unix_command, "gnuplot %s%s", outdatapath, gnuplot_filename); printf("\n Executing the unix command: %s\n", unix_command); system(unix_command); } else { sprintf(unix_command, "xgraph %s%s -t Stiffness_Curve\ -y Displacement_deg -x Torque_N*m &", outdatapath, stiffness_filename[joint]); printf("\n Executing the unix command: %s\n", unix_command); system(unix_command); } exit(0); } /***********************************************************************************/ /* */ /* Functions which input and output data. */ /* */ /***********************************************************************************/ /* Function get_arguments() parses the command-line arguments. */ void get_arguments(int argnum, char **args) { int arg_index; for (arg_index = 1; arg_index < argnum; arg_index++) { if (!strcmp("-debug", args[arg_index])) debug = TRUE; else if (!strcmp("-energy", args[arg_index])) { print_energy = TRUE; calc_energy = TRUE; } else if (!strcmp("-stiffness", args[arg_index])) stiffness_flag = TRUE; else if (!strcmp("-ideal", args[arg_index])) ideal_flag = TRUE; else if (!strcmp("-plot", args[arg_index])) plot = TRUE; else if (!strcmp("-h", args[arg_index]) || !strcmp("-?", args[arg_index]) || !strcmp("-help", args[arg_index])) { fprintf(stderr, "\n Program %s: ",args[0]); usage(); exit(0); } else { fprintf(stderr,"\n ERROR while executing %s: arguments are wrong!\n", args[0]); usage(); exit(1); } } } /* Function usage() prints out the proper usage of the command-line options. */ void usage() { fprintf(stderr, "\n Valid command-line options:\n"); fprintf(stderr, " -debug : activates debugging mode\n"); fprintf(stderr, " -energy: prints energy during runtime\n"); fprintf(stderr, " -stiffness: collects and plots solely stiffness data\n"); fprintf(stderr, " -ideal: imposes an ideal haromonic-drive model\n"); fprintf(stderr, " -plot: save plotting data for many input currents\n"); fprintf(stderr, " -h, -?, -help : prints out help message\n"); } /* Function read_input_data() parses all of the values in the input data file. */ void read_input_data(file_pointer) FILE *file_pointer; { char temp_name[50]; double temp_number; while(fscanf(file_pointer, "%s %lg %*s\n", temp_name, &temp_number) != EOF) { if (0 == strcmp("inertia_in", temp_name)) inertia_in = temp_number; else if (0 == strcmp("inertia_out", temp_name)) inertia_out = temp_number; else if (0 == strcmp("k1_output_constant", temp_name)) k1_output_constant = temp_number; else if (0 == strcmp("k1_output_amplitude", temp_name)) k1_output_amplitude = temp_number; else if (0 == strcmp("k1_output_phase", temp_name)) k1_output_phase = temp_number; else if (0 == strcmp("k2_output_constant", temp_name)) k2_output_constant = temp_number; else if (0 == strcmp("k2_output_amplitude", temp_name)) k2_output_amplitude = temp_number; else if (0 == strcmp("k2_output_phase", temp_name)) k2_output_phase = temp_number; else if (0 == strcmp("hysteresis_width", temp_name)) hysteresis_width = temp_number; else if (0 == strcmp("b_total1", temp_name)) b_total1 = temp_number; else if (0 == strcmp("b_total2", temp_name)) b_total2 = temp_number; else if (0 == strcmp("b_in", temp_name)) b_in = temp_number; else if (0 == strcmp("b_wg_constant", temp_name)) b_wg_constant = temp_number; else if (0 == strcmp("b_wg1", temp_name)) b_wg1 = temp_number; else if (0 == strcmp("b_wg2", temp_name)) b_wg2 = temp_number; else if (0 == strcmp("b_tooth_tip_constant", temp_name)) b_tooth_tip_constant = temp_number; else if (0 == strcmp("b_tooth_tip1", temp_name)) b_tooth_tip1 = temp_number; else if (0 == strcmp("b_tooth_tip2", temp_name)) b_tooth_tip2 = temp_number; else if (0 == strcmp("cyclic_friction_amplitude", temp_name)) cyclic_friction_amplitude = temp_number; else if (0 == strcmp("cyclic_friction_phase", temp_name)) cyclic_friction_phase = temp_number; else if (0 == strcmp("stiction_torque", temp_name)) stiction_torque = temp_number; else if (0 == strcmp("stiction_vel", temp_name)) stiction_vel = temp_number; else if (0 == strcmp("b_out", temp_name)) b_out = temp_number; else if (0 == strcmp("mu", temp_name)) mu_save = temp_number; else if (0 == strcmp("tooth_angle", temp_name)) tooth_angle = temp_number; else if (0 == strcmp("N", temp_name)) N = temp_number; else if (0 == strcmp("cyclic_amplitude", temp_name)) cyclic_amplitude = temp_number; else if (0 == strcmp("cyclic_phase", temp_name)) cyclic_phase = temp_number; else if (0 == strcmp("error_amplitude0", temp_name)) error_amplitude0 = temp_number; else if (0 == strcmp("error_phase0", temp_name)) error_phase0 = temp_number; else if (0 == strcmp("error_amplitude1", temp_name)) error_amplitude1 = temp_number; else if (0 == strcmp("error_phase1", temp_name)) error_phase1 = temp_number; else if (0 == strcmp("error_amplitude2", temp_name)) error_amplitude2 = temp_number; else if (0 == strcmp("error_phase2", temp_name)) error_phase2 = temp_number; else if (0 == strcmp("motor_kt", temp_name)) motor_kt = temp_number; else if (0 == strcmp("motor_kb", temp_name)) motor_kb = temp_number; else if (0 == strcmp("amp_resistance", temp_name)) amp_resistance = temp_number; else if (0 == strcmp("irequested", temp_name)) irequested = temp_number; /* Initial conditions, */ else if (0 == strcmp("initial_pos_in", temp_name)) xstart_sav[POS_IN_INDEX] = temp_number; else if (0 == strcmp("initial_pos_out", temp_name)) xstart_sav[POS_OUT_INDEX] = temp_number; else if (0 == strcmp("initial_vel_in", temp_name)) xstart_sav[VEL_IN_INDEX] = temp_number; else if (0 == strcmp("initial_vel_out", temp_name)) xstart_sav[VEL_OUT_INDEX] = temp_number; /* Time conditions */ else if (0 == strcmp("initial_time", temp_name)) initial_time = temp_number; else if (0 == strcmp("final_time", temp_name)) final_time = temp_number; else if (0 == strcmp("step", temp_name)) step = temp_number; /* Fast plotting flag. */ else if (0 == strcmp("fast_plot", temp_name)) fast_plot = ((int) temp_number); /* Desired harmonic-drive model. */ else if (0 == strcmp("model", temp_name)) model = ((int) temp_number); else { fprintf(stderr,"\n Couldn't parse parameter %s\n", temp_name); } } } /* Function read_input_filenames() reads the data from the second input file * as follows. First it checks to see if the first value on each line is 1. * If the value is 1, then it stores the line number in the vector index[] * so that it can be used later to remember which output variables to store. * Additionally, this function also stores the other informaiton from each line * that has a 1 value to be used later for graph titles and output filenames. * As this function steps through each line of the input file, it counts the * number of output variables selected and stores the result in the variable * num_selected_output_var. */ void read_input_filenames(file_pointer) FILE *file_pointer; { int line_number = 0; int temp_flag; char temp_title[50]; char temp_units[50]; while(fscanf(file_pointer, "%d %s %s\n", &temp_flag, temp_title, temp_units) != EOF) { if(temp_flag) { index[num_selected_output_var] = line_number; strcpy(graph_title[num_selected_output_var], temp_title); strcpy(graph_units[num_selected_output_var], temp_units); sprintf(output_datafile[num_selected_output_var], "%s_hd_%s.dat", joint_name[joint], temp_title); num_selected_output_var++; } line_number++; } } /* Function print_input_parameters() prints out the names and values of * the input parameters. */ void print_input_parameters() { printf("\n"); printf(" Model Parameters:\n"); printf(" Input Inertia = %4.10lf\n", inertia_in); printf(" Output Inertia = %4.10lf\n", inertia_out); printf(" k1 constant = %4.10lf\n", k1_output_constant); printf(" k1 amplitude = %4.10lf\n", k1_output_amplitude); printf(" k1 phase = %4.10lf\n", k1_output_phase); printf(" k2 constant = %4.10lf\n", k2_output_constant); printf(" k2 amplitude = %4.10lf\n", k2_output_amplitude); printf(" k2 phase = %4.10lf\n", k2_output_phase); printf(" Hysteresis Width = %4.10lf\n", hysteresis_width); printf(" b_total1 = %4.10lf\n", b_total1); printf(" b_total2 = %4.10lf\n", b_total2); printf(" b_in = %4.10lf\n", b_in); printf(" b_wg_constant = %4.10lf\n", b_wg_constant); printf(" b_wg1 = %4.10lf\n", b_wg1); printf(" b_wg2 = %4.10lf\n", b_wg2); printf(" b_tooth_tip_constant = %4.10lf\n", b_tooth_tip_constant); printf(" b_tooth_tip1 = %4.10lf\n", b_tooth_tip1); printf(" b_tooth_tip2 = %4.10lf\n", b_tooth_tip2); printf(" Cyclic Friction Amp = %4.10lf\n", cyclic_friction_amplitude); printf(" Cyclic Friction Phase = %4.10lf\n", cyclic_friction_phase); printf(" Stiction Torque = %4.10lf\n", stiction_torque); printf(" Stiction Vel = %4.10lf\n", stiction_vel); printf(" b_out = %4.10lf\n", b_out); printf(" mu = %4.10lf\n", mu_save); printf(" tooth_angle = %4.10lf\n", tooth_angle); printf(" Gear Ratio, N = %4.10lf\n", N); printf(" Cyclic Amplitdue = %4.10lf\n", cyclic_amplitude); printf(" Cyclic Phase = %4.10lf\n", cyclic_phase); printf(" Error Amplitude 0 = %4.10lf\n", error_amplitude0); printf(" Error Amplitude 1 = %4.10lf\n", error_amplitude1); printf(" Error Amplitude 2 = %4.10lf\n", error_amplitude2); printf(" Error Phase Shift0 = %4.10lf\n", error_phase0); printf(" Error Phase Shift1 = %4.10lf\n", error_phase1); printf(" Error Phase Shift2 = %4.10lf\n", error_phase2); printf(" Motor Torque Kt = %4.10lf\n", motor_kt); printf(" Motor Back EMF Kb = %4.10lf\n", motor_kb); printf(" Motor Resistance = %4.10lf\n", amp_resistance); printf(" Motor Current = %4.10lf\n", irequested); printf("\n"); printf("Initial Conditions:\n"); printf(" Input Position = %4.10lf\n", xstart_sav[POS_IN_INDEX]); printf(" Output Position = %4.10lf\n", xstart_sav[POS_OUT_INDEX]); printf(" Input Velocity = %4.10lf\n", xstart_sav[VEL_IN_INDEX]); printf(" Output Velocity = %4.10lf\n", xstart_sav[VEL_OUT_INDEX]); printf("\n"); printf(" Initial Time = %4.10lf\n", initial_time); printf(" Final Time = %4.10lf\n", final_time); printf(" Step Size = %4.10lf\n", step); } /* Function print_variables() prints the names and values of some * important torques, velocities, and positions. */ void print_variables() { printf("\n"); printf("\n pos_in: %20.15lf", v[POS_IN]); printf("\n pos_out: %20.15lf", v[POS_OUT]); printf("\n pos_wg: %20.15lf", v[POS_WG]); printf("\n pos_fs: %20.15lf", v[POS_FS]); printf("\n pos_cs: %20.15lf", v[POS_CS]); printf("\n pos_n_wg %20.15lf", v[POS_N_WG]); printf("\n"); printf("\n vel_in: %20.15lf", v[VEL_IN]); printf("\n vel_out: %20.15lf", v[VEL_OUT]); printf("\n vel_wg: %20.15lf", v[VEL_WG]); printf("\n vel_fs: %20.15lf", v[VEL_FS]); printf("\n vel_cs: %20.15lf", v[VEL_CS]); printf("\n"); printf("\n torque_motor: %20.15lf", v[TORQUE_MOTOR]); printf("\n torque_b_in: %20.15lf", v[TORQUE_B_IN]); printf("\n torque_b_out: %20.15lf", v[TORQUE_B_OUT]); printf("\n torque_wg: %20.15lf", v[TORQUE_WG]); printf("\n torque_fs: %20.15lf", v[TORQUE_FS]); printf("\n torque_cs: %20.15lf", v[TORQUE_CS]); printf("\n torque_k: %20.15lf", v[TORQUE_K]); printf("\n torque_cyclic: %20.15lf", v[TORQUE_CYCLIC]); printf("\n tooth_surface_friction: %20.15lf", v[TORQUE_TOOTH_SURFACE_FRICTION]); printf("\n tooth_surf_total_loss: %20.15lf", v[TORQUE_TOOTH_SURFACE_TOTAL_LOSS]); printf("\n tooth_surface_normal: %20.15lf", v[TORQUE_TOOTH_SURFACE_NORMAL]); printf("\n torque_hd_friction: %20.15lf", v[TORQUE_HD_FRICTION]); printf("\n"); printf("\n current time: %20.15lf", current_time); printf("\n"); fflush(stdout); } /* Function export_vectors() takes a file pointer, two pointers * to vectors of 'kount' doubles and writes them to a * file, each line containing an x and y coordinate. If the slow- * plotting option is selected, the filename will not be printed * to the first line of the file. */ void export_vectors(outfile, xvector, yvector, points, filename) FILE *outfile; double *xvector; double *yvector; int points; char filename[]; { int i; printf("\n Exporting vector to file...."); if(!fast_plot) fprintf(outfile, "\" %s\n", filename); for(i = 1; i <= points; i++) fprintf(outfile, "%12.10lf %12.10lf\n", xvector[i], yvector[i]); printf("done.\n"); } /* Function generate_gnuplot_file() is called when the fast_plot option * is selected and creates a command file in the data directory which * is executed in gnuplot by a system command. */ void generate_gnuplot_file(filename) char filename[]; { int i; FILE *gnufile; create_file(&gnufile, outdatapath, filename); fprintf(gnufile, "cd '%s'\n", outdatapath); fprintf(gnufile, "set xlabel 'time (sec)'\n"); for(i = 0; i < num_selected_output_var; i++) { fprintf(gnufile, "set title '%s'\n", graph_title[i]); fprintf(gnufile, "set ylabel '%s'\n", graph_units[i]); fprintf(gnufile, "plot '%s'\n", output_datafile[i]); fprintf(gnufile, "pause -1\n"); } close_file(gnufile); }