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
00063
00064
00065
00066
00067
00068 : MotionCommand(), isPaused(false), wp(), cp(), body_loc(), body_angle(),
00069 pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00070 time(get_time()), TimeStep(FrameTime), slowmo(1.0f),
00071 CycleOffset(0), TimeOffset(0), NewCycleOffset(false),
00072 vel_xya(0,0,0), target_vel_xya(0,0,0), last_target_vel_xya(0,0,0)
00073 {
00074 init(pfile);
00075 }
00076
00077 WalkMC::CalibrationParam::CalibrationParam() {
00078 for(unsigned int r=0; r<3; r++) {
00079 for(unsigned int c=0; c<11; c++)
00080 f_calibration[r][c]=b_calibration[r][c]=0;
00081 f_calibration[r][r]=b_calibration[r][r]=1;
00082 }
00083 for(unsigned int d=0; d<NUM_DIM; d++)
00084 max_accel[d]=0;
00085 max_vel[0]=max_vel[1]=MAX_DX;
00086 max_vel[2]=MAX_DY;
00087 max_vel[3]=MAX_DA;
00088 }
00089
00090
00091 void WalkMC::DoStart() {
00092 MotionCommand::DoStart();
00093 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00094 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00095 postEvent(e);
00096 travelTime=get_time();
00097 }
00098
00099 void WalkMC::DoStop() {
00100 unsigned int t=get_time();
00101 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
00102 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00103 postEvent(e);
00104 travelTime=t;
00105 MotionCommand::DoStop();
00106 }
00107
00108 void WalkMC::init(const char* pfile)
00109 {
00110
00111 body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
00112 body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));
00113
00114 for(unsigned int i=0; i<NumLegs; i++)
00115 legw[i].air = false;
00116
00117 if(pfile!=NULL)
00118 LoadFile(pfile);
00119 else
00120 LoadFile(config->motion.walk.c_str());
00121
00122 double zeros[JointsPerLeg];
00123 for(unsigned int i=0; i<JointsPerLeg; i++)
00124 zeros[i]=0;
00125 resetLegPos();
00126
00127
00128 }
00129
00130
00131 int WalkMC::isDirty()
00132 {
00133 if(isPaused)
00134 return false;
00135 if((target_vel_xya.x == 0) && (target_vel_xya.y == 0) && (target_vel_xya.z == 0)) {
00136
00137 return ((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0));
00138 }
00139 return true;
00140 }
00141
00142
00143
00144 unsigned int WalkMC::getBinSize() const {
00145 unsigned int used=0;
00146 used+=creatorSize("WalkMC-2.2");
00147 used+=sizeof(wp);
00148 used+=sizeof(cp);
00149 return used;
00150 }
00151
00152 unsigned int WalkMC::LoadBuffer(const char buf[], unsigned int len) {
00153 unsigned int origlen=len;
00154 unsigned int used=0;
00155 if((used=checkCreator("WalkMC-2.2",buf,len,false))!=0) {
00156 len-=used; buf+=used;
00157 if(len>=sizeof(WalkParam))
00158 memcpy(&wp,buf,used=sizeof(WalkParam));
00159 else
00160 return 0;
00161 len-=used; buf+=used;
00162 if(len>=sizeof(CalibrationParam))
00163 memcpy(&cp,buf,used=sizeof(CalibrationParam));
00164 else
00165 memcpy(&cp,buf,used=len);
00166
00167 len-=used; buf+=used;
00168
00169 return origlen-len;
00170 } else if((used=checkCreator("WalkMC",buf,len,false))!=0) {
00171 sout->printf("Pre-version 2.2 walk parameter file\n");
00172 len-=used; buf+=used;
00173 for(unsigned int i=0; i<4; i++) {
00174 if(len>=sizeof(LegParam))
00175 memcpy(&wp.leg[i],buf,used=sizeof(LegParam));
00176 else
00177 return 0;
00178 len-=used; buf+=used;
00179 }
00180 if(0==(used=decode(wp.body_height,buf,len))) return 0;
00181 len-=used; buf+=used;
00182 if(0==(used=decode(wp.body_angle,buf,len))) return 0;
00183 len-=used; buf+=used;
00184 if(0==(used=decode(wp.hop,buf,len))) return 0;
00185 len-=used; buf+=used;
00186 if(0==(used=decode(wp.sway,buf,len))) return 0;
00187 len-=used; buf+=used;
00188 if(0==(used=decode(wp.period,buf,len))) return 0;
00189 len-=used; buf+=used;
00190 if(0==(used=decode(wp.useDiffDrive,buf,len))) return 0;
00191 len-=used; buf+=used;
00192 wp.sag=0;
00193 if(len>=sizeof(CalibrationParam))
00194 memcpy(&cp,buf,used=sizeof(CalibrationParam));
00195 else
00196 return 0;
00197 len-=used; buf+=used;
00198
00199 return origlen-len;
00200 } else {
00201
00202 sout->printf("Assuming old-format walk parameter file\n");
00203 for(unsigned int i=0; i<4; i++) {
00204 if(len>=sizeof(LegParam))
00205 memcpy(&wp.leg[i],buf,used=sizeof(LegParam));
00206 else
00207 return 0;
00208 len-=used; buf+=used;
00209 }
00210 if(0==(used=decode(wp.body_height,buf,len))) return 0;
00211 len-=used; buf+=used;
00212 if(0==(used=decode(wp.body_angle,buf,len))) return 0;
00213 len-=used; buf+=used;
00214 if(0==(used=decode(wp.hop,buf,len))) return 0;
00215 len-=used; buf+=used;
00216 if(0==(used=decode(wp.sway,buf,len))) return 0;
00217 len-=used; buf+=used;
00218 if(0==(used=decode(wp.period,buf,len))) return 0;
00219 len-=used; buf+=used;
00220 wp.useDiffDrive=0;
00221 wp.sag=0;
00222 cp=CalibrationParam();
00223
00224 return origlen-len;
00225 }
00226 }
00227
00228 unsigned int WalkMC::SaveBuffer(char buf[], unsigned int len) const {
00229 unsigned int origlen=len;
00230 unsigned int used=0;
00231 if(0==(used=saveCreator("WalkMC-2.2",buf,len))) return 0;
00232 len-=used; buf+=used;
00233 if(len>=sizeof(WalkParam))
00234 memcpy(buf,&wp,used=sizeof(WalkParam));
00235 else
00236 return 0;
00237 len-=used; buf+=used;
00238 if(len>=sizeof(CalibrationParam))
00239 memcpy(buf,&cp,used=sizeof(CalibrationParam));
00240 else
00241 return 0;
00242 len-=used; buf+=used;
00243
00244 return origlen-len;
00245 }
00246
00247 unsigned int WalkMC::LoadFile(const char* filename) {
00248 return LoadSave::LoadFile(config->motion.makePath(filename).c_str());
00249 }
00250 unsigned int WalkMC::SaveFile(const char* filename) const {
00251 return LoadSave::SaveFile(config->motion.makePath(filename).c_str());
00252 }
00253
00254 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00255 {
00256 #ifdef BOUND_MOTION
00257 da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00258 double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00259
00260 vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00261 double l = v.length();
00262 if(l > 1) v /= l;
00263
00264 dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00265 dy = cp.strafe_max_vel * v.y * fa;
00266 #endif
00267
00268 target_vel_xya.set(dx,dy,da);
00269
00270
00271 }
00272
00273 int WalkMC::updateOutputs() {
00274
00275 if(!isDirty())
00276 return 0;
00277
00278 if(vel_xya.sqlength()==0) {
00279
00280 resetLegPos();
00281 }
00282
00283 unsigned int curT=get_time();
00284 if(last_target_vel_xya!=target_vel_xya) {
00285 last_target_vel_xya=target_vel_xya;
00286 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00287 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00288 postEvent(e);
00289 travelTime=curT;
00290 }
00291
00292 float tm = TimeStep * slowmo / 1000;
00293
00294 vector3d cal_target_vel_xya(target_vel_xya);
00295 if(target_vel_xya.x<0)
00296 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::reverse];
00297 else
00298 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00299 cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00300 cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00301 if(cal_target_vel_xya.sqlength()>.0025)
00302 if(target_vel_xya.x<0)
00303 applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00304 else
00305 applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00306
00307
00308 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);
00309 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);
00310 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);
00311
00312
00313
00314
00315 BodyPosition delta;
00316 delta.loc.set(vel_xya.x*tm,vel_xya.y*tm,0);
00317 delta.angle.set(0,0,vel_xya.z*tm);
00318
00319 time=(int)(curT*slowmo);
00320
00321
00322
00323 if(NewCycleOffset) {
00324 TimeOffset = CycleOffset - time % wp.period;
00325 NewCycleOffset = false;
00326 }
00327
00328
00329 int AdjustedTime = time + TimeOffset;
00330
00331
00332 if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00333 CycleOffset = AdjustedTime % wp.period;
00334 NewCycleOffset = true;
00335 }
00336
00337
00338 for(unsigned int frame=0; frame<NumFrames; frame++) {
00339 cal_target_vel_xya.rotate_z(-delta.angle.z);
00340
00341
00342 angle_delta += delta.angle.z;
00343 pos_delta += delta.loc.rotate_z(angle_delta);
00344
00345
00346
00347
00348
00349 double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00350 double sway = wp.sway*cos(2*M_PI*cycle);
00351 double hop = wp.hop*sin(4*M_PI*cycle);
00352 double height = wp.body_height;
00353 BodyPosition nextpos;
00354 nextpos.loc.set(0,-sway,height+hop);
00355 nextpos.angle.set(0,wp.body_angle,0);
00356
00357
00358 for(unsigned int i=0; i<NumLegs; i++){
00359
00360
00361 float lift_time=wp.leg[i].lift_time;
00362 float down_time=wp.leg[i].down_time;
00363 float dir=1;
00364 if(down_time==lift_time)
00365 dir=0;
00366 else if(down_time<lift_time) {
00367 lift_time=wp.leg[i].down_time;
00368 down_time=wp.leg[i].lift_time;
00369 dir=-1;
00370 }
00371
00372 bool air = (cycle >= lift_time) && (cycle < down_time);
00373 double air_f = down_time - lift_time;
00374 double nextlegangles[JointsPerLeg];
00375
00376 if(air != legw[i].air){
00377 if(air){
00378
00379
00380
00381
00382
00383
00384
00385
00386 tm = wp.period/1000.0 * 0.75;
00387 vector3d vfp;
00388
00389 vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*tm, vel_xya.x+max_accel_xya.x*tm);
00390 vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*tm, vel_xya.y+max_accel_xya.y*tm);
00391 double vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00392
00393
00394
00395
00396
00397 vfp.z = 0.0;
00398 double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00399 vector3d target;
00400 if(wp.useDiffDrive) {
00401 float rot = vfa/cp.max_vel[CalibrationParam::rotate];
00402 if((i&1)==0)
00403 rot=-rot;
00404 vfp.x += cp.max_vel[CalibrationParam::forward]*rot;
00405 target = (wp.leg[i].neutral + vfp*b*dir);
00406 } else {
00407 target = (wp.leg[i].neutral + vfp*b*dir).rotate_z(vfa*b);
00408 }
00409 target.z+=wp.sag;
00410 liftPos[i]=legpos[i];
00411 downPos[i]=target;
00412 legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00413 }else{
00414 legpos[i].z = wp.leg[i].neutral.z;
00415 }
00416 legw[i].air = air;
00417 }
00418
00419 if(air){
00420 legw[i].cyc = (cycle - lift_time) / air_f;
00421 legpos[i] = legw[i].airpath.eval(legw[i].cyc);
00422
00423
00424
00425
00426
00427
00428
00429
00430 double velfraction_x = fabs(vel_xya.x / MAX_DX);
00431 double velfraction_y = fabs(vel_xya.y / MAX_DY);
00432 double velfraction_a = fabs(vel_xya.z / MAX_DA * 2);
00433
00434
00435 double velfraction =
00436 (velfraction_x > velfraction_y) ?
00437 velfraction_x : velfraction_y;
00438 if(velfraction_a > velfraction)
00439 velfraction = velfraction_a;
00440
00441
00442
00443
00444
00445 velfraction-=1;
00446 velfraction*=velfraction;
00447 velfraction*=velfraction;
00448
00449 legpos[i].z *= 1-velfraction;
00450
00451 }else{
00452 if(dir==0)
00453 legpos[i]=wp.leg[i].neutral;
00454 else {
00455 if(wp.useDiffDrive) {
00456 tm = wp.period/1000.0 * 0.75;
00457 double vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00458 legpos[i] -= delta.loc*dir;
00459 float rot = vfa/cp.max_vel[CalibrationParam::rotate]*TimeStep * slowmo / 1000;
00460 if((i&1)==0)
00461 rot=-rot;
00462 legpos[i].x -= cp.max_vel[CalibrationParam::forward]*rot;
00463 } else {
00464 legpos[i] = (legpos[i] - delta.loc*dir).rotate_z(-delta.angle.z);
00465 }
00466 }
00467 }
00468
00469 GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00470 for(unsigned int j=0; j<JointsPerLeg; j++)
00471 cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00472 }
00473
00474
00475 AdjustedTime = time+TimeOffset+(int)(frame*TimeStep*slowmo);
00476 }
00477
00478 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00479 motman->setOutput(this,joint,cmds[joint]);
00480
00481
00482 return NumLegs*JointsPerLeg;
00483 }
00484
00485 void WalkMC::resetLegPos() {
00486 BodyPosition nextpos;
00487 nextpos.loc.set(0,0,wp.body_height);
00488 nextpos.angle.set(0,wp.body_angle,0);
00489 for(unsigned int i=0; i<NumLegs; i++) {
00490 double tmp[JointsPerLeg];
00491 for(unsigned int j=0; j<JointsPerLeg; j++)
00492 tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00493 GetLegPosition(legpos[i],tmp,nextpos,i);
00494
00495
00496
00497
00498
00499 }
00500
00501 }
00502
00503 unsigned int checksum(const char *data,int num)
00504 {
00505 unsigned long c;
00506 int i;
00507
00508 c = 0;
00509 for(i=0; i<num; i++){
00510 c = c ^ (data[i]*13 + 37);
00511 c = (c << 13) | (c >> 19);
00512 }
00513
00514 return(c);
00515 }
00516
00517 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00518 float inmat[11];
00519 inmat[0]=in.x;
00520 inmat[1]=in.y;
00521 inmat[2]=in.z;
00522 inmat[3]=fabs(in.y);
00523 inmat[4]=fabs(in.z);
00524 inmat[5]=exp(-.5f*in.z*in.z)*sin(in.z*2.5f);
00525 inmat[6]=in.x*in.x+in.y*in.y;
00526 inmat[7]=in.x*in.z;
00527 inmat[8]=in.y*in.x;
00528 inmat[9]=in.z*in.y;
00529 inmat[10]=1;
00530 out.set(0,0,0);
00531 for(unsigned int c=0; c<11; c++)
00532 out.x+=mat[0][c]*inmat[c];
00533 for(unsigned int c=0; c<11; c++)
00534 out.y+=mat[1][c]*inmat[c];
00535 for(unsigned int c=0; c<11; c++)
00536 out.z+=mat[2][c]*inmat[c];
00537 }
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552