Homepage Demos Overview Downloads Tutorials 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(), 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   //  RegInit();
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   //  cmds[HeadOffset+TiltOffset].set(.3333,1);
00122 }
00123 
00124 // tss "SmoothWalk" addition follows
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     // we may stopping, but not stopped yet...
00133     return ((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0));
00134   }
00135   return JointsPerLeg*NumLegs;
00136 }
00137 // tss "SmoothWalk" addition ends
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     //return 0;
00163     len-=used; buf+=used;
00164     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
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     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
00195     return origlen-len; 
00196   } else {
00197     // backwards compatability - if there's no creator code, just assume it's a straight WalkParam
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     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
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   //sout->printf("Saved WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
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   // we just modified the target velocity, but we'll hold off on generating
00266   // an event until the changes are actually picked up by the motion system
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); //apply new value
00279   }
00280 }
00281   
00282 int WalkMC::updateOutputs() {
00283   //  cout << "WalkMC,,," << flush;
00284   if(!isDirty())
00285     return 0;
00286 
00287   if(vel_xya.sqlength()==0) {
00288     // we had been stopped - better check someone else didn't move the legs while we weren't using them...
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     //software accel:
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     //no software accel:
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 // tss "SmoothWalk" addition follows
00333   // If necessary, we compute a new TimeOffset here.
00334   if(NewCycleOffset) {
00335     TimeOffset = CycleOffset - time % wp.period;
00336     NewCycleOffset = false;
00337   }
00338 
00339   // Adjusted time--time adjusted for cycle matching
00340   int AdjustedTime = time + TimeOffset;
00341 
00342   // 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.
00343   if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00344     CycleOffset = AdjustedTime % wp.period;
00345     NewCycleOffset = true;
00346   }
00347 // tss "SmoothWalk" addition ends
00348 
00349   for(unsigned int frame=0; frame<NumFrames; frame++) {
00350     cal_target_vel_xya.rotate_z(-delta.angle.z);
00351 
00352     // incorporate movement delta
00353     angle_delta += delta.angle.z;
00354     pos_delta += delta.loc.rotate_z(angle_delta);
00355 
00356     //    cout << "setup,,," << flush;
00357 
00358 // tss "SmoothWalk" modification follows
00359     // double cycle = (double)(time % wp.period) / wp.period;
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         //cout << "leg "<<i<<": " <<AdjustedTime << ' ' << cycle << ' ' << last_cycle << ' ' << midstep << ' ' <<step_count ;
00370         if(last_cycle<midstep && midstep<=ncyc) { //we just completed a step
00371           step_count--;
00372           //cout << " -> " << step_count << endl;
00373           if(step_count==0) { //we're done, copy out any completed frames
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             //    cout << "WalkMC-done" << endl;
00381             return frame==0?0:NumLegs*JointsPerLeg;
00382           }
00383           break; //don't count legs moving in sync as two steps, only ever one step at a time
00384         }
00385         //cout << endl;
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     //    cout << "loop,,," << flush;
00398     for(unsigned int i=0; i<NumLegs; i++){
00399 
00400       //interpret a down time before a lift time as a request to move the leg in reverse (has its uses)
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             cout << i << ":   ";
00420             cout << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << "  ->  ";
00421             GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00422             for(unsigned int j=0; j<JointsPerLeg; j++)
00423               cout << nextlegangles[j] << ' ';
00424             cout << endl;
00425           */
00426           tm = wp.period/1000.0 * 0.75; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
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             //software accel:
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             //no software accel:
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 // Core tss "SmoothWalk" addition follows
00468         // KLUDGY MOD. Goal: reduce the height of the
00469         // AIBO's steps as its velocity nears zero.
00470         // Since I don't know how most of this code 
00471         // works, I'll directly alter legpos[i].z in
00472         // the following code to change the z height
00473         // with velocity.
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); //rotation seems more sensitive
00477 
00478         // Choose the biggest velfraction
00479         double velfraction =
00480           (velfraction_x > velfraction_y) ?
00481             velfraction_x : velfraction_y;
00482         if(velfraction_a > velfraction)
00483           velfraction = velfraction_a;
00484 
00485         // Modify legpos[i].z with velfraction to
00486         // shrink it down
00487         //velfraction = atan(velfraction * M_PI);
00488 
00489         velfraction-=1;
00490         velfraction*=velfraction;
00491         velfraction*=velfraction;
00492 
00493         legpos[i].z *= 1-velfraction;
00494 // Core tss "SmoothWalk" addition ends
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; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
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   //    cout << "WalkMC-done" << endl;
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       for(unsigned int j=0; j<JointsPerLeg; j++)
00545       cout << state->outputs[LegOffset+i*JointsPerLeg+j] << ' ';
00546       cout << "  ->  " << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << endl;
00547     */
00548   }
00549   //cout << "----------------------" << endl;
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 /*! @file
00589  * @brief Implements WalkMC, a MotionCommand for walking around
00590  * @author CMU RoboSoccer 2001-2002 (Creator)
00591  * @author ejt (ported)
00592  *
00593  * @verbinclude CMPack_license.txt
00594  *
00595  * $Author: ejt $
00596  * $Name: tekkotsu-2_2_2 $
00597  * $Revision: 1.30 $
00598  * $State: Exp $
00599  * $Date: 2004/12/20 21:26:24 $
00600  */
00601 

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:16 2005 by Doxygen 1.4.0