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 
00037 #include "Motion/MotionManager.h"
00038 
00039 #include <stdlib.h>
00040 #include <stdio.h>
00041 #include <iostream>
00042 #include <fcntl.h>
00043 #include <unistd.h>
00044 #include <fstream>
00045 #include <cmath>
00046 
00047 //REGIMP(WalkMC);
00048 
00049 //#define BOUND_MOTION
00050 
00051 const float WalkMC::MAX_DX   = 180;//225; // mm/sec
00052 const float WalkMC::MAX_DY   = 140;//170; // mm/sec
00053 const float WalkMC::MAX_DA   = 1.8;//2.1; // rad/sec
00054 // tss "SmoothWalk" modification follows
00055 // const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
00056 const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
00057 
00058 unsigned int checksum(const char *data,int num); //!< computes a file checksum
00059 
00060 WalkMC::WalkMC(const char* pfile/*=NULL*/)
00061 // tss "SmoothWalk" modification follows
00062 //  : MotionCommand(), isPaused(false), wp(), body_loc(), body_angle(),
00063 //    pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00064 //    time(get_time()), TimeStep(FrameTime), vel_xya(0,0,0),
00065 //    target_vel_xya(0,0,0)
00066   : MotionCommand(), isPaused(false), wp(), cp(), body_loc(), body_angle(),
00067     pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00068     time(get_time()), TimeStep(FrameTime), slowmo(1.0f),
00069     CycleOffset(0), TimeOffset(0), NewCycleOffset(false),
00070       vel_xya(0,0,0), target_vel_xya(0,0,0), last_target_vel_xya(0,0,0)
00071 {
00072   init(pfile);
00073 }
00074 
00075 WalkMC::CalibrationParam::CalibrationParam() {
00076   for(unsigned int r=0; r<3; r++) {
00077     for(unsigned int c=0; c<11; c++)
00078       f_calibration[r][c]=b_calibration[r][c]=0;
00079     f_calibration[r][r]=b_calibration[r][r]=1;
00080   }
00081   for(unsigned int d=0; d<NUM_DIM; d++)
00082     max_accel[d]=0;
00083   max_vel[0]=max_vel[1]=MAX_DX;
00084   max_vel[2]=MAX_DY;
00085   max_vel[3]=MAX_DA;
00086 }
00087 
00088 
00089 void WalkMC::DoStart() {
00090   MotionCommand::DoStart();
00091   LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00092   e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00093   postEvent(e);
00094   travelTime=get_time();
00095 }
00096 
00097 void WalkMC::DoStop() {
00098   unsigned int t=get_time();
00099   LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
00100   e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00101   postEvent(e);
00102   travelTime=t;
00103   MotionCommand::DoStop();
00104 }
00105 
00106 void WalkMC::init(const char* pfile)
00107 {
00108   //  RegInit();
00109   body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
00110   body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));
00111 
00112   for(unsigned int i=0; i<NumLegs; i++)
00113     legw[i].air = false;
00114 
00115   if(pfile!=NULL)
00116     LoadFile(pfile);
00117   else
00118     LoadFile("/ms/data/motion/walk.prm");
00119 
00120   double zeros[JointsPerLeg];
00121   for(unsigned int i=0; i<JointsPerLeg; i++)
00122     zeros[i]=0;
00123   resetLegPos();
00124 
00125   //  cmds[HeadOffset+TiltOffset].set(.3333,1);
00126 }
00127 
00128 // tss "SmoothWalk" addition follows
00129 int WalkMC::isDirty()
00130 {
00131   if(isPaused)
00132     return false;
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 true;
00138 }
00139 // tss "SmoothWalk" addition ends
00140 
00141   
00142 unsigned int WalkMC::getBinSize() const {
00143   unsigned int used=0;
00144   used+=creatorSize("WalkMC");
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(0==(used=checkCreator("WalkMC",buf,len,false))) {
00154     // backwards compatability - if there's no creator code, just assume it's a straight WalkParam
00155     sout->printf("Assuming old-format walk parameter file\n");
00156     if(len>=sizeof(WalkParam))
00157       memcpy(&wp,buf,used=sizeof(WalkParam));
00158     else
00159       return 0;
00160     len-=used; buf+=used;
00161     cp=CalibrationParam();
00162     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
00163     return origlen-len; 
00164   }
00165   len-=used; buf+=used;
00166   if(len>=sizeof(WalkParam))
00167     memcpy(&wp,buf,used=sizeof(WalkParam));
00168   else
00169     return 0;
00170   len-=used; buf+=used;
00171   if(len>=sizeof(CalibrationParam))
00172     memcpy(&cp,buf,used=sizeof(CalibrationParam));
00173   else
00174     memcpy(&cp,buf,used=len);
00175   //return 0;
00176   len-=used; buf+=used;
00177   //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
00178   return origlen-len; 
00179 }
00180 
00181 unsigned int WalkMC::SaveBuffer(char buf[], unsigned int len) const {
00182   unsigned int origlen=len;
00183   unsigned int used=0;
00184   if(0==(used=saveCreator("WalkMC",buf,len))) return 0;
00185   len-=used; buf+=used;
00186   if(len>=sizeof(WalkParam))
00187     memcpy(buf,&wp,used=sizeof(WalkParam));
00188   else
00189     return 0;
00190   len-=used; buf+=used;
00191   if(len>=sizeof(CalibrationParam))
00192     memcpy(buf,&cp,used=sizeof(CalibrationParam));
00193   else
00194     return 0;
00195   len-=used; buf+=used;
00196   //sout->printf("Saved 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 }
00199 
00200 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00201 {
00202 #ifdef BOUND_MOTION
00203   da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00204   double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00205 
00206   vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00207   double l = v.length();
00208   if(l > 1) v /= l;
00209 
00210   dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00211   dy = cp.strafe_max_vel * v.y * fa;
00212 #endif
00213 
00214   target_vel_xya.set(dx,dy,da);
00215   // we just modified the target velocity, but we'll hold off on generating
00216   // an event until the changes are actually picked up by the motion system
00217 }
00218 
00219 int WalkMC::updateOutputs() {
00220   //  cout << "WalkMC,,," << flush;
00221   if(!isDirty())
00222     return 0;
00223 
00224   if(vel_xya.sqlength()==0) {
00225     // we had been stopped - better check someone else didn't move the legs while we weren't using them...
00226     resetLegPos(); 
00227   }
00228 
00229   unsigned int curT=get_time();
00230   if(last_target_vel_xya!=target_vel_xya) {
00231     last_target_vel_xya=target_vel_xya;
00232     LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00233     e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00234     postEvent(e);
00235     travelTime=curT;
00236   }
00237   
00238   double t = TimeStep * slowmo / 1000;
00239 
00240   vector3d cal_target_vel_xya(target_vel_xya);
00241   if(target_vel_xya.x<0)
00242     cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::reverse];
00243   else
00244     cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00245   cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00246   cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00247   if(cal_target_vel_xya.sqlength()>.0025)
00248     if(target_vel_xya.x<0)
00249       applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00250     else
00251       applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00252 
00253   //software accel:
00254   vel_xya.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00255   vel_xya.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00256   vel_xya.z = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00257   //no software accel:
00258   //vel_xya=cal_target_vel_xya;
00259   //<end>
00260 
00261   BodyPosition delta;
00262   delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);
00263   delta.angle.set(0,0,vel_xya.z*t);
00264 
00265   time=(int)(curT*slowmo);
00266 
00267 // tss "SmoothWalk" addition follows
00268   // If necessary, we compute a new TimeOffset here.
00269   if(NewCycleOffset) {
00270     TimeOffset = CycleOffset - time % wp.period;
00271     NewCycleOffset = false;
00272   }
00273 
00274   // Adjusted time--time adjusted for cycle matching
00275   int AdjustedTime = time + TimeOffset;
00276 
00277   // 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.
00278   if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00279     CycleOffset = AdjustedTime % wp.period;
00280     NewCycleOffset = true;
00281   }
00282 // tss "SmoothWalk" addition ends
00283 
00284   for(unsigned int frame=0; frame<NumFrames; frame++) {
00285     cal_target_vel_xya.rotate_z(-delta.angle.z);
00286 
00287     // incorporate movement delta
00288     angle_delta += delta.angle.z;
00289     pos_delta += delta.loc.rotate_z(angle_delta);
00290 
00291     //    cout << "setup,,," << flush;
00292 
00293 // tss "SmoothWalk" modification follows
00294     // double cycle = (double)(time % wp.period) / wp.period;
00295     double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00296     double sway   = wp.sway*cos(2*M_PI*cycle);
00297     double hop    = wp.hop*sin(4*M_PI*cycle);
00298     double height = wp.body_height;
00299     BodyPosition nextpos;
00300     nextpos.loc.set(0,-sway,height+hop);
00301     nextpos.angle.set(0,wp.body_angle,0);
00302 
00303     //    cout << "loop,,," << flush;
00304     for(unsigned int i=0; i<NumLegs; i++){
00305 
00306       bool air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
00307       double air_f = wp.leg[i].down_time - wp.leg[i].lift_time;
00308       double nextlegangles[JointsPerLeg];
00309 
00310       if(air != legw[i].air){
00311         if(air){
00312           /*
00313             cout << i << ":   ";
00314             cout << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << "  ->  ";
00315             GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00316             for(unsigned int j=0; j<JointsPerLeg; j++)
00317               cout << nextlegangles[j] << ' ';
00318             cout << endl;
00319           */
00320           t = wp.period/1000.0 * 0.75;
00321           vector3d vfp;
00322           //software accel:
00323           vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00324           vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00325           double vfa   = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00326           //no software accel:
00327           //vfp.x=cal_target_vel_xya.x;
00328           //vfp.y=cal_target_vel_xya.y;
00329           //double vfa=cal_target_vel_xya.z;
00330           //<end>
00331           vfp.z = 0.0;
00332           double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00333           vector3d target = (wp.leg[i].neutral + vfp*b).rotate_z(vfa*b);
00334           legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00335         }else{
00336           legpos[i].z = 0.0;
00337         }
00338         legw[i].air = air;
00339       }
00340 
00341       if(air){
00342         t = (cycle - wp.leg[i].lift_time) / air_f;
00343         legpos[i] = legw[i].airpath.eval(t);
00344 
00345 // Core tss "SmoothWalk" addition follows
00346         // KLUDGY MOD. Goal: reduce the height of the
00347         // AIBO's steps as its velocity nears zero.
00348         // Since I don't know how most of this code 
00349         // works, I'll directly alter legpos[i].z in
00350         // the following code to change the z height
00351         // with velocity.
00352         double velfraction_x = fabs(vel_xya.x / MAX_DX);
00353         double velfraction_y = fabs(vel_xya.y / MAX_DY);
00354         double velfraction_a = fabs(vel_xya.z / MAX_DA * 2); //rotation seems more sensitive
00355 
00356         // Choose the biggest velfraction
00357         double velfraction =
00358           (velfraction_x > velfraction_y) ?
00359             velfraction_x : velfraction_y;
00360         if(velfraction_a > velfraction)
00361           velfraction = velfraction_a;
00362 
00363         // Modify legpos[i].z with velfraction to
00364         // shrink it down
00365         //velfraction = atan(velfraction * M_PI);
00366 
00367         velfraction-=1;
00368         velfraction*=velfraction;
00369         velfraction*=velfraction;
00370 
00371         legpos[i].z *= 1-velfraction;
00372 // Core tss "SmoothWalk" addition ends
00373       }else{
00374         legpos[i] = (legpos[i] - delta.loc).rotate_z(-delta.angle.z);
00375       }
00376 
00377       GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00378       for(unsigned int j=0; j<JointsPerLeg; j++)
00379         cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00380     }
00381 
00382     // tss "SmoothWalk" modification follows
00383     AdjustedTime = time+TimeOffset+(int)(frame*TimeStep*slowmo);
00384   }
00385   
00386   for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00387     motman->setOutput(this,joint,cmds[joint]);
00388 
00389   //    cout << "WalkMC-done" << endl;
00390   return NumLegs*JointsPerLeg;
00391 }
00392 
00393 void WalkMC::resetLegPos() {
00394   BodyPosition nextpos;
00395   nextpos.loc.set(0,0,wp.body_height);
00396   nextpos.angle.set(0,wp.body_angle,0);
00397   for(unsigned int i=0; i<NumLegs; i++) {
00398     double tmp[JointsPerLeg];
00399     for(unsigned int j=0; j<JointsPerLeg; j++)
00400       tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00401     GetLegPosition(legpos[i],tmp,nextpos,i);
00402     /*
00403       for(unsigned int j=0; j<JointsPerLeg; j++)
00404       cout << state->outputs[LegOffset+i*JointsPerLeg+j] << ' ';
00405       cout << "  ->  " << legpos[i].x << ' ' << legpos[i].y << ' ' << legpos[i].z << endl;
00406     */
00407   }
00408   //cout << "----------------------" << endl;
00409 }
00410 
00411 unsigned int checksum(const char *data,int num)
00412 {
00413   unsigned long c;
00414   int i;
00415 
00416   c = 0;
00417   for(i=0; i<num; i++){
00418     c = c ^ (data[i]*13 + 37);
00419     c = (c << 13) | (c >> 19);
00420   }
00421 
00422   return(c);
00423 }
00424 
00425 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00426   float inmat[11];
00427   inmat[0]=in.x;
00428   inmat[1]=in.y;
00429   inmat[2]=in.z;
00430   inmat[3]=fabs(in.y);
00431   inmat[4]=fabs(in.z);
00432   inmat[5]=exp(-.5f*in.z*in.z)*sin(in.z*2.5f);
00433   inmat[6]=in.x*in.x+in.y*in.y;
00434   inmat[7]=in.x*in.z;
00435   inmat[8]=in.y*in.x;
00436   inmat[9]=in.z*in.y;
00437   inmat[10]=1;
00438   out.set(0,0,0);
00439   for(unsigned int c=0; c<11; c++)
00440     out.x+=mat[0][c]*inmat[c];
00441   for(unsigned int c=0; c<11; c++)
00442     out.y+=mat[1][c]*inmat[c];
00443   for(unsigned int c=0; c<11; c++)
00444     out.z+=mat[2][c]*inmat[c];
00445 }
00446 
00447 /*! @file
00448  * @brief Implements WalkMC, a MotionCommand for walking around
00449  * @author CMU RoboSoccer 2001-2002 (Creator)
00450  * @author ejt (ported)
00451  *
00452  * @verbinclude CMPack_license.txt
00453  *
00454  * $Author: ejt $
00455  * $Name: tekkotsu-2_1 $
00456  * $Revision: 1.23 $
00457  * $State: Exp $
00458  * $Date: 2004/02/26 01:02:49 $
00459  */
00460 

Tekkotsu v2.1
Generated Tue Mar 16 23:19:16 2004 by Doxygen 1.3.5