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 #include "WalkMC.h"
00031
00032 #include "Shared/WorldState.h"
00033 #include "Events/EventRouter.h"
00034 #include "Events/LocomotionEvent.h"
00035 #include "Wireless/Socket.h"
00036 #include "Shared/Config.h"
00037
00038 #include "Motion/MotionManager.h"
00039
00040 #include <stdlib.h>
00041 #include <stdio.h>
00042 #include <iostream>
00043 #include <fcntl.h>
00044 #include <unistd.h>
00045 #include <fstream>
00046 #include <cmath>
00047
00048
00049
00050
00051
00052 const float WalkMC::MAX_DX = 180;
00053 const float WalkMC::MAX_DY = 140;
00054 const float WalkMC::MAX_DA = 1.8;
00055
00056
00057 const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
00058
00059 unsigned int checksum(const char *data,int num);
00060
00061 WalkMC::WalkMC(const char* pfile)
00062 : MotionCommand(), isPaused(false), wp(), cp(), body_loc(), body_angle(),
00063 acc_style(DEFAULT_ACCEL), step_count(-1), step_threshold(0), last_cycle(0),
00064 pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00065 time(get_time()), TimeStep(FrameTime), slowmo(1.0f),
00066 CycleOffset(0), TimeOffset(0), NewCycleOffset(false),
00067 vel_xya(0,0,0), target_vel_xya(0,0,0), last_target_vel_xya(0,0,0)
00068 {
00069 init(pfile);
00070 }
00071
00072 WalkMC::CalibrationParam::CalibrationParam() {
00073 for(unsigned int r=0; r<3; r++) {
00074 for(unsigned int c=0; c<11; c++)
00075 f_calibration[r][c]=b_calibration[r][c]=0;
00076 f_calibration[r][r]=b_calibration[r][r]=1;
00077 }
00078 for(unsigned int d=0; d<NUM_DIM; d++)
00079 max_accel[d]=0;
00080 max_vel[0]=max_vel[1]=MAX_DX;
00081 max_vel[2]=MAX_DY;
00082 max_vel[3]=MAX_DA;
00083 }
00084
00085
00086 void WalkMC::DoStart() {
00087 MotionCommand::DoStart();
00088 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00089 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00090 postEvent(e);
00091 travelTime=get_time();
00092 }
00093
00094 void WalkMC::DoStop() {
00095 unsigned int t=get_time();
00096 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
00097
00098
00099 postEvent(e);
00100 travelTime=t;
00101 MotionCommand::DoStop();
00102 }
00103
00104 void WalkMC::init(const char* pfile)
00105 {
00106
00107 body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
00108 body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));
00109
00110 for(unsigned int i=0; i<NumLegs; i++)
00111 legw[i].air = false;
00112
00113 if(pfile!=NULL)
00114 loadFile(pfile);
00115 else
00116 loadFile(config->motion.walk.c_str());
00117
00118 double zeros[JointsPerLeg];
00119 for(unsigned int i=0; i<JointsPerLeg; i++)
00120 zeros[i]=0;
00121 resetLegPos();
00122
00123
00124 }
00125
00126
00127 int WalkMC::isDirty()
00128 {
00129 if(isPaused)
00130 return 0;
00131 if(step_count==0)
00132 return 0;
00133 if((target_vel_xya.x == 0) && (target_vel_xya.y == 0) && (target_vel_xya.z == 0)) {
00134
00135 if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0))
00136 return false;
00137 }
00138 #ifdef TGT_HAS_LEGS
00139 return JointsPerLeg*NumLegs;
00140 #elif defined(TGT_HAS_WHEELS)
00141 return NumWheels;
00142 #else
00143 return 0;
00144 #endif
00145 }
00146
00147
00148
00149 unsigned int WalkMC::getBinSize() const {
00150 unsigned int used=0;
00151 used+=creatorSize("WalkMC-2.2");
00152 used+=sizeof(wp);
00153 used+=sizeof(cp);
00154 return used;
00155 }
00156
00157 unsigned int WalkMC::loadBuffer(const char buf[], unsigned int len) {
00158 enum {
00159 VER1,
00160 VER2,
00161 VER3
00162 } version;
00163
00164 unsigned int origlen=len;
00165 if(checkCreatorInc("WalkMC-2.2",buf,len,false)) {
00166 version=VER3;
00167 } else if(checkCreatorInc("WalkMC",buf,len,false)) {
00168 sout->printf("Pre-2.2 release walk parameter file\n");
00169 version=VER2;
00170 } else {
00171
00172 sout->printf("Assuming CMPack format walk parameter file\n");
00173 version=VER1;
00174 }
00175
00176
00177 for(unsigned int i=0; i<4; ++i) {
00178 if(!decodeInc(wp.leg[i].neutral.x,buf,len)) return 0;
00179 if(!decodeInc(wp.leg[i].neutral.y,buf,len)) return 0;
00180 if(!decodeInc(wp.leg[i].neutral.z,buf,len)) return 0;
00181 if(!decodeInc(wp.leg[i].lift_vel.x,buf,len)) return 0;
00182 if(!decodeInc(wp.leg[i].lift_vel.y,buf,len)) return 0;
00183 if(!decodeInc(wp.leg[i].lift_vel.z,buf,len)) return 0;
00184 if(!decodeInc(wp.leg[i].down_vel.x,buf,len)) return 0;
00185 if(!decodeInc(wp.leg[i].down_vel.y,buf,len)) return 0;
00186 if(!decodeInc(wp.leg[i].down_vel.z,buf,len)) return 0;
00187 if(!decodeInc(wp.leg[i].lift_time,buf,len)) return 0;
00188 if(!decodeInc(wp.leg[i].down_time,buf,len)) return 0;
00189 }
00190
00191 if(!decodeInc(wp.body_height,buf,len)) return 0;
00192 if(!decodeInc(wp.body_angle,buf,len)) return 0;
00193 if(!decodeInc(wp.hop,buf,len)) return 0;
00194 if(!decodeInc(wp.sway,buf,len)) return 0;
00195 if(!decodeInc(wp.period,buf,len)) return 0;
00196 if(version<VER3) wp.useDiffDrive=0; else if(!decodeInc(wp.useDiffDrive,buf,len)) return 0;
00197 if(version<VER3) wp.sag=0; else if(!decodeInc(wp.sag,buf,len)) return 0;
00198 if(!decodeInc(wp.reserved,buf,len)) return 0;
00199
00200 if(version<VER2) {
00201 cp=CalibrationParam();
00202 } else {
00203 for(unsigned int i=0; i<3; ++i)
00204 for(unsigned int j=0; j<11; ++j)
00205 if(!decodeInc(cp.f_calibration[i][j],buf,len)) return 0;
00206 for(unsigned int i=0; i<3; ++i)
00207 for(unsigned int j=0; j<11; ++j)
00208 if(!decodeInc(cp.b_calibration[i][j],buf,len)) return 0;
00209 for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00210 if(!decodeInc(cp.max_accel[i],buf,len)) return 0;
00211 for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00212 if(!decodeInc(cp.max_vel[i],buf,len)) return 0;
00213 }
00214 return origlen-len;
00215 }
00216
00217 unsigned int WalkMC::saveBuffer(char buf[], unsigned int len) const {
00218 unsigned int origlen=len;
00219
00220 if(!saveCreatorInc("WalkMC-2.2",buf,len)) return 0;
00221 for(unsigned int i=0; i<4; ++i) {
00222 if(!encodeInc(wp.leg[i].neutral.x,buf,len)) return 0;
00223 if(!encodeInc(wp.leg[i].neutral.y,buf,len)) return 0;
00224 if(!encodeInc(wp.leg[i].neutral.z,buf,len)) return 0;
00225 if(!encodeInc(wp.leg[i].lift_vel.x,buf,len)) return 0;
00226 if(!encodeInc(wp.leg[i].lift_vel.y,buf,len)) return 0;
00227 if(!encodeInc(wp.leg[i].lift_vel.z,buf,len)) return 0;
00228 if(!encodeInc(wp.leg[i].down_vel.x,buf,len)) return 0;
00229 if(!encodeInc(wp.leg[i].down_vel.y,buf,len)) return 0;
00230 if(!encodeInc(wp.leg[i].down_vel.z,buf,len)) return 0;
00231 if(!encodeInc(wp.leg[i].lift_time,buf,len)) return 0;
00232 if(!encodeInc(wp.leg[i].down_time,buf,len)) return 0;
00233 }
00234
00235 if(!encodeInc(wp.body_height,buf,len)) return 0;
00236 if(!encodeInc(wp.body_angle,buf,len)) return 0;
00237 if(!encodeInc(wp.hop,buf,len)) return 0;
00238 if(!encodeInc(wp.sway,buf,len)) return 0;
00239 if(!encodeInc(wp.period,buf,len)) return 0;
00240 if(!encodeInc(wp.useDiffDrive,buf,len)) return 0;
00241 if(!encodeInc(wp.sag,buf,len)) return 0;
00242 if(!encodeInc(wp.reserved,buf,len)) return 0;
00243
00244 for(unsigned int i=0; i<3; ++i)
00245 for(unsigned int j=0; j<11; ++j)
00246 if(!encodeInc(cp.f_calibration[i][j],buf,len)) return 0;
00247 for(unsigned int i=0; i<3; ++i)
00248 for(unsigned int j=0; j<11; ++j)
00249 if(!encodeInc(cp.b_calibration[i][j],buf,len)) return 0;
00250 for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00251 if(!encodeInc(cp.max_accel[i],buf,len)) return 0;
00252 for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00253 if(!encodeInc(cp.max_vel[i],buf,len)) return 0;
00254 return origlen-len;
00255 }
00256
00257 unsigned int WalkMC::loadFile(const char* filename) {
00258 return LoadSave::loadFile(config->motion.makePath(filename).c_str());
00259 }
00260 unsigned int WalkMC::saveFile(const char* filename) const {
00261 return LoadSave::saveFile(config->motion.makePath(filename).c_str());
00262 }
00263
00264 void WalkMC::setTargetVelocity(double dx,double dy,double da,int n)
00265 {
00266 #ifdef BOUND_MOTION
00267 da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00268 double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00269
00270 vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00271 double l = v.length();
00272 if(l > 1) v /= l;
00273
00274 dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00275 dy = cp.strafe_max_vel * v.y * fa;
00276 #endif
00277
00278 target_vel_xya.set(dx,dy,da);
00279
00280
00281
00282
00283 step_count=n;
00284 }
00285
00286 void WalkMC::setTargetDisplacement(double dx, double dy, double da, unsigned int n) {
00287 if(n==0) {
00288 setTargetVelocity(0,0,0,0);
00289 } else {
00290
00291 float midsteps[NumLegs];
00292 for(unsigned int i=0; i<NumLegs; i++)
00293 midsteps[i] = (wp.leg[i].down_time+wp.leg[i].lift_time)/2;
00294
00295 unsigned int steps_per_cycle=4;
00296 for(unsigned int i=0; i<NumLegs-1; i++)
00297 for(unsigned int j=i+1; j<NumLegs; j++)
00298 if(midsteps[i]==midsteps[j]) {
00299 steps_per_cycle--;
00300 break;
00301 }
00302
00303 double scale=steps_per_cycle/(n*wp.period/1000.0);
00304 double vx=dx*scale;
00305 double vy=dy*scale;
00306 double va=da*scale;
00307 setTargetVelocity(vx,vy,va,n);
00308 }
00309 }
00310
00311 int WalkMC::updateOutputs() {
00312
00313 if(!isDirty())
00314 return 0;
00315
00316 unsigned int curT=get_time();
00317 if(last_target_vel_xya!=target_vel_xya) {
00318 last_target_vel_xya=target_vel_xya;
00319 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00320 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00321
00322 postEvent(e);
00323 travelTime=curT;
00324 }
00325
00326 #if defined(TGT_HAS_LEGS)
00327
00328 if(vel_xya.sqlength()==0) {
00329
00330 resetLegPos();
00331 }
00332
00333 float tm = TimeStep * slowmo / 1000;
00334
00335 vector3d cal_target_vel_xya(target_vel_xya);
00336 if(target_vel_xya.x<0)
00337 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::reverse];
00338 else
00339 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00340 cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00341 cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00342 if(cal_target_vel_xya.sqlength()>.0025)
00343 if(target_vel_xya.x<0)
00344 applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00345 else
00346 applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00347
00348 if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00349
00350 vel_xya.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*tm, vel_xya.x+max_accel_xya.x*tm);
00351 vel_xya.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*tm, vel_xya.y+max_accel_xya.y*tm);
00352 vel_xya.z = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00353 } else {
00354
00355 vel_xya=cal_target_vel_xya;
00356 }
00357
00358 BodyPosition delta;
00359 delta.loc.set(vel_xya.x*tm,vel_xya.y*tm,0);
00360 delta.angle.set(0,0,vel_xya.z*tm);
00361
00362 time=(int)(curT*slowmo);
00363
00364
00365
00366 if(NewCycleOffset) {
00367 TimeOffset = CycleOffset - time % wp.period;
00368 NewCycleOffset = false;
00369 }
00370
00371
00372 int AdjustedTime = time + TimeOffset;
00373
00374
00375 if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00376 CycleOffset = AdjustedTime % wp.period;
00377 NewCycleOffset = true;
00378 }
00379
00380
00381 for(unsigned int frame=0; frame<NumFrames; frame++) {
00382 cal_target_vel_xya.rotate_z(-delta.angle.z);
00383
00384
00385 angle_delta += delta.angle.z;
00386 pos_delta += delta.loc.rotate_z(angle_delta);
00387
00388
00389
00390
00391
00392 AdjustedTime = time + TimeOffset + (int)(frame*TimeStep*slowmo);
00393 double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00394
00395 if(step_count>0) {
00396 for(unsigned int i=0; i<NumLegs; i++){
00397 float midstep;
00398 if(step_threshold<=.5f)
00399 midstep=wp.leg[i].lift_time+(wp.leg[i].down_time-wp.leg[i].lift_time)*step_threshold*2;
00400 else
00401 midstep=wp.leg[i].down_time+(1-wp.leg[i].down_time+wp.leg[i].lift_time)*(step_threshold-.5)*2;
00402 midstep-=floorf(midstep);
00403
00404 bool above_last= (last_cycle<midstep);
00405 bool below_cur = (midstep<=cycle);
00406 bool wrap = (cycle<last_cycle);
00407
00408 if(above_last+below_cur+wrap>1) {
00409 step_count--;
00410
00411 if(step_count==0) {
00412 for(unsigned int f=0; f<frame; f++)
00413 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00414 motman->setOutput(this,joint,cmds[joint][f],f);
00415 CycleOffset = AdjustedTime % wp.period;
00416 NewCycleOffset = true;
00417 last_cycle=cycle;
00418 target_vel_xya.set(0,0,0);
00419 last_target_vel_xya=target_vel_xya;
00420 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00421 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00422
00423 postEvent(e);
00424 postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID,getTravelTime()));
00425
00426 return frame==0?0:NumLegs*JointsPerLeg;
00427 }
00428 break;
00429 }
00430
00431 }
00432 }
00433
00434
00435 double sway = wp.sway*cos(2*M_PI*cycle);
00436 double hop = wp.hop*sin(4*M_PI*cycle);
00437 double height = wp.body_height;
00438 BodyPosition nextpos;
00439 nextpos.loc.set(0,-sway,height+hop);
00440 nextpos.angle.set(0,wp.body_angle,0);
00441
00442
00443 for(unsigned int i=0; i<NumLegs; i++){
00444
00445
00446 float lift_time=wp.leg[i].lift_time;
00447 float down_time=wp.leg[i].down_time;
00448 float dir=1;
00449 if(down_time==lift_time)
00450 dir=0;
00451 else if(down_time<lift_time) {
00452 lift_time=wp.leg[i].down_time;
00453 down_time=wp.leg[i].lift_time;
00454 dir=-1;
00455 }
00456
00457 bool air = (cycle >= lift_time) && (cycle < down_time);
00458 double air_f = down_time - lift_time;
00459 double nextlegangles[JointsPerLeg];
00460
00461 if(air != legw[i].air){
00462 if(air){
00463
00464
00465
00466
00467
00468
00469
00470
00471 tm = wp.period/1000.0 * 0.75;
00472 vector3d vfp;
00473 double vfa;
00474 if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00475
00476 vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*tm, vel_xya.x+max_accel_xya.x*tm);
00477 vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*tm, vel_xya.y+max_accel_xya.y*tm);
00478 vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00479 } else {
00480
00481 vfp.x=cal_target_vel_xya.x;
00482 vfp.y=cal_target_vel_xya.y;
00483 vfa=cal_target_vel_xya.z;
00484 }
00485
00486 vfp.z = 0.0;
00487 double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00488 vector3d target;
00489 if(wp.useDiffDrive) {
00490 float rot = vfa/cp.max_vel[CalibrationParam::rotate];
00491 if((i&1)==0)
00492 rot=-rot;
00493 vfp.x += cp.max_vel[CalibrationParam::forward]*rot;
00494 target = (wp.leg[i].neutral + vfp*b*dir);
00495 } else {
00496 target = (wp.leg[i].neutral + vfp*b*dir).rotate_z(vfa*b);
00497 }
00498 target.z+=wp.sag;
00499 liftPos[i]=legpos[i];
00500 downPos[i]=target;
00501 legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00502 }else{
00503 legpos[i].z = wp.leg[i].neutral.z;
00504 }
00505 legw[i].air = air;
00506 }
00507
00508 if(air){
00509 legw[i].cyc = (cycle - lift_time) / air_f;
00510 legpos[i] = legw[i].airpath.eval(legw[i].cyc);
00511
00512
00513
00514
00515
00516
00517
00518
00519 double velfraction_x = fabs(vel_xya.x / MAX_DX);
00520 double velfraction_y = fabs(vel_xya.y / MAX_DY);
00521 double velfraction_a = fabs(vel_xya.z / MAX_DA * 2);
00522
00523
00524 double velfraction =
00525 (velfraction_x > velfraction_y) ?
00526 velfraction_x : velfraction_y;
00527 if(velfraction_a > velfraction)
00528 velfraction = velfraction_a;
00529
00530
00531
00532
00533
00534 velfraction-=1;
00535 velfraction*=velfraction;
00536 velfraction*=velfraction;
00537
00538 legpos[i].z *= 1-velfraction;
00539
00540 }else{
00541 if(dir==0)
00542 legpos[i]=wp.leg[i].neutral;
00543 else {
00544 if(wp.useDiffDrive) {
00545 tm = wp.period/1000.0 * 0.75;
00546 double vfa;
00547 if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00548 vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00549 } else {
00550 vfa = cal_target_vel_xya.z;
00551 }
00552 legpos[i] -= delta.loc*dir;
00553 float rot = vfa/cp.max_vel[CalibrationParam::rotate]*TimeStep * slowmo / 1000;
00554 if((i&1)==0)
00555 rot=-rot;
00556 legpos[i].x -= cp.max_vel[CalibrationParam::forward]*rot;
00557 } else {
00558 legpos[i] = (legpos[i] - delta.loc*dir).rotate_z(-delta.angle.z);
00559 }
00560 }
00561 }
00562
00563 GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00564 for(unsigned int j=0; j<JointsPerLeg; j++)
00565 cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00566
00567 }
00568
00569 last_cycle=cycle;
00570 }
00571
00572 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00573 motman->setOutput(this,joint,cmds[joint]);
00574
00575
00576 return NumLegs*JointsPerLeg;
00577
00578 #elif defined(TGT_HAS_WHEELS)
00579 vel_xya=target_vel_xya;
00580 float wheelcircum = 2*M_PI*config->motion.wheelRadius;
00581 float baserpm = vel_xya.x / wheelcircum;
00582 float turnrpm = (vel_xya.z*config->motion.wheelBaseWidth/2) / wheelcircum;
00583 float left = baserpm - turnrpm;
00584 float right = baserpm + turnrpm;
00585 for(unsigned int frame=0; frame<NumFrames; frame++) {
00586 cmds[LWheelOffset][frame]=left;
00587 cmds[RWheelOffset][frame]=right;
00588 }
00589
00590
00591 for(unsigned int w=WheelOffset; w<WheelOffset+NumWheels; w++)
00592 motman->setOutput(this,w,cmds[w]);
00593
00594
00595 return NumWheels;
00596
00597 #else
00598 # warning target has neither legs nor wheels -- WalkMC is a no-op
00599 return 0;
00600 #endif
00601 }
00602
00603 void WalkMC::resetLegPos() {
00604 BodyPosition nextpos;
00605 nextpos.loc.set(0,0,wp.body_height);
00606 nextpos.angle.set(0,wp.body_angle,0);
00607 #ifdef TGT_HAS_LEGS
00608 for(unsigned int i=0; i<NumLegs; i++) {
00609 double tmp[JointsPerLeg];
00610 for(unsigned int j=0; j<JointsPerLeg; j++)
00611 tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00612 GetLegPosition(legpos[i],tmp,nextpos,i);
00613
00614
00615
00616
00617
00618 }
00619 #endif
00620
00621 }
00622
00623 unsigned int checksum(const char *data,int num)
00624 {
00625 unsigned long c;
00626 int i;
00627
00628 c = 0;
00629 for(i=0; i<num; i++){
00630 c = c ^ (data[i]*13 + 37);
00631 c = (c << 13) | (c >> 19);
00632 }
00633
00634 return(c);
00635 }
00636
00637 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00638 float inmat[11];
00639 inmat[0]=in.x;
00640 inmat[1]=in.y;
00641 inmat[2]=in.z;
00642 inmat[3]=fabs(in.y);
00643 inmat[4]=fabs(in.z);
00644 inmat[5]=exp(-.5f*in.z*in.z)*sin(in.z*2.5f);
00645 inmat[6]=in.x*in.x+in.y*in.y;
00646 inmat[7]=in.x*in.z;
00647 inmat[8]=in.y*in.x;
00648 inmat[9]=in.z*in.y;
00649 inmat[10]=1;
00650 out.set(0,0,0);
00651 for(unsigned int c=0; c<11; c++)
00652 out.x+=mat[0][c]*inmat[c];
00653 for(unsigned int c=0; c<11; c++)
00654 out.y+=mat[1][c]*inmat[c];
00655 for(unsigned int c=0; c<11; c++)
00656 out.z+=mat[2][c]*inmat[c];
00657 }
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672