Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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 
00034 #include "Events/EventRouter.h"
00035 #include "Events/LocomotionEvent.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 
00046 //REGIMP(WalkMC);
00047 
00048 #define BOUND_MOTION
00049 
00050 const float WalkMC::MAX_DX   = 180;//225; // mm/sec
00051 const float WalkMC::MAX_DY   = 140;//170; // mm/sec
00052 const float WalkMC::MAX_DA   = 1.8;//2.1; // rad/sec
00053 const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
00054 
00055 unsigned int checksum(const char *data,int num); //!< computes a file checksum
00056 template <class data> int read_file(data *arr,int num,const char *filename); //!< reads a set of walk parameters from a file
00057 template <class data> int save_file(data *arr,int num,const char *filename); //!< saves the current walk parameters to a file
00058 
00059 WalkMC::WalkMC(const char* pfile/*=NULL*/)
00060   : MotionCommand(), isPaused(false), wp(), body_loc(), body_angle(),
00061     pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00062     time(get_time()), TimeStep(FrameTime), vel_xya(0,0,0),
00063     target_vel_xya(0,0,0)
00064 {
00065   init(pfile);
00066 }
00067 
00068 void WalkMC::DoStart() {
00069   MotionCommand::DoStart();
00070   LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00071   e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00072   postEvent(e);
00073   travelTime=get_time();
00074 }
00075 
00076 void WalkMC::DoStop() {
00077   unsigned int t=get_time();
00078   LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
00079   e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00080   postEvent(e);
00081   travelTime=t;
00082   MotionCommand::DoStop();
00083 }
00084 
00085 void WalkMC::init(const char* pfile)
00086 {
00087   //  RegInit();
00088   body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
00089   body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));
00090 
00091   for(unsigned int i=0; i<4; i++){
00092     legw[i].air = false;
00093   }
00094 
00095   if(pfile!=NULL)
00096     load(pfile);
00097   else
00098     load("walk.prm");
00099 
00100   double zeros[JointsPerLeg];
00101   for(unsigned int i=0; i<JointsPerLeg; i++)
00102     zeros[i]=0;
00103   for(unsigned int i=0; i<NumLegs; i++)
00104     GetLegPosition(legpos[i],zeros,i);
00105 
00106   //  cmds[HeadOffset+TiltOffset].set(.3333,1);
00107 }
00108 
00109 void WalkMC::load(const char* pfile)
00110 {
00111   mzero(wp);
00112   int n = read_file(&wp,1,pfile);
00113   //  pprintf(TextOutputStream,"Loaded WalkMC Paramaters %s n=%d checksum=0x%X\n\xFF",pfile,n,checksum((char*)&wp,sizeof(wp)));
00114   printf("Loaded WalkMC Parameters %s n=%ud checksum=0x%X\n",pfile,n,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
00115 }
00116 
00117 void WalkMC::save(const char* pfile) const {
00118   int n = save_file(&wp,1,pfile);
00119   printf("Saved WalkMC Parameters %s n=%ud checksum=0x%X\n",pfile,n,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
00120 }
00121 
00122 
00123 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00124 {
00125 #ifdef BOUND_MOTION
00126   da = bound(da, -MAX_DA, MAX_DA);
00127   double fa = 1.0 - fabs(da / MAX_DA);
00128 
00129   vector2d v(dx/MAX_DX,dy/MAX_DY);
00130   double l = v.length();
00131   if(l > 1) v /= l;
00132 
00133   dx = MAX_DX * v.x * fa;
00134   dy = MAX_DY * v.y * fa;
00135 #endif
00136 
00137   target_vel_xya.set(dx,dy,da);
00138 
00139   if(isActive()) {
00140     unsigned int t=get_time();
00141     LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,t-travelTime);
00142     e.setXYA(dx,dy,da);
00143     postEvent(e);
00144     travelTime=t;
00145   }
00146 }
00147 
00148 int WalkMC::updateOutputs() {
00149   //  cout << "WalkMC,,," << flush;
00150   if(!isDirty())
00151     return 0;
00152   
00153   double t = TimeStep / 1000.0;
00154 
00155   vel_xya.x = bound(target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00156   vel_xya.y = bound(target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00157   vel_xya.z = bound(target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00158 
00159   BodyPosition delta;
00160   delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);
00161   delta.angle.set(0,0,vel_xya.z*t);
00162 
00163   time=get_time();
00164 
00165   for(unsigned int frame=0; frame<NumFrames; frame++) {
00166     target_vel_xya.rotate_z(-delta.angle.z);
00167 
00168     // incorporate movement delta
00169     angle_delta += delta.angle.z;
00170     pos_delta += delta.loc.rotate_z(angle_delta);
00171 
00172     //    cout << "setup,,," << flush;
00173 
00174     double cycle = (double)(time % wp.period) / wp.period;
00175     double sway   = wp.sway*cos(2*M_PI*cycle);
00176     double hop    = wp.hop*sin(4*M_PI*cycle);
00177     double height = wp.body_height;
00178     BodyPosition nextpos;
00179     nextpos.loc.set(0,-sway,height+hop);
00180     nextpos.angle.set(0,wp.body_angle,0);
00181 
00182     //    cout << "loop,,," << flush;
00183     for(unsigned int i=0; i<NumLegs; i++){
00184 
00185       bool air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
00186       double air_f = wp.leg[i].down_time - wp.leg[i].lift_time;
00187       double nextlegangles[JointsPerLeg];
00188 
00189       if(air != legw[i].air){
00190         if(air){
00191           t = wp.period/1000.0 * 0.75;
00192           vector3d vfp;
00193           vfp.x = bound(target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00194           vfp.y = bound(target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00195           vfp.z = 0.0;
00196           double vfa   = bound(target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00197           double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00198           vector3d target = (wp.leg[i].neutral + vfp*b).rotate_z(vfa*b);
00199           legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00200         }else{
00201           legpos[i].z = 0.0;
00202         }
00203         legw[i].air = air;
00204       }
00205 
00206       if(air){
00207         t = (cycle - wp.leg[i].lift_time) / air_f;
00208         legpos[i] = legw[i].airpath.eval(t);
00209       }else{
00210         legpos[i] = (legpos[i] - delta.loc).rotate_z(-delta.angle.z);
00211       }
00212 
00213       GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00214       for(unsigned int j=0; j<JointsPerLeg; j++)
00215         cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00216     }
00217 
00218     time += TimeStep;
00219   }
00220 
00221   for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00222     motman->setOutput(this,joint,cmds[joint]);
00223 
00224   //    cout << "WalkMC-done" << endl;
00225   return NumLegs*JointsPerLeg;
00226 }
00227 
00228 void WalkMC::resetLegPos() {
00229   for(unsigned int i=0; i<NumLegs; i++) {
00230     double tmp[JointsPerLeg];
00231     for(unsigned int j=0; j<JointsPerLeg; j++)
00232       tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00233     GetLegPosition(legpos[i],tmp,i);
00234   }
00235 }
00236 
00237 unsigned int checksum(const char *data,int num)
00238 {
00239   unsigned long c;
00240   int i;
00241 
00242   c = 0;
00243   for(i=0; i<num; i++){
00244     c = c ^ (data[i]*13 + 37);
00245     c = (c << 13) | (c >> 19);
00246   }
00247 
00248   return(c);
00249 }
00250 
00251 template <class data>
00252 int read_file(data *arr,int num,const char *filename)
00253 {
00254   char name[256];
00255   int fd;
00256   // HFS::FILE *in;
00257   int n;
00258 
00259   sprintf(name,"/ms/data/motion/%s",filename);
00260 
00261   fd = open(name,O_RDONLY);
00262   if(fd < 0) return(0);
00263   n = read(fd,arr,sizeof(data)*num);
00264   // cprintf(OutputBuf,"read_file: %d of %d\n\xFF",n,sizeof(data)*num);
00265   close(fd);
00266   if(n%sizeof(data)!=0)
00267     cout << "*** WARNING - short data read" << endl;
00268   n /= sizeof(data);
00269 
00270   return(n);
00271 }
00272 
00273 template <class data>
00274 int save_file(data *arr,int num,const char *filename) {
00275   char name[256];
00276   int fd;
00277   // HFS::FILE *in;
00278   int n;
00279 
00280   sprintf(name,"/ms/data/motion/%s",filename);
00281 
00282   cout << "writing " << name << " " << sizeof(data) << endl;
00283   fd = open(name,O_RDWR|O_CREAT|O_TRUNC);
00284   if(fd < 0) {
00285     cout << "open returned " << fd << endl;
00286     return(0);
00287   }
00288   n = write(fd,(const void*)arr,sizeof(data)*num);
00289   // cprintf(OutputBuf,"read_file: %d of %d\n\xFF",n,sizeof(data)*num);
00290   close(fd);
00291   if(n%sizeof(data)!=0)
00292     cout << "*** WARNING - short data write" << endl;
00293   n /= sizeof(data);
00294 
00295   return(n);
00296 }
00297 
00298 /*! @file
00299  * @brief Implements WalkMC, a MotionCommand for walking around
00300  * @author CMU RoboSoccer 2001-2002 (Creator)
00301  * @author ejt (ported)
00302  *
00303  * @verbinclude CMPack_license.txt
00304  *
00305  * $Author: ejt $
00306  * $Name: tekkotsu-1_4_1 $
00307  * $Revision: 1.13 $
00308  * $State: Exp $
00309  * $Date: 2003/05/01 17:19:29 $
00310  */
00311 

Tekkotsu v1.4
Generated Sat Jul 19 00:06:32 2003 by Doxygen 1.3.2