FRICheck.cc
Go to the documentation of this file.
00001 #include "FRICheck.hh"
00002 
00003 #include <float.h>
00004 #include <string.h>
00005 #include <math.h>
00006 
00007 #define DEG *M_PI/180.0
00008 #define RAD /M_PI*180.0
00009 
00010 // limits as of KRC/roboter/dlrrc/cfg/rsidef7.dat
00011 const float FRICheck::margin_ = 0.5 DEG;
00012 const float FRICheck::lim_low_[7] = {-170 DEG, -120 DEG, -170 DEG, -120 DEG, -170 DEG, -120 DEG, -170 DEG};
00013 const float FRICheck::lim_high_[7] = { 170 DEG,  120 DEG,  170 DEG,  120 DEG,  170 DEG,  120 DEG,  170 DEG};
00014 const float FRICheck::lim_vel_[7] = {120 DEG, 120 DEG, 160 DEG, 160 DEG, 250 DEG, 220 DEG, 220 DEG};
00015 const float FRICheck::lim_acc_[7] = {1200 DEG, 1200 DEG, 1600 DEG, 1600 DEG, 2500 DEG, 4400 DEG, 4400 DEG};
00016 
00017 
00018 FRICheck::FRICheck() :
00019   j5_angles_(0), j6_min_(0), j6_max_(0), j56_length_(0)
00020 {
00021   memset(pos_, 0, sizeof(float)*7);
00022   memset(vel_old_, 0, sizeof(float)*7);
00023 }
00024 
00025 
00026 void FRICheck::setPos(float* pos)
00027 {
00028   memcpy(pos_, pos, sizeof(float)*7);
00029 }
00030 
00031 
00032 void FRICheck::adjust(float *pos, float rate, float safety_factor)
00033 {
00034   float vel[7];
00035   
00036   // compute increments
00037   for(int i=0; i < 7; i++)
00038     vel[i] = pos[i] - pos_[i];
00039 
00040   // limit hand movements
00041   hand_check(vel, vel_old_, pos_, rate);
00042   
00043   // limit increments
00044   safety_check(vel, vel_old_, pos_, rate, safety_factor);
00045   
00046   // convert position increments back to positions
00047   for(int i=0; i < 7; i++)
00048     pos[i] = pos_[i] + vel[i];
00049 
00050   // remember old positions and velocities
00051   memcpy(pos_, pos, sizeof(float)*7);
00052   memcpy(vel_old_, vel, sizeof(float)*7);
00053 }
00054 
00055 
00056 //This mapping of the limits was done choosing a value for j5, and then exploring the maximum and minimum value of j6.
00057 
00058 #include <stdio.h>
00059 #include <stdlib.h>
00060 
00061 
00062 double FRICheck::j5_angles_left[]={-130.0 DEG, -120.0 DEG, -100 DEG, -80 DEG, -60 DEG, -50 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, 130 DEG};
00063 double FRICheck::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,-170 DEG,-170 DEG,-170 DEG,-170 DEG,-155 DEG,-122 DEG,-97 DEG,-90 DEG,-86 DEG,-82 DEG,-82 DEG,-82 DEG, -87 DEG};
00064 double FRICheck::j6_max_left[]={-2 DEG, 0 DEG, 0 DEG, 0 DEG, 0 DEG, 4 DEG, 20 DEG, 47 DEG, 65 DEG, 78 DEG, 90 DEG, 110 DEG, 170 DEG, 170 DEG, 170 DEG, 142 DEG, 140 DEG, 140 DEG, 134 DEG, 134 DEG, 138 DEG, 138 DEG, 125 DEG, 112 DEG};
00065 const int FRICheck::length_left = 24;
00066 
00067 double FRICheck::j5_angles_right[] = {-130 DEG,-120 DEG, -100 DEG, -80 DEG, -60 DEG, -50 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, 130 DEG};
00068 double FRICheck::j6_min_right[] = {-115 DEG, -130 DEG, -138 DEG, -140 DEG, -140 DEG, -138 DEG, -132 DEG, -132 DEG, -135 DEG, -150 DEG, -165 DEG, -170 DEG, -170 DEG, -170 DEG, -170 DEG, -70 DEG, -27 DEG, -12 DEG, -5.5 DEG, -3 DEG, -4 DEG, -3 DEG, 0 DEG};
00069 double FRICheck::j6_max_right[] = { 90 DEG, 95 DEG, 84 DEG, 90 DEG, 96 DEG, 102 DEG, 123 DEG, 145 DEG, 156 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, 170 DEG, 170 DEG, 170 DEG};
00070 const int FRICheck::length_right = 24;
00071 
00072 
00073 //interpolate given two data points: x1,y1 and x2,y2
00074 double FRICheck::interpolate(double x, double x1, double y1, double x2, double y2)
00075 {
00076   double diff_y = y2 - y1;
00077   double diff_x = x2 - x1;
00078   double dx = x - x1;
00079   return(y1 + (diff_y/diff_x)*dx);
00080 }
00081 
00082 double FRICheck::slope(double x1, double y1, double x2, double y2)
00083 {
00084   return (y2 - y1)/(x2 - x1);
00085 }
00086 
00087 int FRICheck::find_index(float j5){
00088   int i=0;
00089 
00090   // check for errors
00091   if ( (j5 < j5_angles_[0]) || (j5 > j5_angles_[j56_length_-1]) ){
00092     printf("%f not in [%f .. %f]: ", j5, j5_angles_[0],j5_angles_[j56_length_-1]);
00093     printf("Error, check the input value.\n");
00094     return(-1);
00095   }
00096 
00097   //The array must increase monotically
00098   //need the index where we interpolate
00099   int index=0;
00100   for (i=0; i<= j56_length_-1 ; i++){
00101     if ((j5 > j5_angles_[i]) & (j5 <= j5_angles_[i+1] )){
00102       index=i;
00103       break;
00104     }
00105   }
00106   
00107   return(index);
00108 }
00109 
00110 double FRICheck::slope_min_j6(int index)
00111 {
00112   return slope(j5_angles_[index], j6_min_[index], j5_angles_[index+1], j6_min_[index+1]);
00113 }
00114 
00115 double FRICheck::slope_max_j6(int index)
00116 {
00117   return slope(j5_angles_[index], j6_max_[index], j5_angles_[index+1], j6_max_[index+1]);
00118 }
00119 
00120 
00121 double FRICheck::min_j6(float j5, int index)
00122 {
00123   float min_lim = interpolate(j5,
00124                               j5_angles_[index],  j6_min_[index],
00125                               j5_angles_[index+1],j6_min_[index+1]);
00126   return(min_lim);
00127 }
00128 
00129 double FRICheck::max_j6(float j5, int index)
00130 {
00131   float max_lim = interpolate(j5,
00132                               j5_angles_[index],  j6_max_[index],
00133                               j5_angles_[index+1],j6_max_[index+1]);
00134   return(max_lim);
00135 }
00136 
00137 
00138 void FRICheck::hand_check(float vel[7], float vel_old[7], float pos[7], float rate)
00139 {
00140   // arm side for hand limit check not set, skipping it...
00141   if(j56_length_ == 0 || j5_angles_ == 0 || j6_min_ == 0 || j6_max_ == 0)
00142     return;
00143 
00144   // do a special check for the last two joints, they need to use the 2d map of their limits
00145   // check if we are inside the nice area, if not, stop!
00146   double newpos5=pos[5]+vel[5];
00147   int index = find_index(newpos5);
00148   if(index == -1) {
00149     vel[5] = 0.0;
00150     vel[6] = 0.0;
00151   } 
00152   double min_lim6 = min_j6(newpos5,index);
00153   double max_lim6 = max_j6(newpos5,index);
00154 
00155   //lower limit, speed negative
00156   if(pos[6]+vel[6] > max_lim6) {
00157     // normal direction is: v_n = [slope -1]'
00158     // projection is: v_n * (v_n*v' / (v'*v))
00159     double slope = slope_max_j6(index);
00160     double projected_vel = (slope*vel[5] - vel[6]) / (1 + slope*slope);
00161 
00162     if(projected_vel > 0) {
00163       vel[5] = slope*projected_vel;
00164       vel[6] =      -projected_vel;
00165     } else {
00166       vel[5] = 0.0;
00167       vel[6] = 0.0;
00168     }
00169   }
00170 
00171   //lower limit, speed negative
00172   if(pos[6]+vel[6] < min_lim6) {
00173     // normal direction is: v_n = [slope -1]'
00174     // projection is: v_n * (v_n*v' / (v'*v))
00175     double slope = slope_min_j6(index);
00176     double projected_vel = (-slope*vel[5] + vel[6]) / (1 + slope*slope);
00177 
00178     if(projected_vel > 0) {
00179       vel[5] = -slope*projected_vel;
00180       vel[6] =        projected_vel;
00181     } else {
00182       vel[5] = 0.0;
00183       vel[6] = 0.0;
00184     }
00185   }
00186 }
00187 
00188 #define EPS FLT_EPSILON
00189 
00190 void FRICheck::safety_check(float vel[7], float vel_old[7], float pos[7], float rate, float safety_factor)
00191 {
00192   // final check on all the joints, avoiding to exceed their mechanical limits
00193   for(int i=0; i < 7; i++) {
00194     double vel_max = lim_vel_[i] * rate;
00195     double acc_max = lim_acc_[i]*rate*rate*safety_factor;
00196 
00197     // handle velocity limits
00198     vel[i] = (vel[i] >  vel_max) ?  vel_max : vel[i];
00199     vel[i] = (vel[i] < -vel_max) ? -vel_max : vel[i];
00200 
00201     // handle acceleration limits
00202     vel[i] =  (vel[i]  > vel_old[i] + acc_max) ? vel_old[i] + acc_max : vel[i];
00203     vel[i] =  (vel[i]  < vel_old[i] - acc_max) ? vel_old[i] - acc_max : vel[i];
00204 
00205     //Limit handling in two stages:
00206     //Stage 1:
00207     // calculate the maximum speed now,
00208     // so that we could still stop in time for the limit, i.e.
00209     // | v(t) = a_max*t,  x(t) = Int(v(t)*dt) = .5*a_max*t^2
00210     // | ==> v(x) = sqrt(2*a_max*x)
00211     // | x: distance to limit
00212     // | v: velocity
00213     // | a_max: maximum allowed accelearation
00214 
00215     // BUT: We need to know the velocity that we mustn't exceed
00216     //      at the end of the NEXT timestep:
00217     // | [ x_next = x - .5*v
00218     // | [ v = sqrt(2*a_max*x_next)
00219     // | ==> v(x) = -0.5*a + sqrt(0.5*a_max^2 + 2*a_max*x)
00220 
00221     if (vel[i] > 0.0) { // positive speed, worry about upper limit
00222       double dist_to_limit = (lim_high_[i] - margin_) - pos[i];
00223       if (dist_to_limit > 0.0) { //only change vel[i] if we are before the limit
00224         double vel_max = -0.5*acc_max + sqrt(acc_max*(2*dist_to_limit + 0.5*acc_max)) - EPS;
00225         
00226         vel[i] = ( vel[i] > vel_max ) ? vel_max : vel[i];
00227       }
00228     } else { //negative speed, worry about lower limit
00229       double dist_to_limit = pos[i] - (lim_low_[i] + margin_);
00230       if (dist_to_limit > 0.0) {
00231         double vel_max = -0.5*acc_max + sqrt(acc_max*(2*dist_to_limit + 0.5*acc_max)) - EPS;
00232         
00233         vel[i] = ( vel[i] < -vel_max ) ? -vel_max : vel[i];
00234       }
00235     }
00236 
00237     // Stage 2:
00238     // emergency braking taking advantage of the limit margin (0.5deg)
00239     if( (pos[i] < lim_low_[i]  + margin_) && ( vel[i] < 0.0 ) ) {  //negative velocity, hitting lower limit
00240       vel[i] =  (0.0  > vel_old[i] + acc_max) ? vel_old[i] + acc_max : 0.0;
00241     }
00242     if( (pos[i] > lim_high_[i] - margin_) && ( vel[i] > 0.0 ) ){  //positive velocity, hitting upper limit
00243       vel[i] =  (0.0  < vel_old[i] - acc_max) ? vel_old[i] - acc_max : 0.0;
00244     }
00245   }
00246 }
00247 
00248 
00249 void FRICheck::setHandLimits(double *j5_angles, double *j6_min, double *j6_max, int length)
00250 {
00251   j5_angles_ = j5_angles;
00252   j6_min_ = j6_min;
00253   j6_max_ = j6_max;
00254   j56_length_ = length;
00255 }
00256 
00257 bool FRICheck::setHandSide(FRICheck::HandSide side)
00258 {
00259   switch(side) {
00260   case LEFT:
00261     setHandLimits(j5_angles_left, j6_min_left, j6_max_left, length_left);
00262     return true;
00263   case RIGHT:
00264     setHandLimits(j5_angles_right, j6_min_right, j6_max_right, length_right);
00265     return true;
00266   default:
00267     setHandLimits(0, 0, 0, 0);
00268     return false;
00269   }
00270 }


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Mon Oct 6 2014 09:27:39