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   // tss "SmoothWalk" modification follows
00063   //  : MotionCommand(), isPaused(false), wp(), body_loc(), body_angle(),
00064   //    pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00065   //    time(get_time()), TimeStep(FrameTime), vel_xya(0,0,0),
00066   //    target_vel_xya(0,0,0)
00067 
00068   : MotionCommand(), isPaused(false), wp(), cp(), body_loc(), body_angle(),
00069         pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00070         time(get_time()), TimeStep(FrameTime), slowmo(1.0f),
00071         CycleOffset(0), TimeOffset(0), NewCycleOffset(false),
00072         vel_xya(0,0,0), target_vel_xya(0,0,0), last_target_vel_xya(0,0,0)
00073 {
00074   init(pfile);
00075 }
00076 
00077 WalkMC::CalibrationParam::CalibrationParam() {
00078   for(unsigned int r=0; r<3; r++) {
00079     for(unsigned int c=0; c<11; c++)
00080       f_calibration[r][c]=b_calibration[r][c]=0;
00081     f_calibration[r][r]=b_calibration[r][r]=1;
00082   }
00083   for(unsigned int d=0; d<NUM_DIM; d++)
00084     max_accel[d]=0;
00085   max_vel[0]=max_vel[1]=MAX_DX;
00086   max_vel[2]=MAX_DY;
00087   max_vel[3]=MAX_DA;
00088 }
00089 
00090 
00091 void WalkMC::DoStart() {
00092   MotionCommand::DoStart();
00093   LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00094   e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00095   postEvent(e);
00096   travelTime=get_time();
00097 }
00098 
00099 void WalkMC::DoStop() {
00100   unsigned int t=get_time();
00101   LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
00102   e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00103   postEvent(e);
00104   travelTime=t;
00105   MotionCommand::DoStop();
00106 }
00107 
00108 void WalkMC::init(const char* pfile)
00109 {
00110   //  RegInit();
00111   body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
00112   body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));
00113 
00114   for(unsigned int i=0; i<NumLegs; i++)
00115     legw[i].air = false;
00116 
00117   if(pfile!=NULL)
00118     LoadFile(pfile);
00119   else
00120     LoadFile(config->motion.walk.c_str());
00121 
00122   double zeros[JointsPerLeg];
00123   for(unsigned int i=0; i<JointsPerLeg; i++)
00124     zeros[i]=0;
00125   resetLegPos();
00126 
00127   //  cmds[HeadOffset+TiltOffset].set(.3333,1);
00128 }
00129 
00130 // tss "SmoothWalk" addition follows
00131 int WalkMC::isDirty()
00132 {
00133   if(isPaused)
00134     return false;
00135   if((target_vel_xya.x == 0) && (target_vel_xya.y == 0) && (target_vel_xya.z == 0)) {
00136     // we may stopping, but not stopped yet...
00137     return ((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0));
00138   }
00139   return true;
00140 }
00141 // tss "SmoothWalk" addition ends
00142 
00143   
00144 unsigned int WalkMC::getBinSize() const {
00145   unsigned int used=0;
00146   used+=creatorSize("WalkMC-2.2");
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((used=checkCreator("WalkMC-2.2",buf,len,false))!=0) {
00156     len-=used; buf+=used;
00157     if(len>=sizeof(WalkParam))
00158       memcpy(&wp,buf,used=sizeof(WalkParam));
00159     else
00160       return 0;
00161     len-=used; buf+=used;
00162     if(len>=sizeof(CalibrationParam))
00163       memcpy(&cp,buf,used=sizeof(CalibrationParam));
00164     else
00165       memcpy(&cp,buf,used=len);
00166     //return 0;
00167     len-=used; buf+=used;
00168     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
00169     return origlen-len; 
00170   } else if((used=checkCreator("WalkMC",buf,len,false))!=0) {
00171     sout->printf("Pre-version 2.2 walk parameter file\n");
00172     len-=used; buf+=used;
00173     for(unsigned int i=0; i<4; i++) {
00174       if(len>=sizeof(LegParam))
00175         memcpy(&wp.leg[i],buf,used=sizeof(LegParam));
00176       else
00177         return 0;
00178       len-=used; buf+=used;
00179     }
00180     if(0==(used=decode(wp.body_height,buf,len))) return 0;
00181     len-=used; buf+=used;
00182     if(0==(used=decode(wp.body_angle,buf,len))) return 0;
00183     len-=used; buf+=used;
00184     if(0==(used=decode(wp.hop,buf,len))) return 0;
00185     len-=used; buf+=used;
00186     if(0==(used=decode(wp.sway,buf,len))) return 0;
00187     len-=used; buf+=used;
00188     if(0==(used=decode(wp.period,buf,len))) return 0;
00189     len-=used; buf+=used;
00190     if(0==(used=decode(wp.useDiffDrive,buf,len))) return 0;
00191     len-=used; buf+=used;
00192     wp.sag=0;
00193     if(len>=sizeof(CalibrationParam))
00194       memcpy(&cp,buf,used=sizeof(CalibrationParam));
00195     else
00196       return 0;
00197     len-=used; buf+=used;
00198     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
00199     return origlen-len; 
00200   } else {
00201     // backwards compatability - if there's no creator code, just assume it's a straight WalkParam
00202     sout->printf("Assuming old-format walk parameter file\n");
00203     for(unsigned int i=0; i<4; i++) {
00204       if(len>=sizeof(LegParam))
00205         memcpy(&wp.leg[i],buf,used=sizeof(LegParam));
00206       else
00207         return 0;
00208       len-=used; buf+=used;
00209     }
00210     if(0==(used=decode(wp.body_height,buf,len))) return 0;
00211     len-=used; buf+=used;
00212     if(0==(used=decode(wp.body_angle,buf,len))) return 0;
00213     len-=used; buf+=used;
00214     if(0==(used=decode(wp.hop,buf,len))) return 0;
00215     len-=used; buf+=used;
00216     if(0==(used=decode(wp.sway,buf,len))) return 0;
00217     len-=used; buf+=used;
00218     if(0==(used=decode(wp.period,buf,len))) return 0;
00219     len-=used; buf+=used;
00220     wp.useDiffDrive=0;
00221     wp.sag=0;
00222     cp=CalibrationParam();
00223     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
00224     return origlen-len; 
00225   }
00226 }
00227 
00228 unsigned int WalkMC::SaveBuffer(char buf[], unsigned int len) const {
00229   unsigned int origlen=len;
00230   unsigned int used=0;
00231   if(0==(used=saveCreator("WalkMC-2.2",buf,len))) return 0;
00232   len-=used; buf+=used;
00233   if(len>=sizeof(WalkParam))
00234     memcpy(buf,&wp,used=sizeof(WalkParam));
00235   else
00236     return 0;
00237   len-=used; buf+=used;
00238   if(len>=sizeof(CalibrationParam))
00239     memcpy(buf,&cp,used=sizeof(CalibrationParam));
00240   else
00241     return 0;
00242   len-=used; buf+=used;
00243   //sout->printf("Saved WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
00244   return origlen-len;
00245 }
00246 
00247 unsigned int WalkMC::LoadFile(const char* filename) {
00248   return LoadSave::LoadFile(config->motion.makePath(filename).c_str());
00249 }
00250 unsigned int WalkMC::SaveFile(const char* filename) const {
00251   return LoadSave::SaveFile(config->motion.makePath(filename).c_str());
00252 }
00253 
00254 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00255 {
00256 #ifdef BOUND_MOTION
00257   da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00258   double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00259 
00260   vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00261   double l = v.length();
00262   if(l > 1) v /= l;
00263 
00264   dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00265   dy = cp.strafe_max_vel * v.y * fa;
00266 #endif
00267 
00268   target_vel_xya.set(dx,dy,da);
00269   // we just modified the target velocity, but we'll hold off on generating
00270   // an event until the changes are actually picked up by the motion system
00271 }
00272 
00273 int WalkMC::updateOutputs() {
00274   //  cout << "WalkMC,,," << flush;
00275   if(!isDirty())
00276     return 0;
00277 
00278   if(vel_xya.sqlength()==0) {
00279     // we had been stopped - better check someone else didn't move the legs while we weren't using them...
00280     resetLegPos(); 
00281   }
00282 
00283   unsigned int curT=get_time();
00284   if(last_target_vel_xya!=target_vel_xya) {
00285     last_target_vel_xya=target_vel_xya;
00286     LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00287     e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00288     postEvent(e);
00289     travelTime=curT;
00290   }
00291   
00292   float tm = TimeStep * slowmo / 1000;
00293 
00294   vector3d cal_target_vel_xya(target_vel_xya);
00295   if(target_vel_xya.x<0)
00296     cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::reverse];
00297   else
00298     cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00299   cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00300   cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00301   if(cal_target_vel_xya.sqlength()>.0025)
00302     if(target_vel_xya.x<0)
00303       applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00304     else
00305       applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00306 
00307   //software accel:
00308   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);
00309   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);
00310   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);
00311   //no software accel:
00312   //vel_xya=cal_target_vel_xya;
00313   //<end>
00314 
00315   BodyPosition delta;
00316   delta.loc.set(vel_xya.x*tm,vel_xya.y*tm,0);
00317   delta.angle.set(0,0,vel_xya.z*tm);
00318 
00319   time=(int)(curT*slowmo);
00320 
00321 // tss "SmoothWalk" addition follows
00322   // If necessary, we compute a new TimeOffset here.
00323   if(NewCycleOffset) {
00324     TimeOffset = CycleOffset - time % wp.period;
00325     NewCycleOffset = false;
00326   }
00327 
00328   // Adjusted time--time adjusted for cycle matching
00329   int AdjustedTime = time + TimeOffset;
00330 
00331   // 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.
00332   if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00333     CycleOffset = AdjustedTime % wp.period;
00334     NewCycleOffset = true;
00335   }
00336 // tss "SmoothWalk" addition ends
00337 
00338   for(unsigned int frame=0; frame<NumFrames; frame++) {
00339     cal_target_vel_xya.rotate_z(-delta.angle.z);
00340 
00341     // incorporate movement delta
00342     angle_delta += delta.angle.z;
00343     pos_delta += delta.loc.rotate_z(angle_delta);
00344 
00345     //    cout << "setup,,," << flush;
00346 
00347 // tss "SmoothWalk" modification follows
00348     // double cycle = (double)(time % wp.period) / wp.period;
00349     double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00350     double sway   = wp.sway*cos(2*M_PI*cycle);
00351     double hop    = wp.hop*sin(4*M_PI*cycle);
00352     double height = wp.body_height;
00353     BodyPosition nextpos;
00354     nextpos.loc.set(0,-sway,height+hop);
00355     nextpos.angle.set(0,wp.body_angle,0);
00356 
00357     //    cout << "loop,,," << flush;
00358     for(unsigned int i=0; i<NumLegs; i++){
00359 
00360       //interpret a down time before a lift time as a request to move the leg in reverse (has its uses)
00361       float lift_time=wp.leg[i].lift_time;
00362       float down_time=wp.leg[i].down_time;
00363       float dir=1;
00364       if(down_time==lift_time)
00365         dir=0;
00366       else if(down_time<lift_time) {
00367         lift_time=wp.leg[i].down_time;
00368         down_time=wp.leg[i].lift_time;
00369         dir=-1;
00370       }
00371 
00372       bool air = (cycle >= lift_time) && (cycle < down_time);
00373       double air_f = down_time - lift_time;
00374       double nextlegangles[JointsPerLeg];
00375 
00376       if(air != legw[i].air){
00377         if(air){
00378           /*
00379             cout << i << ":   ";
00380             cout << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << "  ->  ";
00381             GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00382             for(unsigned int j=0; j<JointsPerLeg; j++)
00383               cout << nextlegangles[j] << ' ';
00384             cout << endl;
00385           */
00386           tm = wp.period/1000.0 * 0.75; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
00387           vector3d vfp;
00388           //software accel:
00389           vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*tm, vel_xya.x+max_accel_xya.x*tm);
00390           vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*tm, vel_xya.y+max_accel_xya.y*tm);
00391           double vfa   = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00392           //no software accel:
00393           //vfp.x=cal_target_vel_xya.x;
00394           //vfp.y=cal_target_vel_xya.y;
00395           //double vfa=cal_target_vel_xya.z;
00396           //<end>
00397           vfp.z = 0.0;
00398           double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00399           vector3d target;
00400           if(wp.useDiffDrive) {
00401             float rot = vfa/cp.max_vel[CalibrationParam::rotate];
00402             if((i&1)==0)
00403               rot=-rot;
00404             vfp.x += cp.max_vel[CalibrationParam::forward]*rot;
00405             target = (wp.leg[i].neutral + vfp*b*dir);
00406           } else {
00407             target = (wp.leg[i].neutral + vfp*b*dir).rotate_z(vfa*b);
00408           }
00409           target.z+=wp.sag;
00410           liftPos[i]=legpos[i];
00411           downPos[i]=target;
00412           legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00413         }else{
00414           legpos[i].z = wp.leg[i].neutral.z;
00415         }
00416         legw[i].air = air;
00417       }
00418 
00419       if(air){
00420         legw[i].cyc = (cycle - lift_time) / air_f;
00421         legpos[i] = legw[i].airpath.eval(legw[i].cyc);
00422 
00423 // Core tss "SmoothWalk" addition follows
00424         // KLUDGY MOD. Goal: reduce the height of the
00425         // AIBO's steps as its velocity nears zero.
00426         // Since I don't know how most of this code 
00427         // works, I'll directly alter legpos[i].z in
00428         // the following code to change the z height
00429         // with velocity.
00430         double velfraction_x = fabs(vel_xya.x / MAX_DX);
00431         double velfraction_y = fabs(vel_xya.y / MAX_DY);
00432         double velfraction_a = fabs(vel_xya.z / MAX_DA * 2); //rotation seems more sensitive
00433 
00434         // Choose the biggest velfraction
00435         double velfraction =
00436           (velfraction_x > velfraction_y) ?
00437             velfraction_x : velfraction_y;
00438         if(velfraction_a > velfraction)
00439           velfraction = velfraction_a;
00440 
00441         // Modify legpos[i].z with velfraction to
00442         // shrink it down
00443         //velfraction = atan(velfraction * M_PI);
00444 
00445         velfraction-=1;
00446         velfraction*=velfraction;
00447         velfraction*=velfraction;
00448 
00449         legpos[i].z *= 1-velfraction;
00450 // Core tss "SmoothWalk" addition ends
00451       }else{
00452         if(dir==0)
00453           legpos[i]=wp.leg[i].neutral;
00454         else {
00455           if(wp.useDiffDrive) {
00456             tm = wp.period/1000.0 * 0.75; //wtf is the 0.75 based on?  Don't ask me, i just work here! (ejt)
00457             double vfa   = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*tm, vel_xya.z+max_accel_xya.z*tm);
00458             legpos[i] -= delta.loc*dir;
00459             float rot = vfa/cp.max_vel[CalibrationParam::rotate]*TimeStep * slowmo / 1000;
00460             if((i&1)==0)
00461               rot=-rot;
00462             legpos[i].x -= cp.max_vel[CalibrationParam::forward]*rot;
00463           } else {
00464             legpos[i] = (legpos[i] - delta.loc*dir).rotate_z(-delta.angle.z);
00465           }
00466         }
00467       }
00468 
00469       GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00470       for(unsigned int j=0; j<JointsPerLeg; j++)
00471         cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00472     }
00473 
00474     // tss "SmoothWalk" modification follows
00475     AdjustedTime = time+TimeOffset+(int)(frame*TimeStep*slowmo);
00476   }
00477   
00478   for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00479     motman->setOutput(this,joint,cmds[joint]);
00480 
00481   //    cout << "WalkMC-done" << endl;
00482   return NumLegs*JointsPerLeg;
00483 }
00484 
00485 void WalkMC::resetLegPos() {
00486   BodyPosition nextpos;
00487   nextpos.loc.set(0,0,wp.body_height);
00488   nextpos.angle.set(0,wp.body_angle,0);
00489   for(unsigned int i=0; i<NumLegs; i++) {
00490     double tmp[JointsPerLeg];
00491     for(unsigned int j=0; j<JointsPerLeg; j++)
00492       tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00493     GetLegPosition(legpos[i],tmp,nextpos,i);
00494     /*
00495       for(unsigned int j=0; j<JointsPerLeg; j++)
00496       cout << state->outputs[LegOffset+i*JointsPerLeg+j] << ' ';
00497       cout << "  ->  " << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << endl;
00498     */
00499   }
00500   //cout << "----------------------" << endl;
00501 }
00502 
00503 unsigned int checksum(const char *data,int num)
00504 {
00505   unsigned long c;
00506   int i;
00507 
00508   c = 0;
00509   for(i=0; i<num; i++){
00510     c = c ^ (data[i]*13 + 37);
00511     c = (c << 13) | (c >> 19);
00512   }
00513 
00514   return(c);
00515 }
00516 
00517 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00518   float inmat[11];
00519   inmat[0]=in.x;
00520   inmat[1]=in.y;
00521   inmat[2]=in.z;
00522   inmat[3]=fabs(in.y);
00523   inmat[4]=fabs(in.z);
00524   inmat[5]=exp(-.5f*in.z*in.z)*sin(in.z*2.5f);
00525   inmat[6]=in.x*in.x+in.y*in.y;
00526   inmat[7]=in.x*in.z;
00527   inmat[8]=in.y*in.x;
00528   inmat[9]=in.z*in.y;
00529   inmat[10]=1;
00530   out.set(0,0,0);
00531   for(unsigned int c=0; c<11; c++)
00532     out.x+=mat[0][c]*inmat[c];
00533   for(unsigned int c=0; c<11; c++)
00534     out.y+=mat[1][c]*inmat[c];
00535   for(unsigned int c=0; c<11; c++)
00536     out.z+=mat[2][c]*inmat[c];
00537 }
00538 
00539 /*! @file
00540  * @brief Implements WalkMC, a MotionCommand for walking around
00541  * @author CMU RoboSoccer 2001-2002 (Creator)
00542  * @author ejt (ported)
00543  *
00544  * @verbinclude CMPack_license.txt
00545  *
00546  * $Author: ejt $
00547  * $Name: tekkotsu-2_2 $
00548  * $Revision: 1.29 $
00549  * $State: Exp $
00550  * $Date: 2004/07/24 03:10:59 $
00551  */
00552 

Tekkotsu v2.2
Generated Tue Oct 19 14:19:16 2004 by Doxygen 1.3.9.1