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 return ((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0));
00136 }
00137 return JointsPerLeg*NumLegs;
00138 }
00139
00140
00141
00142 unsigned int WalkMC::getBinSize() const {
00143 unsigned int used=0;
00144 used+=creatorSize("WalkMC-2.2");
00145 used+=sizeof(wp);
00146 used+=sizeof(cp);
00147 return used;
00148 }
00149
00150 unsigned int WalkMC::LoadBuffer(const char buf[], unsigned int len) {
00151 unsigned int origlen=len;
00152 unsigned int used=0;
00153 if((used=checkCreator("WalkMC-2.2",buf,len,false))!=0) {
00154 len-=used; buf+=used;
00155 if(len>=sizeof(WalkParam))
00156 memcpy(&wp,buf,used=sizeof(WalkParam));
00157 else
00158 return 0;
00159 len-=used; buf+=used;
00160 if(len>=sizeof(CalibrationParam))
00161 memcpy(&cp,buf,used=sizeof(CalibrationParam));
00162 else
00163 memcpy(&cp,buf,used=len);
00164
00165 len-=used; buf+=used;
00166
00167 return origlen-len;
00168 } else if((used=checkCreator("WalkMC",buf,len,false))!=0) {
00169 sout->printf("Pre-version 2.2 walk parameter file\n");
00170 len-=used; buf+=used;
00171 for(unsigned int i=0; i<4; i++) {
00172 if(len>=sizeof(LegParam))
00173 memcpy(&wp.leg[i],buf,used=sizeof(LegParam));
00174 else
00175 return 0;
00176 len-=used; buf+=used;
00177 }
00178 if(0==(used=decode(wp.body_height,buf,len))) return 0;
00179 len-=used; buf+=used;
00180 if(0==(used=decode(wp.body_angle,buf,len))) return 0;
00181 len-=used; buf+=used;
00182 if(0==(used=decode(wp.hop,buf,len))) return 0;
00183 len-=used; buf+=used;
00184 if(0==(used=decode(wp.sway,buf,len))) return 0;
00185 len-=used; buf+=used;
00186 if(0==(used=decode(wp.period,buf,len))) return 0;
00187 len-=used; buf+=used;
00188 if(0==(used=decode(wp.useDiffDrive,buf,len))) return 0;
00189 len-=used; buf+=used;
00190 wp.sag=0;
00191 if(len>=sizeof(CalibrationParam))
00192 memcpy(&cp,buf,used=sizeof(CalibrationParam));
00193 else
00194 return 0;
00195 len-=used; buf+=used;
00196
00197 return origlen-len;
00198 } else {
00199
00200 sout->printf("Assuming old-format walk parameter file\n");
00201 for(unsigned int i=0; i<4; i++) {
00202 if(len>=sizeof(LegParam))
00203 memcpy(&wp.leg[i],buf,used=sizeof(LegParam));
00204 else
00205 return 0;
00206 len-=used; buf+=used;
00207 }
00208 if(0==(used=decode(wp.body_height,buf,len))) return 0;
00209 len-=used; buf+=used;
00210 if(0==(used=decode(wp.body_angle,buf,len))) return 0;
00211 len-=used; buf+=used;
00212 if(0==(used=decode(wp.hop,buf,len))) return 0;
00213 len-=used; buf+=used;
00214 if(0==(used=decode(wp.sway,buf,len))) return 0;
00215 len-=used; buf+=used;
00216 if(0==(used=decode(wp.period,buf,len))) return 0;
00217 len-=used; buf+=used;
00218 wp.useDiffDrive=0;
00219 wp.sag=0;
00220 cp=CalibrationParam();
00221
00222 return origlen-len;
00223 }
00224 }
00225
00226 unsigned int WalkMC::SaveBuffer(char buf[], unsigned int len) const {
00227 unsigned int origlen=len;
00228 unsigned int used=0;
00229 if(0==(used=saveCreator("WalkMC-2.2",buf,len))) return 0;
00230 len-=used; buf+=used;
00231 if(len>=sizeof(WalkParam))
00232 memcpy(buf,&wp,used=sizeof(WalkParam));
00233 else
00234 return 0;
00235 len-=used; buf+=used;
00236 if(len>=sizeof(CalibrationParam))
00237 memcpy(buf,&cp,used=sizeof(CalibrationParam));
00238 else
00239 return 0;
00240 len-=used; buf+=used;
00241
00242 return origlen-len;
00243 }
00244
00245 unsigned int WalkMC::LoadFile(const char* filename) {
00246 return LoadSave::LoadFile(config->motion.makePath(filename).c_str());
00247 }
00248 unsigned int WalkMC::SaveFile(const char* filename) const {
00249 return LoadSave::SaveFile(config->motion.makePath(filename).c_str());
00250 }
00251
00252 void WalkMC::setTargetVelocity(double dx,double dy,double da,int n)
00253 {
00254 #ifdef BOUND_MOTION
00255 da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00256 double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00257
00258 vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00259 double l = v.length();
00260 if(l > 1) v /= l;
00261
00262 dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00263 dy = cp.strafe_max_vel * v.y * fa;
00264 #endif
00265
00266 target_vel_xya.set(dx,dy,da);
00267
00268
00269
00270 step_count=n;
00271 }
00272
00273 void WalkMC::setTargetDisplacement(double dx, double dy, double da, unsigned int n) {
00274 if(n==0) {
00275 setTargetVelocity(0,0,0,0);
00276 } else {
00277
00278 float midsteps[NumLegs];
00279 for(unsigned int i=0; i<NumLegs; i++)
00280 midsteps[i] = (wp.leg[i].down_time+wp.leg[i].lift_time)/2;
00281
00282 unsigned int steps_per_cycle=4;
00283 for(unsigned int i=0; i<NumLegs-1; i++)
00284 for(unsigned int j=i+1; j<NumLegs; j++)
00285 if(midsteps[i]==midsteps[j]) {
00286 steps_per_cycle--;
00287 break;
00288 }
00289
00290 double scale=steps_per_cycle/(n*wp.period/1000.0);
00291 double vx=dx*scale;
00292 double vy=dy*scale;
00293 double va=da*scale;
00294 setTargetVelocity(vx,vy,va,n);
00295 }
00296 }
00297
00298 int WalkMC::updateOutputs() {
00299
00300 if(!isDirty())
00301 return 0;
00302
00303 if(vel_xya.sqlength()==0) {
00304
00305 resetLegPos();
00306 }
00307
00308 unsigned int curT=get_time();
00309 if(last_target_vel_xya!=target_vel_xya) {
00310 last_target_vel_xya=target_vel_xya;
00311 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00312 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00313
00314 postEvent(e);
00315 travelTime=curT;
00316 }
00317
00318 float tm = TimeStep * slowmo / 1000;
00319
00320 vector3d cal_target_vel_xya(target_vel_xya);
00321 if(target_vel_xya.x<0)
00322 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::reverse];
00323 else
00324 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00325 cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00326 cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00327 if(cal_target_vel_xya.sqlength()>.0025)
00328 if(target_vel_xya.x<0)
00329 applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00330 else
00331 applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00332
00333 if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00334
00335 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);
00336 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);
00337 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);
00338 } else {
00339
00340 vel_xya=cal_target_vel_xya;
00341 }
00342
00343 BodyPosition delta;
00344 delta.loc.set(vel_xya.x*tm,vel_xya.y*tm,0);
00345 delta.angle.set(0,0,vel_xya.z*tm);
00346
00347 time=(int)(curT*slowmo);
00348
00349
00350
00351 if(NewCycleOffset) {
00352 TimeOffset = CycleOffset - time % wp.period;
00353 NewCycleOffset = false;
00354 }
00355
00356
00357 int AdjustedTime = time + TimeOffset;
00358
00359
00360 if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00361 CycleOffset = AdjustedTime % wp.period;
00362 NewCycleOffset = true;
00363 }
00364
00365
00366 for(unsigned int frame=0; frame<NumFrames; frame++) {
00367 cal_target_vel_xya.rotate_z(-delta.angle.z);
00368
00369
00370 angle_delta += delta.angle.z;
00371 pos_delta += delta.loc.rotate_z(angle_delta);
00372
00373
00374
00375
00376
00377 AdjustedTime = time + TimeOffset + (int)(frame*TimeStep*slowmo);
00378 double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00379
00380 if(step_count>0) {
00381 for(unsigned int i=0; i<NumLegs; i++){
00382 float midstep;
00383 if(step_threshold<=.5f)
00384 midstep=wp.leg[i].lift_time+(wp.leg[i].down_time-wp.leg[i].lift_time)*step_threshold*2;
00385 else
00386 midstep=wp.leg[i].down_time+(1-wp.leg[i].down_time+wp.leg[i].lift_time)*(step_threshold-.5)*2;
00387 midstep-=floorf(midstep);
00388
00389 bool above_last= (last_cycle<midstep);
00390 bool below_cur = (midstep<=cycle);
00391 bool wrap = (cycle<last_cycle);
00392
00393 if(above_last+below_cur+wrap>1) {
00394 step_count--;
00395
00396 if(step_count==0) {
00397 for(unsigned int f=0; f<frame; f++)
00398 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00399 motman->setOutput(this,joint,cmds[joint][f],f);
00400 CycleOffset = AdjustedTime % wp.period;
00401 NewCycleOffset = true;
00402 last_cycle=cycle;
00403 target_vel_xya.set(0,0,0);
00404 last_target_vel_xya=target_vel_xya;
00405 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00406 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00407
00408 postEvent(e);
00409
00410 return frame==0?0:NumLegs*JointsPerLeg;
00411 }
00412 break;
00413 }
00414
00415 }
00416 }
00417
00418
00419 double sway = wp.sway*cos(2*M_PI*cycle);
00420 double hop = wp.hop*sin(4*M_PI*cycle);
00421 double height = wp.body_height;
00422 BodyPosition nextpos;
00423 nextpos.loc.set(0,-sway,height+hop);
00424 nextpos.angle.set(0,wp.body_angle,0);
00425
00426
00427 for(unsigned int i=0; i<NumLegs; i++){
00428
00429
00430 float lift_time=wp.leg[i].lift_time;
00431 float down_time=wp.leg[i].down_time;
00432 float dir=1;
00433 if(down_time==lift_time)
00434 dir=0;
00435 else if(down_time<lift_time) {
00436 lift_time=wp.leg[i].down_time;
00437 down_time=wp.leg[i].lift_time;
00438 dir=-1;
00439 }
00440
00441 bool air = (cycle >= lift_time) && (cycle < down_time);
00442 double air_f = down_time - lift_time;
00443 double nextlegangles[JointsPerLeg];
00444
00445 if(air != legw[i].air){
00446 if(air){
00447
00448
00449
00450
00451
00452
00453
00454
00455 tm = wp.period/1000.0 * 0.75;
00456 vector3d vfp;
00457 double vfa;
00458 if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00459
00460 vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*tm, vel_xya.x+max_accel_xya.x*tm);
00461 vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*tm, vel_xya.y+max_accel_xya.y*tm);
00462 vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00463 } else {
00464
00465 vfp.x=cal_target_vel_xya.x;
00466 vfp.y=cal_target_vel_xya.y;
00467 vfa=cal_target_vel_xya.z;
00468 }
00469
00470 vfp.z = 0.0;
00471 double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00472 vector3d target;
00473 if(wp.useDiffDrive) {
00474 float rot = vfa/cp.max_vel[CalibrationParam::rotate];
00475 if((i&1)==0)
00476 rot=-rot;
00477 vfp.x += cp.max_vel[CalibrationParam::forward]*rot;
00478 target = (wp.leg[i].neutral + vfp*b*dir);
00479 } else {
00480 target = (wp.leg[i].neutral + vfp*b*dir).rotate_z(vfa*b);
00481 }
00482 target.z+=wp.sag;
00483 liftPos[i]=legpos[i];
00484 downPos[i]=target;
00485 legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00486 }else{
00487 legpos[i].z = wp.leg[i].neutral.z;
00488 }
00489 legw[i].air = air;
00490 }
00491
00492 if(air){
00493 legw[i].cyc = (cycle - lift_time) / air_f;
00494 legpos[i] = legw[i].airpath.eval(legw[i].cyc);
00495
00496
00497
00498
00499
00500
00501
00502
00503 double velfraction_x = fabs(vel_xya.x / MAX_DX);
00504 double velfraction_y = fabs(vel_xya.y / MAX_DY);
00505 double velfraction_a = fabs(vel_xya.z / MAX_DA * 2);
00506
00507
00508 double velfraction =
00509 (velfraction_x > velfraction_y) ?
00510 velfraction_x : velfraction_y;
00511 if(velfraction_a > velfraction)
00512 velfraction = velfraction_a;
00513
00514
00515
00516
00517
00518 velfraction-=1;
00519 velfraction*=velfraction;
00520 velfraction*=velfraction;
00521
00522 legpos[i].z *= 1-velfraction;
00523
00524 }else{
00525 if(dir==0)
00526 legpos[i]=wp.leg[i].neutral;
00527 else {
00528 if(wp.useDiffDrive) {
00529 tm = wp.period/1000.0 * 0.75;
00530 double vfa;
00531 if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00532 vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00533 } else {
00534 vfa = cal_target_vel_xya.z;
00535 }
00536 legpos[i] -= delta.loc*dir;
00537 float rot = vfa/cp.max_vel[CalibrationParam::rotate]*TimeStep * slowmo / 1000;
00538 if((i&1)==0)
00539 rot=-rot;
00540 legpos[i].x -= cp.max_vel[CalibrationParam::forward]*rot;
00541 } else {
00542 legpos[i] = (legpos[i] - delta.loc*dir).rotate_z(-delta.angle.z);
00543 }
00544 }
00545 }
00546
00547 GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00548 for(unsigned int j=0; j<JointsPerLeg; j++)
00549 cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00550
00551 }
00552
00553 last_cycle=cycle;
00554 }
00555
00556 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00557 motman->setOutput(this,joint,cmds[joint]);
00558
00559
00560 return NumLegs*JointsPerLeg;
00561 }
00562
00563 void WalkMC::resetLegPos() {
00564 BodyPosition nextpos;
00565 nextpos.loc.set(0,0,wp.body_height);
00566 nextpos.angle.set(0,wp.body_angle,0);
00567 for(unsigned int i=0; i<NumLegs; i++) {
00568 double tmp[JointsPerLeg];
00569 for(unsigned int j=0; j<JointsPerLeg; j++)
00570 tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00571 GetLegPosition(legpos[i],tmp,nextpos,i);
00572
00573
00574
00575
00576
00577 }
00578
00579 }
00580
00581 unsigned int checksum(const char *data,int num)
00582 {
00583 unsigned long c;
00584 int i;
00585
00586 c = 0;
00587 for(i=0; i<num; i++){
00588 c = c ^ (data[i]*13 + 37);
00589 c = (c << 13) | (c >> 19);
00590 }
00591
00592 return(c);
00593 }
00594
00595 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00596 float inmat[11];
00597 inmat[0]=in.x;
00598 inmat[1]=in.y;
00599 inmat[2]=in.z;
00600 inmat[3]=fabs(in.y);
00601 inmat[4]=fabs(in.z);
00602 inmat[5]=exp(-.5f*in.z*in.z)*sin(in.z*2.5f);
00603 inmat[6]=in.x*in.x+in.y*in.y;
00604 inmat[7]=in.x*in.z;
00605 inmat[8]=in.y*in.x;
00606 inmat[9]=in.z*in.y;
00607 inmat[10]=1;
00608 out.set(0,0,0);
00609 for(unsigned int c=0; c<11; c++)
00610 out.x+=mat[0][c]*inmat[c];
00611 for(unsigned int c=0; c<11; c++)
00612 out.y+=mat[1][c]*inmat[c];
00613 for(unsigned int c=0; c<11; c++)
00614 out.z+=mat[2][c]*inmat[c];
00615 }
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630