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