00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifdef PLATFORM_LINUX
00031 #include <stdlib.h>
00032 #include <stdio.h>
00033 #endif
00034
00035 #include <math.h>
00036
00037 #include "Kinematics.h"
00038
00039
00040
00041
00042 const vector3d f_body_to_shoulder ( 59.50, 59.20, 0.00);
00043 const vector3d f_leg_shoulder_to_knee ( 12.80, 0.50, -64.00);
00044 const vector3d f_leg_knee_to_ball (-18.00, 0.00, -67.23);
00045
00046 const vector3d h_body_to_shoulder (-59.50, 59.20, 0.00);
00047 const vector3d h_leg_shoulder_to_knee (-12.80, 0.50, -64.00);
00048 const vector3d h_leg_knee_to_ball ( 18.00, 0.00, -74.87);
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068 #ifdef PLATFORM_LINUX
00069 void print(double *angles,vector3d pos)
00070 {
00071 printf("A(%7.4f,%7.4f,%7.4f) P(%7.2f,%7.2f,%7.2f)\n",
00072 angles[0],angles[1],angles[2],
00073 pos.x,pos.y,pos.z);
00074 }
00075 #endif
00076
00077
00078 const vector3d f_upper = f_leg_shoulder_to_knee;
00079 const vector3d f_lower = f_leg_knee_to_ball;
00080 const vector3d h_upper = h_leg_shoulder_to_knee;
00081 const vector3d h_lower = h_leg_knee_to_ball;
00082
00083
00084 #ifdef PLATFORM_LINUX
00085 int g_leg;
00086 vector3d g_target;
00087 #endif
00088
00089 static int errors;
00090
00091 void KinClearErrors()
00092 {
00093 errors = 0;
00094 }
00095
00096 int KinGetErrors()
00097 {
00098 return(errors);
00099 }
00100
00101 double GetTrigAngle(double a,double b,double d,double mn,double mx,bool add)
00102
00103 {
00104 double theta;
00105 double t,f,c;
00106 int err;
00107
00108 f = d / sqrt(a*a + b*b);
00109 err = 0;
00110
00111 if(fabs(f) > 1.0){
00112 #ifdef PLATFORM_LINUX
00113
00114
00115
00116
00117 #endif
00118 f = (f > 0.0)? 1.0 : -1.0;
00119 err = 1;
00120 }
00121
00122 t = atan2(a,b);
00123 c = acos(f);
00124
00125 theta = add? (t + c) : (t - c);
00126
00127 if(theta < mn){
00128 #ifdef PLATFORM_LINUX
00129
00130
00131
00132
00133 #endif
00134 errors++;
00135 return(mn);
00136 }else if(theta > mx){
00137 #ifdef PLATFORM_LINUX
00138
00139
00140
00141
00142 #endif
00143 errors++;
00144 return(mx);
00145 }else{
00146 errors += err;
00147 return(theta);
00148 }
00149 }
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 const double rotator_min = RAD(-117.0);
00160 const double rotator_max = RAD( 117.0);
00161 const double shoulder_min = RAD( -11.0);
00162 const double shoulder_max = RAD( 97.0);
00163 const double knee_max = RAD( 147.0);
00164 const double knee_min = RAD( -27.0);
00165
00166 const double rotator_kmin = RAD(-90.0);
00167 const double rotator_kmax = RAD( 90.0);
00168 const double shoulder_kmin = shoulder_min;
00169 const double shoulder_kmax = RAD( 90.0);
00170 const double knee_kmax = knee_max;
00171 const double f_knee_kmin = 0.2616;
00172 const double h_knee_kmin = 0.2316;
00173
00174 const double tail_min = RAD(-22);
00175 const double tail_max = RAD( 22);
00176
00177 const double head_tilt_min = RAD(-88.5);
00178 const double head_tilt_max = RAD( 43.0);
00179 const double head_pan_min = RAD(-89.6);
00180 const double head_pan_max = RAD( 89.6);
00181 const double head_roll_min = RAD(-29.0);
00182 const double head_roll_max = RAD( 29.0);
00183
00184
00185 void GetLegAngles(double *angles,vector3d target,int leg)
00186 {
00187 vector3d targ,pos;
00188 double knee,shoulder,rotator;
00189 double a,b,d,dist;
00190 bool front = leg < 2;
00191
00192 #ifdef PLATFORM_LINUX
00193 g_leg = leg;
00194 g_target = target;
00195
00196 #endif
00197
00198 knee = shoulder = rotator = 0.0;
00199
00200 if(leg % 2) target.y = -target.y;
00201
00202 if(front){
00203 targ = target - f_body_to_shoulder;
00204 dist = targ.sqlength();
00205
00206
00207 a = -2*(f_upper.x*f_lower.z - f_upper.z*f_lower.x);
00208 b = 2*(f_upper.x*f_lower.x + f_upper.z*f_lower.z);
00209 d = (dist - f_upper.sqlength() - f_lower.sqlength() - 2*f_upper.y*f_lower.y);
00210 knee = GetTrigAngle(a,b,d,f_knee_kmin,knee_kmax,true);
00211
00212
00213 pos = f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-knee);
00214 shoulder = GetTrigAngle(-pos.z,pos.y,targ.y,shoulder_kmin,shoulder_kmax,false);
00215
00216
00217
00218 pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z;
00219 rotator = GetTrigAngle(-pos.z,pos.x,targ.x,rotator_min,rotator_max,target.z > 0.0);
00220
00221 #ifdef DEBUG
00222
00223
00224
00225
00226 #endif
00227 }else{
00228 targ = target - h_body_to_shoulder;
00229 dist = targ.sqlength();
00230
00231
00232 a = 2*(h_upper.x*h_lower.z - h_upper.z*h_lower.x);
00233 b = 2*(h_upper.x*h_lower.x + h_upper.z*h_lower.z);
00234 d = (dist - h_upper.sqlength() - h_lower.sqlength() - 2*h_upper.y*h_lower.y);
00235 knee = GetTrigAngle(a,b,d,h_knee_kmin,knee_kmax,true);
00236
00237
00238 pos = h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y(knee);
00239 shoulder = GetTrigAngle(-pos.z,pos.y,targ.y,shoulder_kmin,shoulder_kmax,false);
00240
00241
00242 pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z;
00243 rotator = GetTrigAngle(-pos.z,-pos.x,-targ.x,rotator_min,rotator_max,target.z > 0.0);
00244
00245
00246
00247
00248
00249
00250
00251 #ifdef DEBUG
00252
00253
00254
00255
00256 #endif
00257 }
00258
00259 angles[0] = rotator;
00260 angles[1] = shoulder;
00261 angles[2] = knee;
00262 }
00263
00264 void GetLegAngles(double *angles,vector3d target[4],
00265 double body_angle,double body_height)
00266 {
00267 vector3d p;
00268 int i;
00269
00270 for(i=0; i<4; i++){
00271 p = target[i];
00272 p.z -= body_height;
00273 p = p.rotate_y(-body_angle);
00274 GetLegAngles(angles + 3*i,p,i);
00275 }
00276 }
00277
00278 void GetLegAngles(double *angles,vector3d target[4],BodyPosition &bp)
00279 {
00280 vector3d p;
00281 int i;
00282
00283 for(i=0; i<4; i++){
00284 p = (target[i] - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y);
00285 GetLegAngles(angles + 3*i,p,i);
00286 }
00287 }
00288
00289 void GetLegAngles(double *angles,vector3d target,BodyPosition &bp,int leg)
00290 {
00291 vector3d p;
00292
00293 p = (target - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y);
00294 GetLegAngles(angles,p,leg);
00295 }
00296
00297
00298 void GetLegPosition(vector3d& p, const double* ang, int leg)
00299 {
00300 if(leg < 2){
00301 p = f_body_to_shoulder +
00302 (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-ang[2]))
00303 .rotate_x(ang[1]).rotate_y(-ang[0]);
00304 }else{
00305 p = h_body_to_shoulder +
00306 (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y( ang[2]))
00307 .rotate_x(ang[1]).rotate_y( ang[0]);
00308 }
00309
00310 if(leg % 2)
00311 p.y = -p.y;
00312 }
00313
00314
00315 void GetBodyLocation(vector3d &ball,vector3d &toe,const double *ang,int leg)
00316
00317 {
00318 if(leg < 2){
00319 ball = f_body_to_shoulder +
00320 (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-ang[2]))
00321 .rotate_x(ang[1]).rotate_y(-ang[0]);
00322 }else{
00323 ball = h_body_to_shoulder +
00324 (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y( ang[2]))
00325 .rotate_x(ang[1]).rotate_y( ang[0]);
00326 }
00327
00328
00329 toe = ball;
00330
00331 if(leg % 2){
00332 ball.y = -ball.y;
00333 toe.y = -toe.y;
00334 }
00335 }
00336
00337 void GetHeadAngles(double *angles,vector3d target,
00338 double body_angle,double body_height)
00339 {
00340 double tilt,pan;
00341
00342 vector3d neck;
00343 double height;
00344
00345 #ifdef PLATFORM_APERIOS
00346
00347
00348
00349 #endif
00350
00351
00352 neck = body_to_neck.rotate_y(body_angle);
00353 height = body_height + neck.z;
00354 target.z -= height;
00355
00356 double target_xz_dist;
00357
00358 target_xz_dist=hypot(target.x,target.z);
00359
00360
00361
00362 if(target_xz_dist > neck_to_camera.z && target.length() > neck_to_camera.length()) {
00363 double angle_base_target;
00364 double angle_base_target_camera;
00365
00366 angle_base_target = atan2(target.z,target.x);
00367 angle_base_target_camera = asin(neck_to_camera.z/target_xz_dist);
00368
00369 tilt = angle_base_target - angle_base_target_camera + body_angle;
00370 tilt = bound(tilt,head_tilt_min,head_tilt_max);
00371
00372 double camera_dist_to_target;
00373 camera_dist_to_target = sqrt(target_xz_dist*target_xz_dist -
00374 neck_to_camera.z*neck_to_camera.z);
00375
00376 pan = atan2(target.y,camera_dist_to_target);
00377 pan = bound(pan,head_pan_min,head_pan_max);
00378
00379 #ifdef PLATFORM_APERIOS
00380
00381
00382
00383 #endif
00384 }
00385 else {
00386 tilt = pan = 0.0;
00387 }
00388
00389 angles[0] = tilt;
00390 angles[1] = pan;
00391 angles[2] = 0.0;
00392 }
00393
00394 vector3d
00395 RunForwardModel(double *angles, double body_angle, double body_height, vector3d point) {
00396 double tilt=0.0,pan=0.0,roll=0.0;
00397
00398 tilt = angles[0];
00399 pan = angles[1];
00400 roll = angles[2];
00401
00402 point = point.rotate_x(roll);
00403 point += neck_to_camera;
00404 point = point.rotate_z(pan);
00405 point = point.rotate_y(-tilt+body_angle);
00406
00407 vector3d neck;
00408
00409 neck = body_to_neck;
00410 neck = neck.rotate_y(body_angle);
00411 neck.z += body_height;
00412
00413 point.z += neck.z;
00414
00415 return point;
00416 }
00417
00418
00419
00420
00421
00422
00423 void GetHeadPosition(vector3d &location, vector3d &direction, vector3d &up, vector3d &right,
00424 double *angles, double body_angle, double body_height)
00425 {
00426 location = RunForwardModel(angles, body_angle, body_height, vector3d(0.0,0.0,0.0));
00427
00428 vector3d image_x,image_y,image_z;
00429
00430 image_x = RunForwardModel(angles, body_angle, body_height, vector3d(0.0,-1.0, 0.0));
00431 image_y = RunForwardModel(angles, body_angle, body_height, vector3d(0.0, 0.0, 1.0));
00432 image_z = RunForwardModel(angles, body_angle, body_height, vector3d(1.0, 0.0, 0.0));
00433
00434 direction = image_z - location;
00435 direction = direction.norm();
00436
00437 up = image_y - location;
00438 up = up.norm();
00439
00440 right = image_x - location;
00441 right = right.norm();
00442 }
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456