00001
00002 class FRICheck_test;
00003
00004 class FRICheck
00005 {
00006 public:
00007
00008
00009 static const float margin_;
00010 static const float lim_low_[7];
00011 static const float lim_high_[7];
00012 static const float lim_vel_[7];
00013 static const float lim_acc_[7];
00014
00015
00016 static double j5_angles_left[24];
00017 static double j6_min_left[24];
00018 static double j6_max_left[24];
00019 static const int length_left;
00020
00021 static double j5_angles_right[24];
00022 static double j6_max_right[24];
00023 static double j6_min_right[24];
00024 static const int length_right;
00025
00026 private:
00027 float vel_old_[7];
00028 float pos_[7];
00029
00030 double *j5_angles_;
00031 double *j6_min_;
00032 double *j6_max_;
00033 int j56_length_;
00034
00035 double interpolate(double x, double x1, double y1, double x2, double y2);
00036 double slope(double x1, double y1, double x2, double y2);
00037 double slope_min_j6(int index);
00038 double slope_max_j6(int index);
00039
00040 int find_index(float j5);
00041 double min_j6(float j5, int index);
00042 double max_j6(float j5, int index);
00043 void hand_check(float vel[7], float vel_old[7], float pos[7], float rate);
00044
00045 void safety_check(float vel[7], float vel_old[7], float pos[7], float rate, float safety_factor);
00046
00047 friend class FRICheck_test;
00048 public:
00049 FRICheck();
00050
00051 void setPos(float* pos);
00052 float* pos() {return pos_;}
00053
00055 void adjust(float *pos, float rate, float safety_factor);
00056
00057 enum HandSide {
00058 NONE = 0,
00059 LEFT = 1,
00060 RIGHT = 2
00061 };
00062
00064 bool setHandSide(HandSide side);
00066 void setHandLimits(double *j5_angles, double *j6_min, double *j6_max, int length);
00067 };
00068
00069
00070
