Homepage Demos Overview Downloads Tutorials Reference
Credits

WalkMC.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 //This class is ported from Carnegie Mellon's 2001 Robosoccer entry, and falls under their license:
00004 /*=========================================================================
00005     CMPack'02 Source Code Release for OPEN-R SDK v1.0
00006     Copyright (C) 2002 Multirobot Lab [Project Head: Manuela Veloso]
00007     School of Computer Science, Carnegie Mellon University
00008   -------------------------------------------------------------------------
00009     This software is distributed under the GNU General Public License,
00010     version 2.  If you do not have a copy of this licence, visit
00011     www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
00012     Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
00013     in the hope that it will be useful, but WITHOUT ANY WARRANTY,
00014     including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00015   -------------------------------------------------------------------------
00016     Additionally licensed to Sony Corporation under the following terms:
00017 
00018     This software is provided by the copyright holders AS IS and any
00019     express or implied warranties, including, but not limited to, the
00020     implied warranties of merchantability and fitness for a particular
00021     purpose are disclaimed.  In no event shall authors be liable for
00022     any direct, indirect, incidental, special, exemplary, or consequential
00023     damages (including, but not limited to, procurement of substitute
00024     goods or services; loss of use, data, or profits; or business
00025     interruption) however caused and on any theory of liability, whether
00026     in contract, strict liability, or tort (including negligence or
00027     otherwise) arising in any way out of the use of this software, even if
00028     advised of the possibility of such damage.
00029   =========================================================================
00030 */
00031 
00032 #ifndef INCLUDED_WalkMC_h
00033 #define INCLUDED_WalkMC_h
00034 
00035 #include "MotionCommand.h"
00036 #include "Geometry.h"
00037 #include "OldKinematics.h"
00038 #include "Path.h"
00039 #include "Shared/get_time.h"
00040 #include "OutputCmd.h"
00041 #include "Shared/LoadSave.h"
00042 
00043 //! A nice walking class from Carnegie Mellon University's 2001 Robosoccer team, modified to fit this framework, see their <a href="../CMPack_license.txt">license</a>
00044 /*! Moves the feet through a looping path in order to walk - default parameters use
00045  *  a walk low to the ground so you don't walk over the ball.
00046  *
00047  *  There are around 50 parameters which control the walk - these are
00048  *  loaded from a file and can modify almost every aspect of the the
00049  *  gait.  It's a binary file format, I recommend using our Walk Edit
00050  *  menu to edit the parameters in real time and get immediate
00051  *  feedback.  It's a tricky job to find a good set of parameters.
00052  *
00053  *  And then, once you have it walking, there's a whole different
00054  *  problem of actually moving at the speed that's requested.  That's
00055  *  what the calibration parameters do - map the requested target
00056  *  speed to the command to pass to the engine so the resulting motion
00057  *  will hopefully match what you asked for.
00058  *
00059  *  You'll probably want to take a look at the setTargetVelocity()
00060  *  function to control the direction of the walk.
00061  *
00062  *  This class is in some dire need of some cleanup - we (Tekkotsu)
00063  *  didn't write it, never really bothered to get a deep understanding
00064  *  of it, but have none the less hacked around and added stuff on top
00065  *  of it.  So pardon the mess, unless you're feeling ambitious to
00066  *  write your own ;)
00067  *
00068  *  This portion of the code falls under CMPack's license:
00069  *  @verbinclude CMPack_license.txt
00070  */
00071 class WalkMC : public MotionCommand, public LoadSave {
00072 public:
00073   typedef SplinePath<vector3d,float> splinepath; //!<for convenience
00074   typedef HermiteSplineSegment<vector3d,float> spline; //!<for convenience
00075 
00076   //! holds state about each leg's path
00077   struct LegWalkState {
00078     LegWalkState() : airpath(), air(0), cyc(0) {} //!< constructor
00079     spline airpath; //!< the path to follow
00080     bool air; //!< true if in the air
00081     float cyc; //!< % (0,1) of how far along the path we are
00082   };
00083   
00084   //! holds parameters about how to move each leg
00085   struct LegParam {
00086     LegParam() : neutral(), lift_vel(), down_vel(), lift_time(0), down_time(0) {} //!< constructor
00087     vector3d neutral; //!< defines the "neutral" point of each leg - where it is in midstep
00088     vector3d lift_vel; //!< give the velocities to use when raising the paw
00089     vector3d down_vel; //!< give the velocities to use when lowering the paw
00090     double lift_time; //!< the time (as percentage of WalkParam::period) in the cycle to lift (so you can set different offsets between the paws)
00091     double down_time; //!< the time (as percentage of WalkParam::period) in the cycle to put down (so you can set different offsets between the paws)
00092   };
00093 
00094   //! holds more general parameters about the walk
00095   struct WalkParam {
00096     WalkParam() : body_height(0), body_angle(0), hop(0), sway(0), period(0), useDiffDrive(0), sag(0), reserved() {} //!< constructor
00097     LegParam leg[4]; //!< a set of LegParam's, one for each leg
00098     double body_height; //!< the height to hold the body (mm)
00099     double body_angle; //!< the angle to hold the body (rad - 0 is level)
00100     double hop;  //!< sinusoidal hop amplitude
00101     double sway; //!< sinusoidal sway in y direction
00102     long period; //!< the time between steps
00103     long useDiffDrive; //!< if non-zero, diff-drive style turning is used instead of rotational turning
00104     float sag; //!< the amount to sagging to account for when a foot is lifted
00105     float reserved; //!< just live with it
00106   };
00107 
00108   //! holds information to correct for slippage, non-idealities
00109   struct CalibrationParam {
00110     CalibrationParam();
00111 
00112     //! symbolic way to refer to each of the directions
00113     enum dimension_offset {
00114       forward,
00115       reverse, 
00116       strafe,
00117       rotate,
00118       NUM_DIM
00119     };
00120 
00121     float f_calibration[3][11]; //!< matrix of calibration parameters; 3 columns for f,s,r speeds, 2 columns for abs s,r speeds, 1 gabor function, 1 squared planar speed, 3 columns for f*r,s*f,r*s, and 1 column for offset
00122     float b_calibration[3][11]; //!< matrix of calibration parameters; 3 columns for f,s,r speeds, 2 columns for abs s,r speeds, 1 gabor function, 1 squared planar speed, 3 columns for f*r,s*f,r*s, and 1 column for offset
00123 
00124     float max_accel[NUM_DIM]; //!< maximum achievable acceleration, 0 for infinite (mm/s^2)
00125     float max_vel[NUM_DIM]; //!< maximum achievable velocity (mm/s)
00126   };
00127 
00128   //! constructor
00129   WalkMC(const char* pfile=NULL);
00130 
00131   virtual void DoStart(); //!< sends an activate LocomotionEvent
00132   virtual void DoStop();  //!< sends a deactivate LocomotionEvent
00133 
00134   virtual int updateOutputs(); //!< calculates current positions of the paws
00135   
00136   //! Returns true if we are walking. This modified isDirty allows the AIBO to slow down to a stop rather than stopping immediately.
00137   virtual int isDirty();
00138   
00139   //! always true - never autoprunes
00140   virtual int isAlive() { return true; }
00141 
00142   virtual unsigned int getBinSize() const;
00143   virtual unsigned int LoadBuffer(const char buf[], unsigned int len);
00144   virtual unsigned int SaveBuffer(char buf[], unsigned int len) const;
00145   virtual unsigned int LoadFile(const char* filename);
00146   virtual unsigned int SaveFile(const char* filename) const;
00147 
00148   //! set the direction to walk - can specify x (forward), y (left), and angular (counterclockwise) velocities
00149   void setTargetVelocity(double dx,double dy,double da);
00150   //! returns current velocity we're trying to go
00151   const vector3d& getTargetVelocity() { return target_vel_xya; }
00152   //! returns the velocity we're actually moving (subject to clipping at max_accel_xya), doesn't reflect value of getPaused()...
00153   const vector3d& getCurVelocity() const { return vel_xya;}
00154   //! returns the time at which we started traveling along the current vector
00155   unsigned int getStartTravelTime() { return travelTime; }
00156   //! returns the amount of time we've been traveling along the current vector
00157   unsigned int getTravelTime() { return get_time()-getStartTravelTime(); }
00158   
00159   void setPaused(bool p) { isPaused=p; } //!< if set to true, will stop moving
00160   bool getPaused() const { return isPaused; } //!< if is true, we aren't moving
00161   void setHeight(double h) { wp.body_height=h; } //!< sets WalkParam::body_height of #wp
00162   double getHeight() { return wp.body_height; } //!< gets WalkParam::body_height of #wp
00163   void setAngle(double a) { wp.body_angle=a; } //!< sets WalkParam::body_angle of #wp
00164   double getAngle() { return wp.body_angle; } //!< gets WalkParam::body_angle of #wp
00165   void setHop(double h) { wp.hop=h; } //!< sets WalkParam::hop of #wp
00166   double getHop() { return wp.hop; } //!< gets WalkParam::hop of #wp
00167   void setSway(double h) { wp.sway=h; } //!< sets WalkParam::sway of #wp
00168   double getSway() { return wp.sway; } //!< gets WalkParam::sway of #wp
00169   void setPeriod(long p) { wp.period=p; } //!< sets WalkParam::period of #wp
00170   long getPeriod() { return wp.period; } //!< gets WalkParam::period of #wp
00171   void setSlowMo(float p) { slowmo=p; } //!< sets slowmo
00172   float* getSlowMo() { return &slowmo; } //!< gets slowmo
00173 
00174   const vector3d& getLegPosition(LegOrder_t i) const { return legpos[i]; }
00175 
00176   WalkParam & getWP() { return wp; }; //!< returns the current walk parameter structure
00177   CalibrationParam & getCP() { return cp; }; //!< returns the current walk calibration parameter
00178   
00179   //! takes current leg positions from WorldState and tries to match the point in the cycle most like it
00180   void resetLegPos();
00181 
00182   static const float MAX_DX; //!< ==180 mm/sec
00183   static const float MAX_DY; //!< ==140 mm/sec
00184   static const float MAX_DA; //!< ==1.8 rad/sec
00185 // tss "SmoothWalk" modification follows (actually only comment is changed)
00186   // static const vector3d max_accel_xya; //!< vector version of MAX_DX,MAX_DY,MAX_DA
00187   static const vector3d max_accel_xya; //!< maximum acceleration of x, y, and a velocity
00188 
00189  protected:
00190   //! holds current joint commands
00191   OutputCmd cmds[NumOutputs][NumFrames];
00192 
00193   //! converts @a in to calibration parameters and multiplies through the calibration matrix
00194   static void applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out);
00195 
00196  protected:
00197   //! does some setup stuff, calls LoadFile(pfile)
00198   void init(const char* pfile);
00199 
00200   bool isPaused; //!< true if we are paused
00201 
00202   WalkParam wp; //!< current walking parameters (note that it's not static - different WalkMC's can have different setting, handy...
00203   CalibrationParam cp; //!< calibration parameters for current walk.
00204   LegWalkState legw[NumLegs]; //!< current state of each leg
00205   vector3d legpos[NumLegs]; //!< current position of each leg
00206   splinepath body_loc; //!< the path the body goes through while walking (?)
00207   splinepath body_angle; //!< the path the body goes through while walking (?)
00208   vector3d liftPos[NumLegs]; //!< position each of the feet was last lifted
00209   vector3d downPos[NumLegs]; //!< position each of the feet is next going to be set down
00210 
00211   vector3d pos_delta; //!< how much we've moved
00212   float angle_delta; //!< how much we've turned
00213   
00214   unsigned int travelTime; //!< the time of the last call to setTargetVelocity - handy to check the time we've been traveling current vector
00215   int time; //!< time of last call to updateJointCmds() (scaled by slowmo)
00216   int TimeStep; //!< time to pretend passes between each call to updateJointCmds() - usually RobotInfo::FrameTime
00217   float slowmo; //!< scales time values to make the walk move in slow motion for analysis (or fast forward)
00218 
00219   // tss "SmoothWalk" addition follows
00220   /*! The CycleOffset variable is used to ensure that each time the AIBO
00221    *  starts walking, it starts at the same point in the walk cycle as
00222    *  where it stopped. This measure is intended to decrease the amount
00223    *  of jerking (and hence deviation) that occurs when the AIBO starts
00224    *  walking forward and then suddenly stops. */
00225   int CycleOffset;
00226   /*! Each CycleOffset corresponds to a different TimeOffset once the
00227    *  robot starts walking again. Consider this example: the robot
00228    *  stops 2/3 of the way through the cycle, then starts again 1/3
00229    *  of the way through the cycle on the absolute clock. The time
00230    *  offset to advance the clock to the right part of the cycle is
00231    *  1/3 of a cycle, so we set TimeOffset to 1/3 cycle and add that to
00232    *  every clock value used in the walk code. */
00233   int TimeOffset;
00234   /*! Every time we stop, we know we'll have a new CycleOffset, and we'll
00235    *  need to compute a new TimeOffset. This boolean says as much. */
00236   bool NewCycleOffset;
00237 // tss "SmoothWalk" addition ends
00238 
00239   vector3d vel_xya; //!< the current velocity we're moving
00240   vector3d target_vel_xya; //!< the velocity that was requested
00241   vector3d last_target_vel_xya; //!< the velocity that was last sent to motion
00242 };
00243 
00244 /* struct LegState{
00245    long attr,reserved;
00246    point3d pos;
00247    double angles[3];
00248    };
00249    
00250    struct HeadState{
00251    long attr,reserved;
00252    vector3d target;
00253    double angles[3];
00254    };
00255    
00256    struct BodyState{
00257    BodyPosition pos;
00258    LegState leg[4];
00259    HeadState head;
00260    }; 
00261 */
00262 
00263 /*! @file
00264  * @brief Describes WalkMC, a MotionCommand for walking around
00265  * @author CMU RoboSoccer 2001-2002 (Creator)
00266  * @author ejt (ported)
00267  * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
00268  * 
00269  * @verbinclude CMPack_license.txt
00270  *
00271  * $Author: ejt $
00272  * $Name: tekkotsu-2_2 $
00273  * $Revision: 1.27 $
00274  * $State: Exp $
00275  * $Date: 2004/08/05 20:29:04 $
00276  */
00277 
00278 #endif

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