Tekkotsu 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     if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0))
00136       return false;
00137   }
00138 #ifdef TGT_HAS_LEGS
00139   return JointsPerLeg*NumLegs;
00140 #elif defined(TGT_HAS_WHEELS)
00141   return NumWheels;
00142 #else
00143   return 0;
00144 #endif
00145 }
00146 // tss "SmoothWalk" addition ends
00147 
00148   
00149 unsigned int WalkMC::getBinSize() const {
00150   unsigned int used=0;
00151   used+=creatorSize("WalkMC-2.2");
00152   used+=sizeof(wp);
00153   used+=sizeof(cp);
00154   return used;  
00155 }
00156 
00157 unsigned int WalkMC::loadBuffer(const char buf[], unsigned int len) {
00158   enum {
00159     VER1, // original CMPack version
00160     VER2, // added calibration parameters
00161     VER3 // added diff drive and sag parameter
00162   } version;
00163   
00164   unsigned int origlen=len;
00165   if(checkCreatorInc("WalkMC-2.2",buf,len,false)) {
00166     version=VER3;
00167   } else if(checkCreatorInc("WalkMC",buf,len,false)) {
00168     sout->printf("Pre-2.2 release walk parameter file\n");
00169     version=VER2;
00170   } else {
00171     // backwards compatability - if there's no creator code, just assume it's a straight WalkParam
00172     sout->printf("Assuming CMPack format walk parameter file\n");
00173     version=VER1;
00174   }
00175   
00176   // Leg parameters
00177   for(unsigned int i=0; i<4; ++i) {
00178     if(!decodeInc(wp.leg[i].neutral.x,buf,len)) return 0;
00179     if(!decodeInc(wp.leg[i].neutral.y,buf,len)) return 0;
00180     if(!decodeInc(wp.leg[i].neutral.z,buf,len)) return 0;
00181     if(!decodeInc(wp.leg[i].lift_vel.x,buf,len)) return 0;
00182     if(!decodeInc(wp.leg[i].lift_vel.y,buf,len)) return 0;
00183     if(!decodeInc(wp.leg[i].lift_vel.z,buf,len)) return 0;
00184     if(!decodeInc(wp.leg[i].down_vel.x,buf,len)) return 0;
00185     if(!decodeInc(wp.leg[i].down_vel.y,buf,len)) return 0;
00186     if(!decodeInc(wp.leg[i].down_vel.z,buf,len)) return 0;
00187     if(!decodeInc(wp.leg[i].lift_time,buf,len)) return 0;
00188     if(!decodeInc(wp.leg[i].down_time,buf,len)) return 0;
00189   }
00190   // Body parameters
00191   if(!decodeInc(wp.body_height,buf,len)) return 0;
00192   if(!decodeInc(wp.body_angle,buf,len)) return 0;
00193   if(!decodeInc(wp.hop,buf,len)) return 0;
00194   if(!decodeInc(wp.sway,buf,len)) return 0;
00195   if(!decodeInc(wp.period,buf,len)) return 0;
00196   if(version<VER3) wp.useDiffDrive=0; else if(!decodeInc(wp.useDiffDrive,buf,len)) return 0;
00197   if(version<VER3) wp.sag=0; else if(!decodeInc(wp.sag,buf,len)) return 0;
00198   if(!decodeInc(wp.reserved,buf,len)) return 0;
00199   // Calibration parameters
00200   if(version<VER2) {
00201     cp=CalibrationParam();
00202   } else {
00203     for(unsigned int i=0; i<3; ++i)
00204       for(unsigned int j=0; j<11; ++j)
00205         if(!decodeInc(cp.f_calibration[i][j],buf,len)) return 0;
00206     for(unsigned int i=0; i<3; ++i)
00207       for(unsigned int j=0; j<11; ++j)
00208         if(!decodeInc(cp.b_calibration[i][j],buf,len)) return 0;
00209     for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00210       if(!decodeInc(cp.max_accel[i],buf,len)) return 0;
00211     for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00212       if(!decodeInc(cp.max_vel[i],buf,len)) return 0;
00213   }
00214   return origlen-len; 
00215 }
00216 
00217 unsigned int WalkMC::saveBuffer(char buf[], unsigned int len) const {
00218   unsigned int origlen=len;
00219   // Leg parameters
00220   if(!saveCreatorInc("WalkMC-2.2",buf,len)) return 0;
00221   for(unsigned int i=0; i<4; ++i) {
00222     if(!encodeInc(wp.leg[i].neutral.x,buf,len)) return 0;
00223     if(!encodeInc(wp.leg[i].neutral.y,buf,len)) return 0;
00224     if(!encodeInc(wp.leg[i].neutral.z,buf,len)) return 0;
00225     if(!encodeInc(wp.leg[i].lift_vel.x,buf,len)) return 0;
00226     if(!encodeInc(wp.leg[i].lift_vel.y,buf,len)) return 0;
00227     if(!encodeInc(wp.leg[i].lift_vel.z,buf,len)) return 0;
00228     if(!encodeInc(wp.leg[i].down_vel.x,buf,len)) return 0;
00229     if(!encodeInc(wp.leg[i].down_vel.y,buf,len)) return 0;
00230     if(!encodeInc(wp.leg[i].down_vel.z,buf,len)) return 0;
00231     if(!encodeInc(wp.leg[i].lift_time,buf,len)) return 0;
00232     if(!encodeInc(wp.leg[i].down_time,buf,len)) return 0;
00233   }
00234   // Body parameters
00235   if(!encodeInc(wp.body_height,buf,len)) return 0;
00236   if(!encodeInc(wp.body_angle,buf,len)) return 0;
00237   if(!encodeInc(wp.hop,buf,len)) return 0;
00238   if(!encodeInc(wp.sway,buf,len)) return 0;
00239   if(!encodeInc(wp.period,buf,len)) return 0;
00240   if(!encodeInc(wp.useDiffDrive,buf,len)) return 0;
00241   if(!encodeInc(wp.sag,buf,len)) return 0;
00242   if(!encodeInc(wp.reserved,buf,len)) return 0;
00243   // Calibration parameters
00244   for(unsigned int i=0; i<3; ++i)
00245     for(unsigned int j=0; j<11; ++j)
00246       if(!encodeInc(cp.f_calibration[i][j],buf,len)) return 0;
00247   for(unsigned int i=0; i<3; ++i)
00248     for(unsigned int j=0; j<11; ++j)
00249       if(!encodeInc(cp.b_calibration[i][j],buf,len)) return 0;
00250   for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00251     if(!encodeInc(cp.max_accel[i],buf,len)) return 0;
00252   for(unsigned int i=0; i<CalibrationParam::NUM_DIM; ++i)
00253     if(!encodeInc(cp.max_vel[i],buf,len)) return 0;
00254   return origlen-len; 
00255 }
00256 
00257 unsigned int WalkMC::loadFile(const char* filename) {
00258   return LoadSave::loadFile(config->motion.makePath(filename).c_str());
00259 }
00260 unsigned int WalkMC::saveFile(const char* filename) const {
00261   return LoadSave::saveFile(config->motion.makePath(filename).c_str());
00262 }
00263 
00264 void WalkMC::setTargetVelocity(double dx,double dy,double da,int n)
00265 {
00266 #ifdef BOUND_MOTION
00267   da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00268   double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00269 
00270   vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00271   double l = v.length();
00272   if(l > 1) v /= l;
00273 
00274   dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00275   dy = cp.strafe_max_vel * v.y * fa;
00276 #endif
00277 
00278   target_vel_xya.set(dx,dy,da);
00279   //std::cout << "set " << dx << ' ' << dy << ' ' << da << std::endl;
00280   // we just modified the target velocity, but we'll hold off on generating
00281   // an event until the changes are actually picked up by the motion system
00282 
00283   step_count=n;
00284 }
00285 
00286 void WalkMC::setTargetDisplacement(double dx, double dy, double da, unsigned int n) {
00287   if(n==0) {
00288     setTargetVelocity(0,0,0,0);
00289   } else {
00290     //compute the point in cycle of the mid-step
00291     float midsteps[NumLegs];
00292     for(unsigned int i=0; i<NumLegs; i++)
00293       midsteps[i] = (wp.leg[i].down_time+wp.leg[i].lift_time)/2;
00294     //find the number of unique steps per cycle
00295     unsigned int steps_per_cycle=4;
00296     for(unsigned int i=0; i<NumLegs-1; i++)
00297       for(unsigned int j=i+1; j<NumLegs; j++)
00298         if(midsteps[i]==midsteps[j]) {
00299           steps_per_cycle--;
00300           break;
00301         }
00302     //compute velocity needed to move requested distance in the time it takes to do n steps
00303     double scale=steps_per_cycle/(n*wp.period/1000.0);
00304     double vx=dx*scale;
00305     double vy=dy*scale;
00306     double va=da*scale;
00307     setTargetVelocity(vx,vy,va,n); //apply new value
00308   }
00309 }
00310   
00311 int WalkMC::updateOutputs() {
00312   //  cout << "WalkMC,,," << flush;
00313   if(!isDirty())
00314     return 0;
00315 
00316   unsigned int curT=get_time();
00317   if(last_target_vel_xya!=target_vel_xya) {
00318     last_target_vel_xya=target_vel_xya;
00319     LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00320     e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00321     //cout << e.getDescription(true) << endl;
00322     postEvent(e);
00323     travelTime=curT;
00324   }
00325   
00326 #if defined(TGT_HAS_LEGS)
00327   
00328   if(vel_xya.sqlength()==0) {
00329     // we had been stopped - better check someone else didn't move the legs while we weren't using them...
00330     resetLegPos(); 
00331   }
00332   
00333   float tm = TimeStep * slowmo / 1000;
00334 
00335   vector3d cal_target_vel_xya(target_vel_xya);
00336   if(target_vel_xya.x<0)
00337     cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::reverse];
00338   else
00339     cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00340   cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00341   cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00342   if(cal_target_vel_xya.sqlength()>.0025)
00343     if(target_vel_xya.x<0)
00344       applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00345     else
00346       applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00347 
00348   if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00349     //software accel:
00350     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);
00351     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);
00352     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);
00353   } else {
00354     //no software accel:
00355     vel_xya=cal_target_vel_xya;
00356   }
00357 
00358   BodyPosition delta;
00359   delta.loc.set(vel_xya.x*tm,vel_xya.y*tm,0);
00360   delta.angle.set(0,0,vel_xya.z*tm);
00361 
00362   time=(int)(curT*slowmo);
00363 
00364 // tss "SmoothWalk" addition follows
00365   // If necessary, we compute a new TimeOffset here.
00366   if(NewCycleOffset) {
00367     TimeOffset = CycleOffset - time % wp.period;
00368     NewCycleOffset = false;
00369   }
00370 
00371   // Adjusted time--time adjusted for cycle matching
00372   int AdjustedTime = time + TimeOffset;
00373 
00374   // 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.
00375   if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00376     CycleOffset = AdjustedTime % wp.period;
00377     NewCycleOffset = true;
00378   }
00379 // tss "SmoothWalk" addition ends
00380 
00381   for(unsigned int frame=0; frame<NumFrames; frame++) {
00382     cal_target_vel_xya.rotate_z(-delta.angle.z);
00383 
00384     // incorporate movement delta
00385     angle_delta += delta.angle.z;
00386     pos_delta += delta.loc.rotate_z(angle_delta);
00387 
00388     //    cout << "setup,,," << flush;
00389 
00390 // tss "SmoothWalk" modification follows
00391     // double cycle = (double)(time % wp.period) / wp.period;
00392     AdjustedTime = time + TimeOffset + (int)(frame*TimeStep*slowmo);
00393     double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00394 
00395     if(step_count>0) {
00396       for(unsigned int i=0; i<NumLegs; i++){
00397         float midstep;
00398         if(step_threshold<=.5f)
00399           midstep=wp.leg[i].lift_time+(wp.leg[i].down_time-wp.leg[i].lift_time)*step_threshold*2;
00400         else
00401           midstep=wp.leg[i].down_time+(1-wp.leg[i].down_time+wp.leg[i].lift_time)*(step_threshold-.5)*2;
00402         midstep-=floorf(midstep);
00403         //cout << "leg "<<i<<": " <<AdjustedTime << ' ' << cycle << ' ' << last_cycle << ' ' << midstep << ' ' <<step_count ;
00404         bool above_last= (last_cycle<midstep);
00405         bool below_cur = (midstep<=cycle);
00406         bool wrap      = (cycle<last_cycle);
00407         //need any 2 of the conditions: above_last && below_cur || wrap && (above_last || below_cur)
00408         if(above_last+below_cur+wrap>1) { //we just completed a step
00409           step_count--;
00410           //cout << " -> " << step_count << endl;
00411           if(step_count==0) { //we're done, copy out any completed frames
00412             for(unsigned int f=0; f<frame; f++)
00413               for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00414                 motman->setOutput(this,joint,cmds[joint][f],f);
00415             CycleOffset = AdjustedTime % wp.period;
00416             NewCycleOffset = true;
00417             last_cycle=cycle;
00418             target_vel_xya.set(0,0,0);
00419             last_target_vel_xya=target_vel_xya;
00420             LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00421             e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00422             //cout << e.getDescription(true) << endl;
00423             postEvent(e);
00424             postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID,getTravelTime()));
00425             //    cout << "WalkMC-done" << endl;
00426             return frame==0?0:NumLegs*JointsPerLeg;
00427           }
00428           break; //don't count legs moving in sync as two steps, only ever one step at a time
00429         }
00430         //cout << endl;
00431       }
00432     }
00433     
00434 
00435     double sway   = wp.sway*cos(2*M_PI*cycle);
00436     double hop    = wp.hop*sin(4*M_PI*cycle);
00437     double height = wp.body_height;
00438     BodyPosition nextpos;
00439     nextpos.loc.set(0,-sway,height+hop);
00440     nextpos.angle.set(0,wp.body_angle,0);
00441 
00442     //    cout << "loop,,," << flush;
00443     for(unsigned int i=0; i<NumLegs; i++){
00444 
00445       //interpret a down time before a lift time as a request to move the leg in reverse (has its uses)
00446       float lift_time=wp.leg[i].lift_time;
00447       float down_time=wp.leg[i].down_time;
00448       float dir=1;
00449       if(down_time==lift_time)
00450         dir=0;
00451       else if(down_time<lift_time) {
00452         lift_time=wp.leg[i].down_time;
00453         down_time=wp.leg[i].lift_time;
00454         dir=-1;
00455       }
00456 
00457       bool air = (cycle >= lift_time) && (cycle < down_time);
00458       double air_f = down_time - lift_time;
00459       double nextlegangles[JointsPerLeg];
00460 
00461       if(air != legw[i].air){
00462         if(air){
00463           /*
00464             cout << i << ":   ";
00465             cout << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << "  ->  ";
00466             GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00467             for(unsigned int j=0; j<JointsPerLeg; j++)
00468               cout << nextlegangles[j] << ' ';
00469             cout << endl;
00470           */
00471           tm = wp.period/1000.0 * 0.75; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
00472           vector3d vfp;
00473           double vfa;
00474           if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00475             //software accel:
00476             vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*tm, vel_xya.x+max_accel_xya.x*tm);
00477             vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*tm, vel_xya.y+max_accel_xya.y*tm);
00478             vfa   = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00479           } else {
00480             //no software accel:
00481             vfp.x=cal_target_vel_xya.x;
00482             vfp.y=cal_target_vel_xya.y;
00483             vfa=cal_target_vel_xya.z;
00484           }
00485           
00486           vfp.z = 0.0;
00487           double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00488           vector3d target;
00489           if(wp.useDiffDrive) {
00490             float rot = vfa/cp.max_vel[CalibrationParam::rotate];
00491             if((i&1)==0)
00492               rot=-rot;
00493             vfp.x += cp.max_vel[CalibrationParam::forward]*rot;
00494             target = (wp.leg[i].neutral + vfp*b*dir);
00495           } else {
00496             target = (wp.leg[i].neutral + vfp*b*dir).rotate_z(vfa*b);
00497           }
00498           target.z+=wp.sag;
00499           liftPos[i]=legpos[i];
00500           downPos[i]=target;
00501           legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00502         }else{
00503           legpos[i].z = wp.leg[i].neutral.z;
00504         }
00505         legw[i].air = air;
00506       }
00507 
00508       if(air){
00509         legw[i].cyc = (cycle - lift_time) / air_f;
00510         legpos[i] = legw[i].airpath.eval(legw[i].cyc);
00511 
00512 // Core tss "SmoothWalk" addition follows
00513         // KLUDGY MOD. Goal: reduce the height of the
00514         // AIBO's steps as its velocity nears zero.
00515         // Since I don't know how most of this code 
00516         // works, I'll directly alter legpos[i].z in
00517         // the following code to change the z height
00518         // with velocity.
00519         double velfraction_x = fabs(vel_xya.x / MAX_DX);
00520         double velfraction_y = fabs(vel_xya.y / MAX_DY);
00521         double velfraction_a = fabs(vel_xya.z / MAX_DA * 2); //rotation seems more sensitive
00522 
00523         // Choose the biggest velfraction
00524         double velfraction =
00525           (velfraction_x > velfraction_y) ?
00526             velfraction_x : velfraction_y;
00527         if(velfraction_a > velfraction)
00528           velfraction = velfraction_a;
00529 
00530         // Modify legpos[i].z with velfraction to
00531         // shrink it down
00532         //velfraction = atan(velfraction * M_PI);
00533 
00534         velfraction-=1;
00535         velfraction*=velfraction;
00536         velfraction*=velfraction;
00537 
00538         legpos[i].z *= 1-velfraction;
00539 // Core tss "SmoothWalk" addition ends
00540       }else{
00541         if(dir==0)
00542           legpos[i]=wp.leg[i].neutral;
00543         else {
00544           if(wp.useDiffDrive) {
00545             tm = wp.period/1000.0 * 0.75; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
00546             double vfa;
00547             if(step_count<0 && (acc_style==CALIBRATION_ACCEL || acc_style==DEFAULT_ACCEL && !config->motion.inf_walk_accel)) {
00548               vfa   = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00549             } else {
00550               vfa   = cal_target_vel_xya.z;
00551             }
00552             legpos[i] -= delta.loc*dir;
00553             float rot = vfa/cp.max_vel[CalibrationParam::rotate]*TimeStep * slowmo / 1000;
00554             if((i&1)==0)
00555               rot=-rot;
00556             legpos[i].x -= cp.max_vel[CalibrationParam::forward]*rot;
00557           } else {
00558             legpos[i] = (legpos[i] - delta.loc*dir).rotate_z(-delta.angle.z);
00559           }
00560         }
00561       }
00562 
00563       GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00564       for(unsigned int j=0; j<JointsPerLeg; j++)
00565         cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00566 
00567     }
00568 
00569     last_cycle=cycle;
00570   }
00571   
00572   for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00573     motman->setOutput(this,joint,cmds[joint]);
00574   
00575   //    cout << "WalkMC-done" << endl;
00576   return NumLegs*JointsPerLeg;
00577   
00578 #elif defined(TGT_HAS_WHEELS)
00579   vel_xya=target_vel_xya;
00580   float wheelcircum = 2*M_PI*config->motion.wheelRadius;
00581   float baserpm = vel_xya.x / wheelcircum;
00582   float turnrpm = (vel_xya.z*config->motion.wheelBaseWidth/2) / wheelcircum;
00583   float left = baserpm - turnrpm;
00584   float right = baserpm + turnrpm;
00585   for(unsigned int frame=0; frame<NumFrames; frame++) {
00586     cmds[LWheelOffset][frame]=left;
00587     cmds[RWheelOffset][frame]=right;
00588   }
00589   //std::cout << "update " << vel_xya.x << ' ' << vel_xya.y << ' ' << vel_xya.z << " -> " << left << ' ' << right << std::endl;
00590   
00591   for(unsigned int w=WheelOffset; w<WheelOffset+NumWheels; w++)
00592     motman->setOutput(this,w,cmds[w]);
00593   
00594   //    cout << "WalkMC-done" << endl;
00595   return NumWheels;
00596   
00597 #else
00598 #  warning target has neither legs nor wheels -- WalkMC is a no-op
00599   return 0;
00600 #endif
00601 }
00602 
00603 void WalkMC::resetLegPos() {
00604   BodyPosition nextpos;
00605   nextpos.loc.set(0,0,wp.body_height);
00606   nextpos.angle.set(0,wp.body_angle,0);
00607 #ifdef TGT_HAS_LEGS
00608   for(unsigned int i=0; i<NumLegs; i++) {
00609     double tmp[JointsPerLeg];
00610     for(unsigned int j=0; j<JointsPerLeg; j++)
00611       tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00612     GetLegPosition(legpos[i],tmp,nextpos,i);
00613     /*
00614       for(unsigned int j=0; j<JointsPerLeg; j++)
00615       cout << state->outputs[LegOffset+i*JointsPerLeg+j] << ' ';
00616       cout << "  ->  " << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << endl;
00617     */
00618   }
00619 #endif
00620   //cout << "----------------------" << endl;
00621 }
00622 
00623 unsigned int checksum(const char *data,int num)
00624 {
00625   unsigned long c;
00626   int i;
00627 
00628   c = 0;
00629   for(i=0; i<num; i++){
00630     c = c ^ (data[i]*13 + 37);
00631     c = (c << 13) | (c >> 19);
00632   }
00633 
00634   return(c);
00635 }
00636 
00637 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00638   float inmat[11];
00639   inmat[0]=in.x;
00640   inmat[1]=in.y;
00641   inmat[2]=in.z;
00642   inmat[3]=fabs(in.y);
00643   inmat[4]=fabs(in.z);
00644   inmat[5]=exp(-.5f*in.z*in.z)*sin(in.z*2.5f);
00645   inmat[6]=in.x*in.x+in.y*in.y;
00646   inmat[7]=in.x*in.z;
00647   inmat[8]=in.y*in.x;
00648   inmat[9]=in.z*in.y;
00649   inmat[10]=1;
00650   out.set(0,0,0);
00651   for(unsigned int c=0; c<11; c++)
00652     out.x+=mat[0][c]*inmat[c];
00653   for(unsigned int c=0; c<11; c++)
00654     out.y+=mat[1][c]*inmat[c];
00655   for(unsigned int c=0; c<11; c++)
00656     out.z+=mat[2][c]*inmat[c];
00657 }
00658 
00659 /*! @file
00660  * @brief Implements WalkMC, a MotionCommand for walking around
00661  * @author CMU RoboSoccer 2001-2002 (Creator)
00662  * @author ejt (ported)
00663  *
00664  * @verbinclude CMPack_license.txt
00665  *
00666  * $Author: ejt $
00667  * $Name: tekkotsu-4_0 $
00668  * $Revision: 1.42 $
00669  * $State: Exp $
00670  * $Date: 2007/11/18 06:47:03 $
00671  */
00672 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:56 2007 by Doxygen 1.5.4