Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WalkMC.cc

Go to the documentation of this file.
00001 //This class is ported from Carnegie Mellon's 2001 Robosoccer entry, and falls under their license:
00002 /*=========================================================================
00003     CMPack'02 Source Code Release for OPEN-R SDK v1.0
00004     Copyright (C) 2002 Multirobot Lab [Project Head: Manuela Veloso]
00005     School of Computer Science, Carnegie Mellon University
00006   -------------------------------------------------------------------------
00007     This software is distributed under the GNU General Public License,
00008     version 2.  If you do not have a copy of this licence, visit
00009     www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
00010     Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
00011     in the hope that it will be useful, but WITHOUT ANY WARRANTY,
00012     including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00013   -------------------------------------------------------------------------
00014     Additionally licensed to Sony Corporation under the following terms:
00015 
00016     This software is provided by the copyright holders AS IS and any
00017     express or implied warranties, including, but not limited to, the
00018     implied warranties of merchantability and fitness for a particular
00019     purpose are disclaimed.  In no event shall authors be liable for
00020     any direct, indirect, incidental, special, exemplary, or consequential
00021     damages (including, but not limited to, procurement of substitute
00022     goods or services; loss of use, data, or profits; or business
00023     interruption) however caused and on any theory of liability, whether
00024     in contract, strict liability, or tort (including negligence or
00025     otherwise) arising in any way out of the use of this software, even if
00026     advised of the possibility of such damage.
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 //REGIMP(WalkMC);
00049 
00050 //#define BOUND_MOTION
00051 
00052 const float WalkMC::MAX_DX   = 180;//225; // mm/sec
00053 const float WalkMC::MAX_DY   = 140;//170; // mm/sec
00054 const float WalkMC::MAX_DA   = 1.8;//2.1; // rad/sec
00055 // tss "SmoothWalk" modification follows
00056 // const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
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); //!< computes a file checksum
00060 
00061 WalkMC::WalkMC(const char* pfile/*=NULL*/)
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   //leave values at 0!
00098   //e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00099   postEvent(e);
00100   travelTime=t;
00101   MotionCommand::DoStop();
00102 }
00103 
00104 void WalkMC::init(const char* pfile)
00105 {
00106   //  RegInit();
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   //  cmds[HeadOffset+TiltOffset].set(.3333,1);
00124 }
00125 
00126 // tss "SmoothWalk" addition follows
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     // we may stopping, but not stopped yet...
00135     return ((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0));
00136   }
00137   return JointsPerLeg*NumLegs;
00138 }
00139 // tss "SmoothWalk" addition ends
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     //return 0;
00165     len-=used; buf+=used;
00166     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
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     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
00197     return origlen-len; 
00198   } else {
00199     // backwards compatability - if there's no creator code, just assume it's a straight WalkParam
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     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
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   //sout->printf("Saved WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
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   // we just modified the target velocity, but we'll hold off on generating
00268   // an event until the changes are actually picked up by the motion system
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     //compute the point in cycle of the mid-step
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     //find the number of unique steps per cycle
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     //compute velocity needed to move requested distance in the time it takes to do n steps
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); //apply new value
00295   }
00296 }
00297   
00298 int WalkMC::updateOutputs() {
00299   //  cout << "WalkMC,,," << flush;
00300   if(!isDirty())
00301     return 0;
00302 
00303   if(vel_xya.sqlength()==0) {
00304     // we had been stopped - better check someone else didn't move the legs while we weren't using them...
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     //cout << e.getDescription(true) << endl;
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     //software accel:
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     //no software accel:
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 // tss "SmoothWalk" addition follows
00350   // If necessary, we compute a new TimeOffset here.
00351   if(NewCycleOffset) {
00352     TimeOffset = CycleOffset - time % wp.period;
00353     NewCycleOffset = false;
00354   }
00355 
00356   // Adjusted time--time adjusted for cycle matching
00357   int AdjustedTime = time + TimeOffset;
00358 
00359   // If walking speeds have dwindled down to zero, save our time offset from the beginning of the current walk cycle. Once we start walking again, we start up at the same offset to prevent jerky starts.
00360   if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00361     CycleOffset = AdjustedTime % wp.period;
00362     NewCycleOffset = true;
00363   }
00364 // tss "SmoothWalk" addition ends
00365 
00366   for(unsigned int frame=0; frame<NumFrames; frame++) {
00367     cal_target_vel_xya.rotate_z(-delta.angle.z);
00368 
00369     // incorporate movement delta
00370     angle_delta += delta.angle.z;
00371     pos_delta += delta.loc.rotate_z(angle_delta);
00372 
00373     //    cout << "setup,,," << flush;
00374 
00375 // tss "SmoothWalk" modification follows
00376     // double cycle = (double)(time % wp.period) / wp.period;
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         //cout << "leg "<<i<<": " <<AdjustedTime << ' ' << cycle << ' ' << last_cycle << ' ' << midstep << ' ' <<step_count ;
00389         bool above_last= (last_cycle<midstep);
00390         bool below_cur = (midstep<=cycle);
00391         bool wrap      = (cycle<last_cycle);
00392         //need any 2 of the conditions: above_last && below_cur || wrap && (above_last || below_cur)
00393         if(above_last+below_cur+wrap>1) { //we just completed a step
00394           step_count--;
00395           //cout << " -> " << step_count << endl;
00396           if(step_count==0) { //we're done, copy out any completed frames
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             //cout << e.getDescription(true) << endl;
00408             postEvent(e);
00409             //    cout << "WalkMC-done" << endl;
00410             return frame==0?0:NumLegs*JointsPerLeg;
00411           }
00412           break; //don't count legs moving in sync as two steps, only ever one step at a time
00413         }
00414         //cout << endl;
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     //    cout << "loop,,," << flush;
00427     for(unsigned int i=0; i<NumLegs; i++){
00428 
00429       //interpret a down time before a lift time as a request to move the leg in reverse (has its uses)
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             cout << i << ":   ";
00449             cout << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << "  ->  ";
00450             GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00451             for(unsigned int j=0; j<JointsPerLeg; j++)
00452               cout << nextlegangles[j] << ' ';
00453             cout << endl;
00454           */
00455           tm = wp.period/1000.0 * 0.75; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
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             //software accel:
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             //no software accel:
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 // Core tss "SmoothWalk" addition follows
00497         // KLUDGY MOD. Goal: reduce the height of the
00498         // AIBO's steps as its velocity nears zero.
00499         // Since I don't know how most of this code 
00500         // works, I'll directly alter legpos[i].z in
00501         // the following code to change the z height
00502         // with velocity.
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); //rotation seems more sensitive
00506 
00507         // Choose the biggest velfraction
00508         double velfraction =
00509           (velfraction_x > velfraction_y) ?
00510             velfraction_x : velfraction_y;
00511         if(velfraction_a > velfraction)
00512           velfraction = velfraction_a;
00513 
00514         // Modify legpos[i].z with velfraction to
00515         // shrink it down
00516         //velfraction = atan(velfraction * M_PI);
00517 
00518         velfraction-=1;
00519         velfraction*=velfraction;
00520         velfraction*=velfraction;
00521 
00522         legpos[i].z *= 1-velfraction;
00523 // Core tss "SmoothWalk" addition ends
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; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
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   //    cout << "WalkMC-done" << endl;
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       for(unsigned int j=0; j<JointsPerLeg; j++)
00574       cout << state->outputs[LegOffset+i*JointsPerLeg+j] << ' ';
00575       cout << "  ->  " << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << endl;
00576     */
00577   }
00578   //cout << "----------------------" << endl;
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 /*! @file
00618  * @brief Implements WalkMC, a MotionCommand for walking around
00619  * @author CMU RoboSoccer 2001-2002 (Creator)
00620  * @author ejt (ported)
00621  *
00622  * @verbinclude CMPack_license.txt
00623  *
00624  * $Author: ejt $
00625  * $Name: tekkotsu-2_4_1 $
00626  * $Revision: 1.34 $
00627  * $State: Exp $
00628  * $Date: 2005/06/29 22:01:55 $
00629  */
00630 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4