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<4; i++){
00113     legw[i].air = false;
00114   }
00115 
00116   if(pfile!=NULL)
00117     LoadFile(pfile);
00118   else
00119     LoadFile("/ms/data/motion/walk.prm");
00120 
00121   double zeros[JointsPerLeg];
00122   for(unsigned int i=0; i<JointsPerLeg; i++)
00123     zeros[i]=0;
00124   for(unsigned int i=0; i<NumLegs; i++)
00125     GetLegPosition(legpos[i],zeros,i);
00126 
00127   //  cmds[HeadOffset+TiltOffset].set(.3333,1);
00128 }
00129 
00130 // tss "SmoothWalk" addition follows
00131 int WalkMC::isDirty()
00132 {
00133   if(isPaused) return false;
00134   if((target_vel_xya.x == 0) && (target_vel_xya.y == 0) && (target_vel_xya.z == 0)) {
00135   if((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0)) return true
00136 ;
00137   else return false;
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");
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(0==(used=checkCreator("WalkMC",buf,len,false))) {
00156     // backwards compatability - if there's no creator code, just assume it's a straight WalkParam
00157     sout->printf("Assuming old-format walk parameter file\n");
00158     if(len>=sizeof(WalkParam))
00159       memcpy(&wp,buf,used=sizeof(WalkParam));
00160     else
00161       return 0;
00162     len-=used; buf+=used;
00163     cp=CalibrationParam();
00164     //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
00165     return origlen-len; 
00166   }
00167   len-=used; buf+=used;
00168   if(len>=sizeof(WalkParam))
00169     memcpy(&wp,buf,used=sizeof(WalkParam));
00170   else
00171     return 0;
00172   len-=used; buf+=used;
00173   if(len>=sizeof(CalibrationParam))
00174     memcpy(&cp,buf,used=sizeof(CalibrationParam));
00175   else
00176     memcpy(&cp,buf,used=len);
00177   //return 0;
00178   len-=used; buf+=used;
00179   //sout->printf("Loaded WalkMC Parameters n=%ud checksum=0x%X\n",origlen-len,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)+sizeof(cp)));
00180   return origlen-len; 
00181 }
00182 
00183 unsigned int WalkMC::SaveBuffer(char buf[], unsigned int len) const {
00184   unsigned int origlen=len;
00185   unsigned int used=0;
00186   if(0==(used=saveCreator("WalkMC",buf,len))) return 0;
00187   len-=used; buf+=used;
00188   if(len>=sizeof(WalkParam))
00189     memcpy(buf,&wp,used=sizeof(WalkParam));
00190   else
00191     return 0;
00192   len-=used; buf+=used;
00193   if(len>=sizeof(CalibrationParam))
00194     memcpy(buf,&cp,used=sizeof(CalibrationParam));
00195   else
00196     return 0;
00197   len-=used; buf+=used;
00198   //sout->printf("Saved 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 }
00201 
00202 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00203 {
00204 #ifdef BOUND_MOTION
00205   da = bound(da, -cp.rotate_max_vel, cp.rotate_max_vel);
00206   double fa = 1.0 - fabs(da / cp.rotate_max_vel);
00207 
00208   vector2d v(dx>0?dx/cp.forward_max_vel:dx/cp.reverse_max_vel,dy/cp.strafe_max_vel);
00209   double l = v.length();
00210   if(l > 1) v /= l;
00211 
00212   dx = (dx>0?cp.forward_max_vel:cp.reverse_max_vel) * v.x * fa;
00213   dy = cp.strafe_max_vel * v.y * fa;
00214 #endif
00215 
00216   target_vel_xya.set(dx,dy,da);
00217   // we just modified the target velocity, but we'll hold off on generating
00218   // an event until the changes are actually picked up by the motion system
00219 }
00220 
00221 int WalkMC::updateOutputs() {
00222   //  cout << "WalkMC,,," << flush;
00223   if(!isDirty())
00224     return 0;
00225 
00226   unsigned int curT=get_time();
00227   if(last_target_vel_xya!=target_vel_xya) {
00228     last_target_vel_xya=target_vel_xya;
00229     LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,getTravelTime());
00230     e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00231     postEvent(e);
00232     travelTime=curT;
00233   }
00234   
00235   double t = TimeStep * slowmo / 1000;
00236 
00237   vector3d cal_target_vel_xya(target_vel_xya);
00238   cal_target_vel_xya.x/=cp.max_vel[CalibrationParam::forward];
00239   cal_target_vel_xya.y/=cp.max_vel[CalibrationParam::strafe];
00240   cal_target_vel_xya.z/=cp.max_vel[CalibrationParam::rotate];
00241   if(cal_target_vel_xya.sqlength()>.0025)
00242     if(target_vel_xya.x<0)
00243       applyCalibration(cp.b_calibration,target_vel_xya,cal_target_vel_xya);
00244     else
00245       applyCalibration(cp.f_calibration,target_vel_xya,cal_target_vel_xya);
00246 
00247   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);
00248   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);
00249   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);
00250 
00251   BodyPosition delta;
00252   delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);
00253   delta.angle.set(0,0,vel_xya.z*t);
00254 
00255   time=(int)(curT*slowmo);
00256 
00257 // tss "SmoothWalk" addition follows
00258   // If necessary, we compute a new TimeOffset here.
00259   if(NewCycleOffset) {
00260     TimeOffset = CycleOffset - time % wp.period;
00261     NewCycleOffset = false;
00262   }
00263 
00264   // Adjusted time--time adjusted for cycle matching
00265   int AdjustedTime = time + TimeOffset;
00266 
00267   // 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.
00268   if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
00269     CycleOffset = AdjustedTime % wp.period;
00270     NewCycleOffset = true;
00271   }
00272 // tss "SmoothWalk" addition ends
00273 
00274   for(unsigned int frame=0; frame<NumFrames; frame++) {
00275     cal_target_vel_xya.rotate_z(-delta.angle.z);
00276 
00277     // incorporate movement delta
00278     angle_delta += delta.angle.z;
00279     pos_delta += delta.loc.rotate_z(angle_delta);
00280 
00281     //    cout << "setup,,," << flush;
00282 
00283 // tss "SmoothWalk" modification follows
00284     // double cycle = (double)(time % wp.period) / wp.period;
00285     double cycle = (double)(AdjustedTime % wp.period) / wp.period;
00286     double sway   = wp.sway*cos(2*M_PI*cycle);
00287     double hop    = wp.hop*sin(4*M_PI*cycle);
00288     double height = wp.body_height;
00289     BodyPosition nextpos;
00290     nextpos.loc.set(0,-sway,height+hop);
00291     nextpos.angle.set(0,wp.body_angle,0);
00292 
00293     //    cout << "loop,,," << flush;
00294     for(unsigned int i=0; i<NumLegs; i++){
00295 
00296       bool air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
00297       double air_f = wp.leg[i].down_time - wp.leg[i].lift_time;
00298       double nextlegangles[JointsPerLeg];
00299 
00300       if(air != legw[i].air){
00301         if(air){
00302           t = wp.period/1000.0 * 0.75;
00303           vector3d vfp;
00304           vfp.x = bound(cal_target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00305           vfp.y = bound(cal_target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00306           vfp.z = 0.0;
00307           double vfa   = bound(cal_target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00308           double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00309           vector3d target = (wp.leg[i].neutral + vfp*b).rotate_z(vfa*b);
00310           legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00311         }else{
00312           legpos[i].z = 0.0;
00313         }
00314         legw[i].air = air;
00315       }
00316 
00317       if(air){
00318         t = (cycle - wp.leg[i].lift_time) / air_f;
00319         legpos[i] = legw[i].airpath.eval(t);
00320 
00321 // Core tss "SmoothWalk" addition follows
00322         // KLUDGY MOD. Goal: reduce the height of the
00323         // AIBO's steps as its velocity nears zero.
00324         // Since I don't know how most of this code 
00325         // works, I'll directly alter legpos[i].z in
00326         // the following code to change the z height
00327         // with velocity.
00328         double velfraction_x = fabs(vel_xya.x / MAX_DX);
00329         double velfraction_y = fabs(vel_xya.y / MAX_DY);
00330         double velfraction_a = fabs(vel_xya.z / MAX_DA);
00331 
00332         // Choose the biggest velfraction
00333         double velfraction =
00334           (velfraction_x > velfraction_y) ?
00335             velfraction_x : velfraction_y;
00336         if(velfraction_a > velfraction)
00337           velfraction = velfraction_a;
00338 
00339         // Modify legpos[i].z with velfraction to
00340         // shrink it down
00341         //velfraction = atan(velfraction * M_PI);
00342 
00343         velfraction-=1;
00344         velfraction*=velfraction;
00345         velfraction*=velfraction;
00346 
00347         legpos[i].z *= 1-velfraction;
00348 // Core tss "SmoothWalk" addition ends
00349       }else{
00350         legpos[i] = (legpos[i] - delta.loc).rotate_z(-delta.angle.z);
00351       }
00352 
00353       GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00354       for(unsigned int j=0; j<JointsPerLeg; j++)
00355         cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00356     }
00357 
00358     // tss "SmoothWalk" modification follows
00359     AdjustedTime = time+TimeOffset+(int)(frame*TimeStep*slowmo);
00360   }
00361   
00362   for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00363     motman->setOutput(this,joint,cmds[joint]);
00364 
00365   //    cout << "WalkMC-done" << endl;
00366   return NumLegs*JointsPerLeg;
00367 }
00368 
00369 void WalkMC::resetLegPos() {
00370   for(unsigned int i=0; i<NumLegs; i++) {
00371     double tmp[JointsPerLeg];
00372     for(unsigned int j=0; j<JointsPerLeg; j++)
00373       tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00374     GetLegPosition(legpos[i],tmp,i);
00375   }
00376 }
00377 
00378 unsigned int checksum(const char *data,int num)
00379 {
00380   unsigned long c;
00381   int i;
00382 
00383   c = 0;
00384   for(i=0; i<num; i++){
00385     c = c ^ (data[i]*13 + 37);
00386     c = (c << 13) | (c >> 19);
00387   }
00388 
00389   return(c);
00390 }
00391 
00392 void WalkMC::applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out) {
00393   float inmat[11];
00394   inmat[0]=in.x;
00395   inmat[1]=in.y;
00396   inmat[2]=in.z;
00397   inmat[3]=fabs(in.y);
00398   inmat[4]=fabs(in.z);
00399   inmat[5]=atan2(in.y,inmat[3]);
00400   inmat[6]=in.x*in.x+in.y*in.y;
00401   inmat[7]=in.x*in.z;
00402   inmat[8]=in.y*in.x;
00403   inmat[9]=in.z*in.y;
00404   inmat[10]=1;
00405   out.set(0,0,0);
00406   for(unsigned int c=0; c<11; c++)
00407     out.x+=mat[0][c]*inmat[c];
00408   for(unsigned int c=0; c<11; c++)
00409     out.y+=mat[1][c]*inmat[c];
00410   for(unsigned int c=0; c<11; c++)
00411     out.z+=mat[2][c]*inmat[c];
00412 }
00413 
00414 /*! @file
00415  * @brief Implements WalkMC, a MotionCommand for walking around
00416  * @author CMU RoboSoccer 2001-2002 (Creator)
00417  * @author ejt (ported)
00418  *
00419  * @verbinclude CMPack_license.txt
00420  *
00421  * $Author: ejt $
00422  * $Name: tekkotsu-2_0 $
00423  * $Revision: 1.21 $
00424  * $State: Exp $
00425  * $Date: 2004/01/19 20:36:06 $
00426  */
00427 

Tekkotsu v2.0
Generated Wed Jan 21 03:20:30 2004 by Doxygen 1.3.4