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
00037 #include "Motion/MotionManager.h"
00038
00039 #include <stdlib.h>
00040 #include <stdio.h>
00041 #include <iostream>
00042 #include <fcntl.h>
00043 #include <unistd.h>
00044 #include <fstream>
00045 #include <cmath>
00046
00047
00048
00049
00050
00051 const float WalkMC::MAX_DX = 180;
00052 const float WalkMC::MAX_DY = 140;
00053 const float WalkMC::MAX_DA = 1.8;
00054
00055
00056 const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
00057
00058 unsigned int checksum(const char *data,int num);
00059
00060 WalkMC::WalkMC(const char* pfile)
00061
00062
00063
00064
00065
00066 : MotionCommand(), isPaused(false), wp(), cp(), body_loc(), body_angle(),
00067 pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00068 time(get_time()), TimeStep(FrameTime), slowmo(1.0f),
00069 CycleOffset(0), TimeOffset(0), NewCycleOffset(false),
00070 vel_xya(0,0,0), target_vel_xya(0,0,0), last_target_vel_xya(0,0,0)
00071 {
00072 init(pfile);
00073 }
00074
00075 WalkMC::CalibrationParam::CalibrationParam() {
00076 for(unsigned int r=0; r<3; r++) {
00077 for(unsigned int c=0; c<11; c++)
00078 f_calibration[r][c]=b_calibration[r][c]=0;
00079 f_calibration[r][r]=b_calibration[r][r]=1;
00080 }
00081 for(unsigned int d=0; d<NUM_DIM; d++)
00082 max_accel[d]=0;
00083 max_vel[0]=max_vel[1]=MAX_DX;
00084 max_vel[2]=MAX_DY;
00085 max_vel[3]=MAX_DA;
00086 }
00087
00088
00089 void WalkMC::DoStart() {
00090 MotionCommand::DoStart();
00091 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00092 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00093 postEvent(e);
00094 travelTime=get_time();
00095 }
00096
00097 void WalkMC::DoStop() {
00098 unsigned int t=get_time();
00099 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
00100 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00101 postEvent(e);
00102 travelTime=t;
00103 MotionCommand::DoStop();
00104 }
00105
00106 void WalkMC::init(const char* pfile)
00107 {
00108
00109 body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
00110 body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));
00111
00112 for(unsigned int i=0; i<NumLegs; i++)
00113 legw[i].air = false;
00114
00115 if(pfile!=NULL)
00116 LoadFile(pfile);
00117 else
00118 LoadFile("/ms/data/motion/walk.prm");
00119
00120 double zeros[JointsPerLeg];
00121 for(unsigned int i=0; i<JointsPerLeg; i++)
00122 zeros[i]=0;
00123 resetLegPos();
00124
00125
00126 }
00127
00128
00129 int WalkMC::isDirty()
00130 {
00131 if(isPaused)
00132 return false;
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 true;
00138 }
00139
00140
00141
00142 unsigned int WalkMC::getBinSize() const {
00143 unsigned int used=0;
00144 used+=creatorSize("WalkMC");
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(0==(used=checkCreator("WalkMC",buf,len,false))) {
00154
00155 sout->printf("Assuming old-format walk parameter file\n");
00156 if(len>=sizeof(WalkParam))
00157 memcpy(&wp,buf,used=sizeof(WalkParam));
00158 else
00159 return 0;
00160 len-=used; buf+=used;
00161 cp=CalibrationParam();
00162
00163 return origlen-len;
00164 }
00165 len-=used; buf+=used;
00166 if(len>=sizeof(WalkParam))
00167 memcpy(&wp,buf,used=sizeof(WalkParam));
00168 else
00169 return 0;
00170 len-=used; buf+=used;
00171 if(len>=sizeof(CalibrationParam))
00172 memcpy(&cp,buf,used=sizeof(CalibrationParam));
00173 else
00174 memcpy(&cp,buf,used=len);
00175
00176 len-=used; buf+=used;
00177
00178 return origlen-len;
00179 }
00180
00181 unsigned int WalkMC::SaveBuffer(char buf[], unsigned int len) const {
00182 unsigned int origlen=len;
00183 unsigned int used=0;
00184 if(0==(used=saveCreator("WalkMC",buf,len))) return 0;
00185 len-=used; buf+=used;
00186 if(len>=sizeof(WalkParam))
00187 memcpy(buf,&wp,used=sizeof(WalkParam));
00188 else
00189 return 0;
00190 len-=used; buf+=used;
00191 if(len>=sizeof(CalibrationParam))
00192 memcpy(buf,&cp,used=sizeof(CalibrationParam));
00193 else
00194 return 0;
00195 len-=used; buf+=used;
00196
00197 return origlen-len;
00198 }
00199
00200 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00201 {
00202 #ifdef BOUND_MOTION
00203 da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00204 double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00205
00206 vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00207 double l = v.length();
00208 if(l > 1) v /= l;
00209
00210 dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00211 dy = cp.strafe_max_vel * v.y * fa;
00212 #endif
00213
00214 target_vel_xya.set(dx,dy,da);
00215
00216
00217 }
00218
00219 int WalkMC::updateOutputs() {
00220
00221 if(!isDirty())
00222 return 0;
00223
00224 if(vel_xya.sqlength()==0) {
00225
00226 resetLegPos();
00227 }
00228
00229 unsigned int curT=get_time();
00230 if(last_target_vel_xya!=target_vel_xya) {
00231 last_target_vel_xya=target_vel_xya;
00232 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00233 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00234 postEvent(e);
00235 travelTime=curT;
00236 }
00237
00238 double t = TimeStep * slowmo / 1000;
00239
00240 vector3d cal_target_vel_xya(target_vel_xya);
00241 if(target_vel_xya.x<0)
00242 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::reverse];
00243 else
00244 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00245 cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00246 cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00247 if(cal_target_vel_xya.sqlength()>.0025)
00248 if(target_vel_xya.x<0)
00249 applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00250 else
00251 applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00252
00253
00254 vel_xya.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00255 vel_xya.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00256 vel_xya.z = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00257
00258
00259
00260
00261 BodyPosition delta;
00262 delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);
00263 delta.angle.set(0,0,vel_xya.z*t);
00264
00265 time=(int)(curT*slowmo);
00266
00267
00268
00269 if(NewCycleOffset) {
00270 TimeOffset = CycleOffset - time % wp.period;
00271 NewCycleOffset = false;
00272 }
00273
00274
00275 int AdjustedTime = time + TimeOffset;
00276
00277
00278 if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00279 CycleOffset = AdjustedTime % wp.period;
00280 NewCycleOffset = true;
00281 }
00282
00283
00284 for(unsigned int frame=0; frame<NumFrames; frame++) {
00285 cal_target_vel_xya.rotate_z(-delta.angle.z);
00286
00287
00288 angle_delta += delta.angle.z;
00289 pos_delta += delta.loc.rotate_z(angle_delta);
00290
00291
00292
00293
00294
00295 double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00296 double sway = wp.sway*cos(2*M_PI*cycle);
00297 double hop = wp.hop*sin(4*M_PI*cycle);
00298 double height = wp.body_height;
00299 BodyPosition nextpos;
00300 nextpos.loc.set(0,-sway,height+hop);
00301 nextpos.angle.set(0,wp.body_angle,0);
00302
00303
00304 for(unsigned int i=0; i<NumLegs; i++){
00305
00306 bool air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
00307 double air_f = wp.leg[i].down_time - wp.leg[i].lift_time;
00308 double nextlegangles[JointsPerLeg];
00309
00310 if(air != legw[i].air){
00311 if(air){
00312
00313
00314
00315
00316
00317
00318
00319
00320 t = wp.period/1000.0 * 0.75;
00321 vector3d vfp;
00322
00323 vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00324 vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00325 double vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00326
00327
00328
00329
00330
00331 vfp.z = 0.0;
00332 double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00333 vector3d target = (wp.leg[i].neutral + vfp*b).rotate_z(vfa*b);
00334 legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00335 }else{
00336 legpos[i].z = 0.0;
00337 }
00338 legw[i].air = air;
00339 }
00340
00341 if(air){
00342 t = (cycle - wp.leg[i].lift_time) / air_f;
00343 legpos[i] = legw[i].airpath.eval(t);
00344
00345
00346
00347
00348
00349
00350
00351
00352 double velfraction_x = fabs(vel_xya.x / MAX_DX);
00353 double velfraction_y = fabs(vel_xya.y / MAX_DY);
00354 double velfraction_a = fabs(vel_xya.z / MAX_DA * 2);
00355
00356
00357 double velfraction =
00358 (velfraction_x > velfraction_y) ?
00359 velfraction_x : velfraction_y;
00360 if(velfraction_a > velfraction)
00361 velfraction = velfraction_a;
00362
00363
00364
00365
00366
00367 velfraction-=1;
00368 velfraction*=velfraction;
00369 velfraction*=velfraction;
00370
00371 legpos[i].z *= 1-velfraction;
00372
00373 }else{
00374 legpos[i] = (legpos[i] - delta.loc).rotate_z(-delta.angle.z);
00375 }
00376
00377 GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00378 for(unsigned int j=0; j<JointsPerLeg; j++)
00379 cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00380 }
00381
00382
00383 AdjustedTime = time+TimeOffset+(int)(frame*TimeStep*slowmo);
00384 }
00385
00386 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00387 motman->setOutput(this,joint,cmds[joint]);
00388
00389
00390 return NumLegs*JointsPerLeg;
00391 }
00392
00393 void WalkMC::resetLegPos() {
00394 BodyPosition nextpos;
00395 nextpos.loc.set(0,0,wp.body_height);
00396 nextpos.angle.set(0,wp.body_angle,0);
00397 for(unsigned int i=0; i<NumLegs; i++) {
00398 double tmp[JointsPerLeg];
00399 for(unsigned int j=0; j<JointsPerLeg; j++)
00400 tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00401 GetLegPosition(legpos[i],tmp,nextpos,i);
00402
00403
00404
00405
00406
00407 }
00408
00409 }
00410
00411 unsigned int checksum(const char *data,int num)
00412 {
00413 unsigned long c;
00414 int i;
00415
00416 c = 0;
00417 for(i=0; i<num; i++){
00418 c = c ^ (data[i]*13 + 37);
00419 c = (c << 13) | (c >> 19);
00420 }
00421
00422 return(c);
00423 }
00424
00425 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00426 float inmat[11];
00427 inmat[0]=in.x;
00428 inmat[1]=in.y;
00429 inmat[2]=in.z;
00430 inmat[3]=fabs(in.y);
00431 inmat[4]=fabs(in.z);
00432 inmat[5]=exp(-.5f*in.z*in.z)*sin(in.z*2.5f);
00433 inmat[6]=in.x*in.x+in.y*in.y;
00434 inmat[7]=in.x*in.z;
00435 inmat[8]=in.y*in.x;
00436 inmat[9]=in.z*in.y;
00437 inmat[10]=1;
00438 out.set(0,0,0);
00439 for(unsigned int c=0; c<11; c++)
00440 out.x+=mat[0][c]*inmat[c];
00441 for(unsigned int c=0; c<11; c++)
00442 out.y+=mat[1][c]*inmat[c];
00443 for(unsigned int c=0; c<11; c++)
00444 out.z+=mat[2][c]*inmat[c];
00445 }
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460