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<4; i++){
00113 legw[i].air = false;
00114 }
00115
00116 if(pfile!=NULL)
00117 LoadFile(pfile);
00118 else
00119 LoadFile("/ms/data/motion/walk.prm");
00120
00121 double zeros[JointsPerLeg];
00122 for(unsigned int i=0; i<JointsPerLeg; i++)
00123 zeros[i]=0;
00124 for(unsigned int i=0; i<NumLegs; i++)
00125 GetLegPosition(legpos[i],zeros,i);
00126
00127
00128 }
00129
00130
00131 int WalkMC::isDirty()
00132 {
00133 if(isPaused) return false;
00134 if((target_vel_xya.x == 0) && (target_vel_xya.y == 0) && (target_vel_xya.z == 0)) {
00135 if((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0)) return true
00136 ;
00137 else return false;
00138 }
00139 return true;
00140 }
00141
00142
00143
00144 unsigned int WalkMC::getBinSize() const {
00145 unsigned int used=0;
00146 used+=creatorSize("WalkMC");
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(0==(used=checkCreator("WalkMC",buf,len,false))) {
00156
00157 sout->printf("Assuming old-format walk parameter file\n");
00158 if(len>=sizeof(WalkParam))
00159 memcpy(&wp,buf,used=sizeof(WalkParam));
00160 else
00161 return 0;
00162 len-=used; buf+=used;
00163 cp=CalibrationParam();
00164
00165 return origlen-len;
00166 }
00167 len-=used; buf+=used;
00168 if(len>=sizeof(WalkParam))
00169 memcpy(&wp,buf,used=sizeof(WalkParam));
00170 else
00171 return 0;
00172 len-=used; buf+=used;
00173 if(len>=sizeof(CalibrationParam))
00174 memcpy(&cp,buf,used=sizeof(CalibrationParam));
00175 else
00176 memcpy(&cp,buf,used=len);
00177
00178 len-=used; buf+=used;
00179
00180 return origlen-len;
00181 }
00182
00183 unsigned int WalkMC::SaveBuffer(char buf[], unsigned int len) const {
00184 unsigned int origlen=len;
00185 unsigned int used=0;
00186 if(0==(used=saveCreator("WalkMC",buf,len))) return 0;
00187 len-=used; buf+=used;
00188 if(len>=sizeof(WalkParam))
00189 memcpy(buf,&wp,used=sizeof(WalkParam));
00190 else
00191 return 0;
00192 len-=used; buf+=used;
00193 if(len>=sizeof(CalibrationParam))
00194 memcpy(buf,&cp,used=sizeof(CalibrationParam));
00195 else
00196 return 0;
00197 len-=used; buf+=used;
00198
00199 return origlen-len;
00200 }
00201
00202 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00203 {
00204 #ifdef BOUND_MOTION
00205 da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00206 double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00207
00208 vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00209 double l = v.length();
00210 if(l > 1) v /= l;
00211
00212 dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00213 dy = cp.strafe_max_vel * v.y * fa;
00214 #endif
00215
00216 target_vel_xya.set(dx,dy,da);
00217
00218
00219 }
00220
00221 int WalkMC::updateOutputs() {
00222
00223 if(!isDirty())
00224 return 0;
00225
00226 unsigned int curT=get_time();
00227 if(last_target_vel_xya!=target_vel_xya) {
00228 last_target_vel_xya=target_vel_xya;
00229 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00230 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00231 postEvent(e);
00232 travelTime=curT;
00233 }
00234
00235 double t = TimeStep * slowmo / 1000;
00236
00237 vector3d cal_target_vel_xya(target_vel_xya);
00238 cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00239 cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00240 cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00241 if(cal_target_vel_xya.sqlength()>.0025)
00242 if(target_vel_xya.x<0)
00243 applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00244 else
00245 applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00246
00247 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);
00248 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);
00249 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);
00250
00251 BodyPosition delta;
00252 delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);
00253 delta.angle.set(0,0,vel_xya.z*t);
00254
00255 time=(int)(curT*slowmo);
00256
00257
00258
00259 if(NewCycleOffset) {
00260 TimeOffset = CycleOffset - time % wp.period;
00261 NewCycleOffset = false;
00262 }
00263
00264
00265 int AdjustedTime = time + TimeOffset;
00266
00267
00268 if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00269 CycleOffset = AdjustedTime % wp.period;
00270 NewCycleOffset = true;
00271 }
00272
00273
00274 for(unsigned int frame=0; frame<NumFrames; frame++) {
00275 cal_target_vel_xya.rotate_z(-delta.angle.z);
00276
00277
00278 angle_delta += delta.angle.z;
00279 pos_delta += delta.loc.rotate_z(angle_delta);
00280
00281
00282
00283
00284
00285 double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00286 double sway = wp.sway*cos(2*M_PI*cycle);
00287 double hop = wp.hop*sin(4*M_PI*cycle);
00288 double height = wp.body_height;
00289 BodyPosition nextpos;
00290 nextpos.loc.set(0,-sway,height+hop);
00291 nextpos.angle.set(0,wp.body_angle,0);
00292
00293
00294 for(unsigned int i=0; i<NumLegs; i++){
00295
00296 bool air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
00297 double air_f = wp.leg[i].down_time - wp.leg[i].lift_time;
00298 double nextlegangles[JointsPerLeg];
00299
00300 if(air != legw[i].air){
00301 if(air){
00302 t = wp.period/1000.0 * 0.75;
00303 vector3d vfp;
00304 vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00305 vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00306 vfp.z = 0.0;
00307 double vfa = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00308 double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00309 vector3d target = (wp.leg[i].neutral + vfp*b).rotate_z(vfa*b);
00310 legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00311 }else{
00312 legpos[i].z = 0.0;
00313 }
00314 legw[i].air = air;
00315 }
00316
00317 if(air){
00318 t = (cycle - wp.leg[i].lift_time) / air_f;
00319 legpos[i] = legw[i].airpath.eval(t);
00320
00321
00322
00323
00324
00325
00326
00327
00328 double velfraction_x = fabs(vel_xya.x / MAX_DX);
00329 double velfraction_y = fabs(vel_xya.y / MAX_DY);
00330 double velfraction_a = fabs(vel_xya.z / MAX_DA);
00331
00332
00333 double velfraction =
00334 (velfraction_x > velfraction_y) ?
00335 velfraction_x : velfraction_y;
00336 if(velfraction_a > velfraction)
00337 velfraction = velfraction_a;
00338
00339
00340
00341
00342
00343 velfraction-=1;
00344 velfraction*=velfraction;
00345 velfraction*=velfraction;
00346
00347 legpos[i].z *= 1-velfraction;
00348
00349 }else{
00350 legpos[i] = (legpos[i] - delta.loc).rotate_z(-delta.angle.z);
00351 }
00352
00353 GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00354 for(unsigned int j=0; j<JointsPerLeg; j++)
00355 cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00356 }
00357
00358
00359 AdjustedTime = time+TimeOffset+(int)(frame*TimeStep*slowmo);
00360 }
00361
00362 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00363 motman->setOutput(this,joint,cmds[joint]);
00364
00365
00366 return NumLegs*JointsPerLeg;
00367 }
00368
00369 void WalkMC::resetLegPos() {
00370 for(unsigned int i=0; i<NumLegs; i++) {
00371 double tmp[JointsPerLeg];
00372 for(unsigned int j=0; j<JointsPerLeg; j++)
00373 tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00374 GetLegPosition(legpos[i],tmp,i);
00375 }
00376 }
00377
00378 unsigned int checksum(const char *data,int num)
00379 {
00380 unsigned long c;
00381 int i;
00382
00383 c = 0;
00384 for(i=0; i<num; i++){
00385 c = c ^ (data[i]*13 + 37);
00386 c = (c << 13) | (c >> 19);
00387 }
00388
00389 return(c);
00390 }
00391
00392 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00393 float inmat[11];
00394 inmat[0]=in.x;
00395 inmat[1]=in.y;
00396 inmat[2]=in.z;
00397 inmat[3]=fabs(in.y);
00398 inmat[4]=fabs(in.z);
00399 inmat[5]=atan2(in.y,inmat[3]);
00400 inmat[6]=in.x*in.x+in.y*in.y;
00401 inmat[7]=in.x*in.z;
00402 inmat[8]=in.y*in.x;
00403 inmat[9]=in.z*in.y;
00404 inmat[10]=1;
00405 out.set(0,0,0);
00406 for(unsigned int c=0; c<11; c++)
00407 out.x+=mat[0][c]*inmat[c];
00408 for(unsigned int c=0; c<11; c++)
00409 out.y+=mat[1][c]*inmat[c];
00410 for(unsigned int c=0; c<11; c++)
00411 out.z+=mat[2][c]*inmat[c];
00412 }
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427