1 // ************************************************************************** 2 // FILENAME: Aquarobo.h 3 // PURPOSE: THIS FILE REPRESENTS AQUAROBOT'S CONSTANT PARAMETERS 4 // AUTHOR: Kenji Suzuki 5 // DATE: 18 Feb 1993 6 // COMMENT: unit: length=[cm], time=[sec], angle=[rad], 7 // UPDATE: 15 June 1993 8 // ************************************************************************** 9 10 #ifndef AQUAROBO_H 11 #define AQUAROBO_H 12 13 #include <Gait.hpp> 14 15 #ifndef M_PI 16 #define M_PI 3.14159265358979323846 17 #endif 18 19 #define PI M_PI 20 #define DPI (2.0*PI) 21 #define HPI (PI/2.0) 22 #define DR (PI/180.0) 23 #define RD (180.0/PI) 24 #define D2R (PI/180.0) 25 #define R2D (180.0/PI) 26 27 #define ROOT3 1.732050807568877193176604123436845839023590087890625 28 #define EQU_TRIANGLE (3.0/2.0) 29 30 #define LEG 6 31 #define LEG1 0 32 #define LEG2 1 33 #define LEG3 2 34 #define LEG4 3 35 #define LEG5 4 36 #define LEG6 5 37 38 #define CB 0 39 #define HIP 1 40 #define KNEE1 2 41 #define KNEE2 3 42 #define FOOT 4 43 44 #define VJOINT 4 45 #define JOINT 5 46 #define JOINT0 0 47 #define JOINT1 1 48 #define JOINT2 2 49 #define JOINT3 3 50 #define JOINT4 4 51 52 // link length 53 #define LINK0 37.5 /* Center of the Body to HIP joint */ 54 #define LINK1 20.0 /* HIP to KNEE1 joint */ 55 #define LINK2 52.0 /* KNEE1 to KNEE2 joint */ 56 #define LINK3 102.0 /* KNEE2 to FOOT(ANKLE) joint */ 57 #define LINK4 3.0 /* FOOT to SOLE */ 58 59 // square of link length 60 #define LINK02 (37.5* 37.5) 61 #define LINK12 (20.0* 20.0) 62 #define LINK22 (52.0* 52.0) 63 #define LINK32 (102.0*102.0) 64 #define LINK42 ( 3.0* 3.0) 65 66 67 #define MAXMIN 2 68 69 #define TWO_DIM 2 70 #define THREE_DIM 3 71 #define XY 2 72 #define XYZ 3 73 #define EULER 3 74 #define XW 0 75 #define YW 1 76 #define ZW 2 77 #define XB 0 78 #define YB 1 79 #define ZB 2 80 #define X 0 81 #define Y 1 82 #define Z 2 83 84 // Joint angles when the robot is in RESET posture. 85 #define J1ANGLE_RESET ( 0.00*DR) /* HIP */ 86 #define J2ANGLE_RESET ( 67.38*DR) /* KNEE1 */ 87 #define J3ANGLE_RESET (-157.38*DR) /* KNEE2 */ 88 #define J4ANGLE_RESET ( 0.00*DR) /* ANKLE */ 89 90 // Joint angles when the robot is in START posture. 91 #define J1ANGLE_START ( 0.00*DR) /* HIP */ 92 #define J2ANGLE_START ( 8.21*DR) /* KNEE1 */ 93 #define J3ANGLE_START ( -98.21*DR) /* KNEE2 */ 94 #define J4ANGLE_START ( 0.00*DR) /* ANKLE */ 95 96 // Minimum joint angle limit. 97 #define J0ANGLE_MIN ( 0.00*DR) 98 #define J1ANGLE_MIN ( -60.00*DR) /* HIP */ 99 #define J2ANGLE_MIN (-105.62*DR) /* KNEE1 */ 100 #define J3ANGLE_MIN (-157.38*DR) /* KNEE2 */ 101 #define J4ANGLE_MIN ( -22.00*DR) /* ANKLE */ 102 103 // Maxmum joint angle limit. 104 #define J0ANGLE_MAX ( 300.00*DR) 105 #define J1ANGLE_MAX ( 60.00*DR) /* HIP */ 106 #define J2ANGLE_MAX ( 74.38*DR) /* KNEE1 */ 107 #define J3ANGLE_MAX ( 22.62*DR) /* KNEE2 */ 108 #define J4ANGLE_MAX ( 22.00*DR) /* ANKLE */ 109 110 // distance from CB to FOOT when robot is in START posture. 111 #define STANCE 108.97 112 #define STRIDE 120.0 113 #define FOOT_HEIGHT 60.0 //20.0 114 #define BODY_HEIGHT 94.57 115 #define BODY_SPEED 5.0 /* [cm/sec] */ 116 #define R_CWV 25.0 /* radius of cwv [cm] (32cm) */ 117 #define D_CWV (2.0*R_CWV) 118 119 // These are for array indexes 120 // #define ORIENTATION 3 121 #define AZIMUTH 0 122 #define ELEVATION 1 123 #define ROLL 2 124 125 #define YAW 0 126 #define PITCH 1 127 #define ROLL 2 128 129 // #define AZIMUTH 3 130 // #define ELEVATION 4 131 // #define ROLL 5 132 // #define YAW 3 133 // #define PITCH 4 134 // #define ROLL 5 135 136 #define FINE 5.0 137 #define DS_MAX 5.0 138 #define DS_INIT 1.0 139 // #define SCONST (DS_MAX/(90.0*90.0)) 140 141 #define TP0 0 142 #define BODY0 1 143 #define TP1 2 144 #define BODY1 3 145 146 147 #define MAX_TRANSFER_B_FOOT_VEL 25.0 // cm/sec. Previous value: 50.0 148 149 150 #define LONG_TIME 100.0 151 152 153 #if 0 154 /* came from mml.h(Dr.Kanayama's project) 155 #define PI 3.14159265358979323846 156 #define DPI 6.28318530717958647692 // PI * 2 157 #define RAD 57.29577951308232087684 // 180/PI 158 #define HPI 1.57079632679489661923 // PI/2 159 #define PI34 2.35619449019234492885 // 3PI/4 160 #define PI4 0.78539816339744830962 // PI/4 161 #define NEGATIVE_SPEED -1.0 162 */ 163 #endif 164 165 #define aqmin(a,b) ((a)<(b)?(a):(b)) 166 #define aqmax(a,b) ((a)>(b)?(a):(b)) 167 #define sqr(xc) ((xc)*(xc)) 168 169 #if 0 170 /* 171 #define CUBE(x) ((x)*(x)*(x)) 172 #define SQR(xc) ( (xc) * (xc) ) 173 #define sqr(xc) ( (xc) * (xc) ) 174 #define EU_DIS(x1,y1,x2,y2) (sqrt(((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2)))) 175 #define DIST(x1,y1,x2,y2) (fabs((x1)-(x2))+fabs((y1)-(y2))) 176 #define SQRT(xc) (sqrt(1. + (xc)*(xc))) 177 #define PAR_LN(x,c) (0.5*(x)*SQRT((x)/(c))+0.5/(c)*log((x)/(c)+SQRT((x)/(c)))) 178 #define r2d(r) ((r) * RAD) 179 #define d2r(d) ((d) / RAD) 180 */ 181 #endif 182 183 184 #endif 185 186 // EOF 187