FRICheck_legacy.cc
Go to the documentation of this file.
00001 #include "FRICheck_legacy.hh"
00002 
00003 #include <string.h>
00004 #include <math.h>
00005 
00006 #define DEG *M_PI/180.0
00007 #define RAD /M_PI*180.0
00008 
00009 
00010 
00011 // limits as of KRC/roboter/dlrrc/cfg/rsidef7.dat
00012 const float FRICheck_legacy::lim_low[7] = {-169.5 DEG, -119.5 DEG, -169.5 DEG, -119.5 DEG, -169.5 DEG, -119.5 DEG, -169.5 DEG};
00013 const float FRICheck_legacy::lim_high[7] = { 169.5 DEG,  119.5 DEG,  169.5 DEG,  119.5 DEG,  169.5 DEG,  119.5 DEG,  169.5 DEG};
00014 
00015 const float FRICheck_legacy::lim_vel[7] = {120 DEG, 120 DEG, 160 DEG, 160 DEG, 250 DEG, 220 DEG, 220 DEG};
00016 const float FRICheck_legacy::lim_acc[7] = {1200, 1200, 1600, 1600, 2500, 4400, 4400};
00017 
00018 
00019 void safety_check(float *vel, float *vel_old, float *pos, float rate);
00020 
00021 
00022 
00023 FRICheck_legacy::FRICheck_legacy() :
00024   j5_angles(0), j6_min(0), j6_max(0), length(0)
00025 {
00026   memset(pos_, 0, sizeof(float)*7);
00027   memset(vel_old_, 0, sizeof(float)*7);
00028 }
00029 
00030 
00031 void FRICheck_legacy::setPos(float* pos)
00032 {
00033   memcpy(pos_, pos, sizeof(float)*7);
00034 }
00035 
00036 
00037 
00038 void FRICheck_legacy::adjust(float *pos, float rate)
00039 {
00040   
00041   float vel[16];
00042   
00043   // compute increments
00044   for(int i=0; i < 7; i++)
00045     vel[i] = pos[i] - pos_[i];
00046   
00047   // limit increments
00048   safety_check(vel, vel_old_, pos_, rate);
00049     
00050   // convert position increments back to positions
00051   for(int i=0; i < 7; i++)
00052     pos[i] = pos_[i] + vel[i];
00053 
00054   // remember old positions and velocities
00055   memcpy(pos_, pos, sizeof(float)*7);
00056   memcpy(vel_old_, vel, sizeof(float)*7);
00057 }
00058 
00059 
00060 //This mapping of the limits was done choosing a value for j5, and then exploring the maximum and minimum value of j6.
00061 
00062 #include <stdio.h>
00063 #include <stdlib.h>
00064 
00065 
00066 double j5_angles_left[]={-120.0 DEG, -100 DEG, -80 DEG, -60 DEG, -40 DEG, -35 DEG, -30 DEG, -25 DEG, -20 DEG, -10 DEG, 0 DEG, 10 DEG, 20 DEG, 30 DEG, 40 DEG, 50 DEG, 60 DEG, 70 DEG, 80  DEG,100 DEG, 120 DEG};
00067 double j6_min_left[]={-170 DEG, -170 DEG, -170 DEG, -170 DEG,-170 DEG,-170 DEG,-170 DEG,-170 DEG,-170 DEG,-170 DEG,-170 DEG,-160 DEG,-150 DEG,-140 DEG,-105 DEG,-90 DEG,-85 DEG,-85 DEG,-80 DEG,-75 DEG,-75 DEG};
00068 double j6_max_left[]={-4 DEG, -4 DEG, 0 DEG, 0 DEG, 10 DEG, 30 DEG, 50 DEG, 60 DEG, 65 DEG, 70 DEG, 80 DEG, 80 DEG, 85 DEG, 90 DEG, 90 DEG, 90 DEG, 90 DEG, 90 DEG, 90 DEG, 90 DEG, 90 DEG};
00069 int length_left = 21;
00070 
00071 double j5_angles_right[] = {-120 DEG, -100 DEG, -80 DEG, -60 DEG, -40 DEG, -35 DEG, -30 DEG, -25 DEG, -20 DEG, -10 DEG, 0 DEG, 10 DEG, 20 DEG, 30 DEG, 40 DEG, 50 DEG, 60 DEG, 70 DEG, 80 DEG, 100 DEG, 120 DEG};
00072 double j6_max_right[] = { 84 DEG, 84 DEG, 90 DEG, 90 DEG, 100 DEG, 120 DEG, 140 DEG, 150 DEG, 155 DEG, 160 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG, 170 DEG};
00073 double j6_min_right[] = {-80 DEG, -80 DEG, -80 DEG, -80 DEG, -80 DEG, -80 DEG, -80 DEG, -80 DEG, -80 DEG, -80 DEG, -80 DEG, -70 DEG, -60 DEG, -50 DEG, -15 DEG, 0 DEG, 5 DEG, 5 DEG, 10 DEG, 15 DEG, 15 DEG};
00074 int length_right = 21;
00075 
00076 // general limits (applying to both arms)
00077 double liml[7] = {-169.5 DEG, -119.5 DEG, -169.5 DEG, -119.5 DEG, -169.5 DEG, -119.5 DEG, -169.5 DEG};
00078 double limh[7] = { 169.5 DEG,  119.5 DEG,  169.5 DEG,  119.5 DEG,  169.5 DEG,  119.5 DEG,  169.5 DEG};
00079 
00080 
00081 //interpolate given two data points: x1,y1 and x2,y2
00082 double FRICheck_legacy::interpolate(double x, double x1, double y1, double x2, double y2)
00083 {
00084   double diff_y=y2 - y1;
00085   double diff_x=x2 - x1;
00086   double dx=x - x1;
00087   return(y1 + (diff_y/diff_x)*dx);
00088 }
00089 
00090 int FRICheck_legacy::find_index(float j5){
00091     //printf("Array length: %d\n",length);
00092     
00093     int i=0;
00094 
00095     // check for errors
00096     if ( (j5 < j5_angles[0]) || (j5 > j5_angles[length-1]) ){
00097       printf("%f not in [%f .. %f]: ", j5, j5_angles[0],j5_angles[length-1]);
00098       printf("Error, check the input value.\n");
00099       return(-1);
00100     }
00101 
00102     //The array must increase monotically
00103     //need the index where we interpolate
00104     int index=0;
00105     for (i=0; i<= length-1 ; i++){
00106         if ((j5 > j5_angles[i]) & (j5 <= j5_angles[i+1] )){
00107             index=i;
00108             break;
00109         }
00110     }
00111 
00112     return(index);
00113 }
00114 
00115 double FRICheck_legacy::min_j6(float j5, int index)
00116 {
00117   float min_lim = interpolate(j5,
00118                               j5_angles[index],  j6_min[index],
00119                               j5_angles[index+1],j6_min[index+1]);
00120   return(min_lim);
00121 }
00122 
00123 double FRICheck_legacy::max_j6(float j5, int index)
00124 {
00125   float max_lim = interpolate(j5,
00126                               j5_angles[index],  j6_max[index],
00127                               j5_angles[index+1],j6_max[index+1]);
00128   return(max_lim);
00129 }
00130 
00131 
00132 void FRICheck_legacy::safety_check(float *vel, float *vel_old, float *pos, float rate)
00133 {
00134   if(length == 0 || j5_angles == 0 || j6_min == 0 || j6_max == 0) {
00135     printf("arm side for safety limits checking not set, panicking...\n");
00136     exit(-1);
00137   }
00138 
00139   //The following values vel_max and acc_max are decided by us
00140   double vel_max = (100 DEG) * rate;
00141   double acc_max = (600 DEG) * rate * rate;
00142 
00143   //These are the hard acceleration limits defined in the RSI-Def7 file
00144   double acc_max_hard[7];
00145   acc_max_hard[0]=(1200 DEG) * rate * rate;
00146   acc_max_hard[1]=(1200 DEG) * rate * rate;
00147   acc_max_hard[2]=(1600 DEG) * rate * rate;
00148   acc_max_hard[3]=(1600 DEG) * rate * rate;
00149   acc_max_hard[4]=(2500 DEG) * rate * rate;
00150   acc_max_hard[5]=(4400 DEG) * rate * rate;
00151   acc_max_hard[6]=(4400 DEG) * rate * rate;
00152 
00153   //rate down the accel limits for safety
00154   for (unsigned int i=0; i <7 ; i++) {
00155     acc_max_hard[i]=acc_max_hard[i]*0.9;
00156   }
00157 
00158   // do a special check for the last two joints, they need to use the 2d map of their limits
00159   // check if we are inside the nice area, if not, stop!
00160   double newpos5=pos[5]+vel[5];
00161   int index = find_index(newpos5);
00162   double min_lim6=min_j6(newpos5,index);
00163   double max_lim6=max_j6(newpos5,index);
00164   int n_cycles=5;
00165 
00166   if(index == -1) {
00167     vel[5] = 0.0;
00168     vel[6] = 0.0;
00169   }
00170   
00171   //lower limit, speed negative
00172   if ( ( pos[6]+vel[6] < min_lim6 ) && (pos[6]+n_cycles*vel[6] < min_lim6 ) ) {
00173     vel[6] =  (0.0  > vel_old[6] + acc_max) ? vel_old[6] + acc_max : 0.0;
00174     //limit 5 also (both sides)
00175     vel[5] =  (0.0  > vel_old[5] + acc_max) ? vel_old[5] + acc_max : 0.0;
00176     vel[5] =  (0.0  < vel_old[5] - acc_max) ? vel_old[5] - acc_max : 0.0;
00177   }
00178 
00179   //upper limit, speed positive
00180   if (( pos[6]+vel[6] > max_lim6 ) && ( pos[6]+n_cycles*vel[6] > max_lim6  )  ) {
00181     vel[6] =  (0.0  < vel_old[6] - acc_max) ? vel_old[6] - acc_max : 0.0;
00182     //limit 5 also (both sides)
00183     vel[5] =  (0.0  > vel_old[5] + acc_max) ? vel_old[5] + acc_max : 0.0;
00184     vel[5] =  (0.0  < vel_old[5] - acc_max) ? vel_old[5] - acc_max : 0.0;
00185   }
00186 
00187 
00188   // final check on all the joints, avoiding to exceed their mechanical limits
00189   for(int i=0; i < 7; i++) {
00190 
00191     // handle velocity limits
00192     vel[i] = (vel[i] >  vel_max) ?  vel_max : vel[i];
00193     vel[i] = (vel[i] < -vel_max) ? -vel_max : vel[i];
00194 
00195     // handle normal acceleration limits
00196     vel[i] =  (vel[i]  > vel_old[i] + acc_max) ? vel_old[i] + acc_max : vel[i];
00197     vel[i] =  (vel[i]  < vel_old[i] - acc_max) ? vel_old[i] - acc_max : vel[i];
00198 
00199     //Limit handling in two stages:
00200     //Stage 1:
00201     // calculate the maximum speed now,
00202     // so that we could still stop in time for the limit, i.e.
00203     // | v(t) = a_max*t,  x(t) = Int(v(t)*dt) = .5*a_max*t^2
00204     // | ==> v(x) = sqrt(2*a_max*x)
00205     // | x: distance to limit
00206     // | v: velocity
00207     // | a_max: maximum allowed accelearation
00208 
00209     // BUT: We need to know the velocity that we mustn't exceed
00210     //      at the end of the NEXT timestep:
00211     // | [ x_next = x - v
00212     // | [ v = sqrt(2*a_max*x_next)
00213     // | ==> v(x) = -a + sqrt(a_max^2 + 2*a_max*x)
00214     if (vel[i] > 0.0) { // positive speed, worry about upper limit
00215         double dist_to_limit = limh[i] - pos[i];
00216         if (dist_to_limit > 0.0) { //only change vel[i] if we are before the limit
00217             double vel_max = -acc_max_hard[i] + sqrt(acc_max_hard[i]*(2*dist_to_limit+acc_max_hard[i]));
00218 
00219             vel[i] = ( vel[i] > vel_max ) ? vel_max : vel[i];
00220         }
00221     } else { //negative speed, worry about lower limit
00222         double dist_to_limit = pos[i] - liml[i];
00223         if (dist_to_limit > 0.0) {
00224             double vel_max = -acc_max_hard[i] + sqrt(acc_max_hard[i]*(2*dist_to_limit+acc_max_hard[i]));
00225 
00226             vel[i] = ( vel[i] < -vel_max ) ? -vel_max : vel[i];
00227         }
00228     }
00229 
00230     // Stage 2:
00231     // emergency braking taking advantage of the limit margin (0.5deg)
00232     if( ( (pos[i]) < liml[i] ) && ( vel[i] < 0.0 ) ) {  //negative velocity, hitting lower limit
00233       vel[i] =  (0.0  > vel_old[i] + acc_max_hard[i]) ? vel_old[i] + acc_max_hard[i] : 0.0;
00234     }
00235     if( ( (pos[i]) > limh[i] ) && ( vel[i] > 0.0 ) ){  //positive velocity, hitting upper limit
00236       vel[i] =  (0.0  < vel_old[i] - acc_max_hard[i]) ? vel_old[i] - acc_max_hard[i] : 0.0;
00237     }
00238   }
00239 }
00240 
00241 
00242 int FRICheck_legacy::safety_set_side(int side)
00243 {
00244   switch(side) {
00245   case SAFETY_LEFT:
00246     j5_angles = j5_angles_left;
00247     j6_min = j6_min_left;
00248     j6_max = j6_max_left;
00249     length = length_left;
00250     return 1;
00251   case SAFETY_RIGHT:
00252     j5_angles = j5_angles_right;
00253     j6_min = j6_min_right;
00254     j6_max = j6_max_right;
00255     length = length_right;
00256     return 1;
00257   default:
00258     return 0;
00259   }
00260 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Thu May 23 2013 05:28:18