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 "Kinematics.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  *  @bug the legs try (briefly) to straighten out when first starting to move
00072  */
00073 class WalkMC : public MotionCommand, public LoadSave {
00074 public:
00075   typedef SplinePath<vector3d,double> splinepath; //!<for convenience
00076   typedef HermiteSplineSegment<vector3d,double> spline; //!<for convenience
00077 
00078   //! holds state about each leg's path
00079   struct LegWalkState {
00080     LegWalkState() : airpath(), air(0) {} //!< constructor
00081     spline airpath; //!< the path to follow
00082     bool air; //!< true if in the air
00083   };
00084   
00085   //! holds parameters about how to move each leg
00086   struct LegParam {
00087     LegParam() : neutral(), lift_vel(), down_vel(), lift_time(0), down_time(0) {} //!< constructor
00088     vector3d neutral; //!< defines the "neutral" point of each leg - where it is in midstep
00089     vector3d lift_vel; //!< give the velocities to use when raising the paw
00090     vector3d down_vel; //!< give the velocities to use when lowering the paw
00091     double lift_time; //!< the time (as percentage of WalkParam::period) in the cycle to lift (so you can set different offsets between the paws)
00092     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)
00093   };
00094 
00095   //! holds more general parameters about the walk
00096   struct WalkParam {
00097     WalkParam() : body_height(0), body_angle(0), hop(0), sway(0), period(0), reserved() {} //!< constructor
00098     LegParam leg[4]; //!< a set of LegParam's, one for each leg
00099     double body_height; //!< the height to hold the body (mm)
00100     double body_angle; //!< the angle to hold the body (rad - 0 is level)
00101     double hop;  //!< sinusoidal hop amplitude
00102     double sway; //!< sinusoidal sway in y direction
00103     long period; //!< the time between steps
00104     long reserved; //!< live with it
00105   };
00106 
00107   //! holds information to correct for slippage, non-idealities
00108   struct CalibrationParam {
00109     CalibrationParam();
00110 
00111     //! symbolic way to refer to each of the directions
00112     enum dimension_offset {
00113       forward,
00114       reverse, 
00115       strafe,
00116       rotate,
00117       NUM_DIM
00118     };
00119 
00120     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
00121     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
00122 
00123     float max_accel[NUM_DIM]; //!< maximum achievable acceleration, 0 for infinite (mm/s^2)
00124     float max_vel[NUM_DIM]; //!< maximum achievable velocity (mm/s)
00125   };
00126 
00127   //! constructor
00128   WalkMC(const char* pfile=NULL);
00129 
00130   virtual void DoStart(); //!< sends an activate LocomotionEvent
00131   virtual void DoStop();  //!< sends a deactivate LocomotionEvent
00132 
00133   virtual int updateOutputs(); //!< calculates current positions of the paws
00134   
00135   //! Returns true if we are walking. This modified isDirty allows the AIBO to slow down to a stop rather than stopping immediately.
00136   virtual int isDirty();
00137   
00138   //! always true - never autoprunes
00139   virtual int isAlive() { return true; }
00140 
00141   virtual unsigned int getBinSize() const;
00142   virtual unsigned int LoadBuffer(const char buf[], unsigned int len);
00143   virtual unsigned int SaveBuffer(char buf[], unsigned int len) const;
00144 
00145   //! set the direction to walk - can specify x (forward), y (left), and angular (counterclockwise) velocities
00146   void setTargetVelocity(double dx,double dy,double da);
00147   //! returns current velocity we're trying to go
00148   const vector3d& getTargetVelocity() { return target_vel_xya; }
00149   //! returns the velocity we're actually moving (subject to clipping at max_accel_xya), doesn't reflect value of getPaused()...
00150   const vector3d& getCurVelocity() const { return vel_xya;}
00151   //! returns the time at which we started traveling along the current vector
00152   unsigned int getStartTravelTime() { return travelTime; }
00153   //! returns the amount of time we've been traveling along the current vector
00154   unsigned int getTravelTime() { return get_time()-getStartTravelTime(); }
00155   
00156   void setPaused(bool p) { isPaused=p; } //!< if set to true, will stop moving
00157   bool getPaused() const { return isPaused; } //!< if is true, we aren't moving
00158   void setHeight(double h) { wp.body_height=h; } //!< sets WalkParam::body_height of #wp
00159   double getHeight() { return wp.body_height; } //!< gets WalkParam::body_height of #wp
00160   void setAngle(double a) { wp.body_angle=a; } //!< sets WalkParam::body_angle of #wp
00161   double getAngle() { return wp.body_angle; } //!< gets WalkParam::body_angle of #wp
00162   void setHop(double h) { wp.hop=h; } //!< sets WalkParam::hop of #wp
00163   double getHop() { return wp.hop; } //!< gets WalkParam::hop of #wp
00164   void setSway(double h) { wp.sway=h; } //!< sets WalkParam::sway of #wp
00165   double getSway() { return wp.sway; } //!< gets WalkParam::sway of #wp
00166   void setPeriod(long p) { wp.period=p; } //!< sets WalkParam::period of #wp
00167   long getPeriod() { return wp.period; } //!< gets WalkParam::period of #wp
00168   void setSlowMo(float p) { slowmo=p; } //!< sets slowmo
00169   float* getSlowMo() { return &slowmo; } //!< gets slowmo
00170 
00171   WalkParam & getWP() { return wp; }; //!< returns the current walk parameter structure
00172   CalibrationParam & getCP() { return cp; }; //!< returns the current walk calibration parameter
00173   
00174   //! takes current leg positions from WorldState and tries to match the point in the cycle most like it
00175   void resetLegPos();
00176 
00177   static const float MAX_DX; //!< ==180 mm/sec
00178   static const float MAX_DY; //!< ==140 mm/sec
00179   static const float MAX_DA; //!< ==1.8 rad/sec
00180 // tss "SmoothWalk" modification follows (actually only comment is changed)
00181   // static const vector3d max_accel_xya; //!< vector version of MAX_DX,MAX_DY,MAX_DA
00182   static const vector3d max_accel_xya; //!< maximum acceleration of x, y, and a velocity
00183 
00184  protected:
00185   //! holds current joint commands
00186   OutputCmd cmds[NumOutputs][NumFrames];
00187 
00188   //! converts @a in to calibration parameters and multiplies through the calibration matrix
00189   static void applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out);
00190 
00191  protected:
00192   //! does some setup stuff, calls LoadFile(pfile)
00193   void init(const char* pfile);
00194 
00195   bool isPaused; //!< true if we are paused
00196 
00197   WalkParam wp; //!< current walking parameters (note that it's not static - different WalkMC's can have different setting, handy...
00198   CalibrationParam cp; //!< calibration parameters for current walk.
00199   LegWalkState legw[NumLegs]; //!< current state of each leg
00200   vector3d legpos[NumLegs]; //!< current position of each leg
00201   splinepath body_loc; //!< the path the body goes through while walking (?)
00202   splinepath body_angle; //!< the path the body goes through while walking (?)
00203 
00204   vector3d pos_delta; //!< how much we've moved
00205   double angle_delta; //!< how much we've turned
00206   
00207   unsigned int travelTime; //!< the time of the last call to setTargetVelocity - handy to check the time we've been traveling current vector
00208   int time; //!< time of last call to updateJointCmds() (scaled by slowmo)
00209   int TimeStep; //!< time to pretend passes between each call to updateJointCmds() - usually RobotInfo::FrameTime
00210   float slowmo; //!< scales time values to make the walk move in slow motion for analysis (or fast forward)
00211 
00212   // tss "SmoothWalk" addition follows
00213   /*! The CycleOffset variable is used to ensure that each time the AIBO
00214    *  starts walking, it starts at the same point in the walk cycle as
00215    *  where it stopped. This measure is intended to decrease the amount
00216    *  of jerking (and hence deviation) that occurs when the AIBO starts
00217    *  walking forward and then suddenly stops. */
00218   int CycleOffset;
00219   /*! Each CycleOffset corresponds to a different TimeOffset once the
00220    *  robot starts walking again. Consider this example: the robot
00221    *  stops 2/3 of the way through the cycle, then starts again 1/3
00222    *  of the way through the cycle on the absolute clock. The time
00223    *  offset to advance the clock to the right part of the cycle is
00224    *  1/3 of a cycle, so we set TimeOffset to 1/3 cycle and add that to
00225    *  every clock value used in the walk code. */
00226   int TimeOffset;
00227   /*! Every time we stop, we know we'll have a new CycleOffset, and we'll
00228    *  need to compute a new TimeOffset. This boolean says as much. */
00229   bool NewCycleOffset;
00230 // tss "SmoothWalk" addition ends
00231 
00232   vector3d vel_xya; //!< the current velocity we're moving
00233   vector3d target_vel_xya; //!< the velocity that was requested
00234   vector3d last_target_vel_xya; //!< the velocity that was last sent to motion
00235 };
00236 
00237 /* struct LegState{
00238    long attr,reserved;
00239    point3d pos;
00240    double angles[3];
00241    };
00242    
00243    struct HeadState{
00244    long attr,reserved;
00245    vector3d target;
00246    double angles[3];
00247    };
00248    
00249    struct BodyState{
00250    BodyPosition pos;
00251    LegState leg[4];
00252    HeadState head;
00253    }; 
00254 */
00255 
00256 /*! @file
00257  * @brief Describes WalkMC, a MotionCommand for walking around
00258  * @author CMU RoboSoccer 2001-2002 (Creator)
00259  * @author ejt (ported)
00260  * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
00261  * 
00262  * @verbinclude CMPack_license.txt
00263  *
00264  * $Author: ejt $
00265  * $Name: tekkotsu-2_1 $
00266  * $Revision: 1.21 $
00267  * $State: Exp $
00268  * $Date: 2004/03/03 18:42:16 $
00269  */
00270 
00271 #endif

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