Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

CMPackWalkMC.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_CMPackWalkMC_h
00033 #define INCLUDED_CMPackWalkMC_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, have none the less hacked around and added stuff on top
00064  *  of it.  So pardon the mess, unless you're feeling ambitious to
00065  *  write your own ;)
00066  *
00067  *  This portion of the code falls under CMPack's license:
00068  *  @verbinclude CMPack_license.txt
00069  */
00070 class CMPackWalkMC : public MotionCommand, public LoadSave {
00071 public:
00072   typedef SplinePath<vector3d,float> splinepath; //!<for convenience
00073   typedef HermiteSplineSegment<vector3d,float> spline; //!<for convenience
00074 
00075   //! holds state about each leg's path
00076   struct LegWalkState {
00077     LegWalkState() : airpath(), air(0), cyc(0) {} //!< constructor
00078     spline airpath; //!< the path to follow
00079     bool air; //!< true if in the air
00080     float cyc; //!< % (0,1) of how far along the path we are
00081   };
00082   
00083   //! holds parameters about how to move each leg
00084   struct LegParam {
00085     LegParam() : neutral(), lift_vel(), down_vel(), lift_time(0), down_time(0) {} //!< constructor
00086     vector3d neutral; //!< defines the "neutral" point of each leg - where it is in midstep
00087     vector3d lift_vel; //!< give the velocities to use when raising the paw
00088     vector3d down_vel; //!< give the velocities to use when lowering the paw
00089     double lift_time; //!< the time (as percentage of WalkParam::period) in the cycle to lift (so you can set different offsets between the paws)
00090     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)
00091   };
00092 
00093   //! holds more general parameters about the walk
00094   struct WalkParam {
00095     WalkParam() : body_height(0), body_angle(0), hop(0), sway(0), period(0), useDiffDrive(0), sag(0), reserved() {} //!< constructor
00096     LegParam leg[4]; //!< a set of LegParam's, one for each leg
00097     double body_height; //!< the height to hold the body (mm)
00098     double body_angle; //!< the angle to hold the body (rad - 0 is level)
00099     double hop;  //!< sinusoidal hop amplitude
00100     double sway; //!< sinusoidal sway in y direction
00101     int period; //!< the time between steps
00102     int useDiffDrive; //!< if non-zero, diff-drive style turning is used instead of rotational turning
00103     float sag; //!< the amount to sagging to account for when a foot is lifted
00104     float reserved; //!< just live with it
00105   };
00106 
00107   //! holds information to correct for slippage, non-idealities
00108   struct CalibrationParam {
00109     CalibrationParam(); //!< constructor, sets calibration matricies to identity
00110 
00111     //! symbolic way to refer to each of the directions
00112     enum dimension_offset {
00113       forward, //!< forward (x)
00114       reverse, //!< backward (-x)
00115       strafe,  //!< sideways (y)
00116       rotate,  //!< spin (z/a)
00117       NUM_DIM  //!< number of directions we calibrate for
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   CMPackWalkMC(const char* pfile=NULL);
00129 
00130   virtual void start(); //!< sends an activate LocomotionEvent
00131   virtual void stop();  //!< sends a deactivate LocomotionEvent
00132 
00133         //! Posts a LocomotionEvent. Also forces an output frame setting wheel velocities to zero; needed because if we remove a motion command there may be nothing left to zero the velocities.
00134         virtual void zeroVelocities();
00135 
00136   virtual int updateOutputs(); //!< calculates current positions of the legs or speeds of the wheels
00137   
00138   //! Returns true if we are walking. This modified isDirty allows the AIBO to slow down to a stop rather than stopping immediately.
00139   virtual int isDirty();
00140   virtual void setDirty() { isPaused = false; }
00141   
00142   //! Will prune if we've taken the requested number of steps.
00143   virtual int isAlive() { return step_count != 0; }
00144 
00145   virtual unsigned int getBinSize() const;
00146   virtual unsigned int loadBuffer(const char buf[], unsigned int len, const char* filename=NULL);
00147   virtual unsigned int saveBuffer(char buf[], unsigned int len) const;
00148   virtual unsigned int loadFile(const char* filename);
00149   virtual unsigned int saveFile(const char* filename) const;
00150 
00151   //! returns current velocity we're trying to go
00152   const vector3d& getTargetVelocity() { return target_vel_xya; }
00153   //! returns the velocity we're actually moving (subject to clipping at max_accel_xya), doesn't reflect value of getPaused()...
00154   const vector3d& getCurVelocity() const { return vel_xya;}
00155   //! returns the time at which we started traveling along the current vector, in milliseconds
00156   unsigned int getStartTravelTime() { return travelTime; }
00157   //! returns the amount of time we've been traveling along the current vector, in milliseconds
00158   unsigned int getTravelTime() { return get_time()-getStartTravelTime(); }
00159 
00160   //! set the direction to walk
00161   /*! @param dx forward velocity (millimeters per second)
00162    *  @param dy left velocity (millimeters per second)
00163    *  @param da counterclockwise velocity (radians per second) */
00164   void setTargetVelocity(double dx,double dy,double da);
00165   
00166   //! set the direction to walk
00167   /*! @param dx forward velocity (millimeters per second)
00168    *  @param dy left velocity (millimeters per second)
00169    *  @param da counterclockwise velocity (radians per second)
00170    *  @param time (seconds) duration to walk before stopping and posting a status event; time will be 
00171    *  rounded to the nearest step time. (This is internally converted to number of steps) */
00172   void setTargetVelocity(double dx,double dy,double da,double time);
00173   
00174   //! recalculates the target velocity so steps are of a given length to achieve the specified displacement in @a n steps
00175   /*! @param dx length of displacement in millimeters along body's x axis
00176    *  @param dy length of displacement in millimeters along body's y axis
00177    *  @param da amount to turn in radians, counter-clockwise viewed from above
00178    *  @param time (seconds) duration in which to do the displacement (thus controls velocity); time will be 
00179    *  rounded to the nearest step time. (This is internally converted to number of steps)
00180    *
00181    *  @see setTargetVelocity() */
00182   void setTargetDisplacement(double dx, double dy, double da, double time);
00183 
00184   void setTargetDisplacement(double dx, double dy, double da, double vx, double vy, double va) {}
00185 
00186   //! returns remaining steps (#step_count) (negative means infinite)
00187   int getRemainingSteps() const { return step_count; }
00188 
00189   
00190   float getStepThreshold() const { return step_threshold; } //!< returns the step threshold -- see #step_threshold
00191   void setStepThreshold(float st) { step_threshold=st; } //!< sets the step threshold -- see #step_threshold
00192   
00193   void setPaused(bool p) { isPaused=p; } //!< if set to true, will stop moving
00194   bool getPaused() const { return isPaused; } //!< if is true, we aren't moving
00195   void setHeight(double h) { wp.body_height=h; } //!< sets WalkParam::body_height of #wp
00196   double getHeight() { return wp.body_height; } //!< gets WalkParam::body_height of #wp
00197   void setAngle(double a) { wp.body_angle=a; } //!< sets WalkParam::body_angle of #wp
00198   double getAngle() { return wp.body_angle; } //!< gets WalkParam::body_angle of #wp
00199   void setHop(double h) { wp.hop=h; } //!< sets WalkParam::hop of #wp
00200   double getHop() { return wp.hop; } //!< gets WalkParam::hop of #wp
00201   void setSway(double h) { wp.sway=h; } //!< sets WalkParam::sway of #wp
00202   double getSway() { return wp.sway; } //!< gets WalkParam::sway of #wp
00203   void setPeriod(long p) { wp.period=p; } //!< sets WalkParam::period of #wp
00204   long getPeriod() { return wp.period; } //!< gets WalkParam::period of #wp
00205   void setSlowMo(float p) { slowmo=p; } //!< sets #slowmo
00206   float* getSlowMo() { return &slowmo; } //!< gets #slowmo
00207 
00208   //!used to specify value for #acc_style
00209   enum AccelerationStyle_t {
00210     CALIBRATION_ACCEL, //!< use the acceleration specified by the calibration parameters
00211     INF_ACCEL,         //!< ignore calibration acceleration (attempt infinite acceleration)
00212     DEFAULT_ACCEL      //!< reference the value of Config::motion_config::walk_acceleration
00213   };
00214   virtual void setAccelerationStyle(AccelerationStyle_t acc) { acc_style=acc; } //!< sets #acc_style
00215   virtual AccelerationStyle_t getAccelerationStyle() const { return acc_style; } //!< returns #acc_style
00216 
00217 #ifdef TGT_HAS_LEGS
00218   //! returns the current leg position of leg @a i
00219   const vector3d& getLegPosition(LegOrder_t i) const { return legpos[i]; }
00220 #endif
00221 
00222   struct WalkParam& getWP() { return wp; }; //!< returns the current walk parameter structure
00223   struct CalibrationParam& getCP() { return cp; }; //!< returns the current walk calibration parameter
00224   
00225   //! takes current leg positions from WorldState and tries to match the point in the cycle most like it
00226   void resetLegPos();
00227 
00228   static const float MAX_DX; //!< maximum forward velocity, for setTargetDisplacement
00229   static const float MAX_DY; //!< maximum sideways velocity, for setTargetDisplacement
00230   static const float MAX_DA; //!< maximum angular velocity, for setTargetDisplacement
00231 #ifdef TGT_HAS_WHEELS
00232   static const float OPTIMAL_DA; //!< optimal angular velocity for Create odometry in setTargetDisplacement
00233 #endif
00234   
00235   float getMaxXVel() const { return MAX_DX; } //!< more portable than directly accessing #MAX_DX
00236   float getMaxYVel() const { return MAX_DY; } //!< more portable than directly accessing #MAX_DY
00237   float getMaxAVel() const { return MAX_DA; } //!< more portable than directly accessing #MAX_DA
00238 
00239   static const vector3d max_accel_xya; //!< maximum acceleration of x, y, and a velocity
00240 
00241  protected:
00242   //! holds current joint commands
00243   OutputCmd cmds[NumOutputs][NumFrames];
00244 
00245   //! converts @a in to calibration parameters and multiplies through the calibration matrix
00246   static void applyCalibration(const float mat[3][11], const vector3d& in, vector3d& out);
00247 
00248  protected:
00249   //! does some setup stuff, calls loadFile(pfile)
00250   void init(const char* pfile);
00251 
00252   bool isPaused; //!< true if we are paused
00253   bool dirty;  //!< Ignored; needed to make WaypointWalkMC::setDirty() work
00254 
00255   WalkParam wp; //!< current walking parameters (note that it's not static - different CMPackWalkMC's can have different setting, handy...
00256   CalibrationParam cp; //!< calibration parameters for current walk.
00257   LegWalkState legw[NumLegs]; //!< current state of each leg
00258   vector3d legpos[NumLegs]; //!< current position of each leg
00259   splinepath body_loc; //!< the path the body goes through while walking (?)
00260   splinepath body_angle; //!< the path the body goes through while walking (?)
00261   vector3d liftPos[NumLegs]; //!< position each of the feet was last lifted
00262   vector3d downPos[NumLegs]; //!< position each of the feet is next going to be set down
00263   
00264   AccelerationStyle_t acc_style; //!< lets you switch between finite or infinite acceleration models
00265 
00266   int step_count; //!< number of steps to take before stopping; if negative, walk forever.
00267   float step_threshold; //!< point in a leg's cycle where the step counter should be decremented; 0 - leg lift, .25 - mid-air, .5 - leg down, .75 - mid-ground
00268 
00269   double last_cycle; //!< where the walk is in it its cycle, updated at the end of each call to updateOutputs()
00270 
00271   vector3d pos_delta; //!< how much we've moved
00272   float angle_delta; //!< how much we've turned
00273   
00274   unsigned int travelTime; //!< the time of the last call to setTargetVelocity - handy to check the time we've been traveling current vector
00275   int time; //!< time of last call to updateOutputs() (scaled by slowmo)
00276   int TimeStep; //!< time to pretend passes between each call to updateOutputs() - usually RobotInfo::FrameTime
00277   float slowmo; //!< scales time values to make the walk move in slow motion for analysis (or fast forward)
00278 
00279   // tss "SmoothWalk" addition follows
00280   /*! The CycleOffset variable is used to ensure that each time the AIBO
00281    *  starts walking, it starts at the same point in the walk cycle as
00282    *  where it stopped. This measure is intended to decrease the amount
00283    *  of jerking (and hence deviation) that occurs when the AIBO starts
00284    *  walking forward and then suddenly stops. */
00285   int CycleOffset;
00286   /*! Each CycleOffset corresponds to a different TimeOffset once the
00287    *  robot starts walking again. Consider this example: the robot
00288    *  stops 2/3 of the way through the cycle, then starts again 1/3
00289    *  of the way through the cycle on the absolute clock. The time
00290    *  offset to advance the clock to the right part of the cycle is
00291    *  1/3 of a cycle, so we set TimeOffset to 1/3 cycle and add that to
00292    *  every clock value used in the walk code. */
00293   int TimeOffset;
00294   /*! Every time we stop, we know we'll have a new CycleOffset, and we'll
00295    *  need to compute a new TimeOffset. This boolean says as much. */
00296   bool NewCycleOffset;
00297 // tss "SmoothWalk" addition ends
00298 
00299   vector3d target_disp_xya; //!< requested displacement
00300   vector3d vel_xya; //!< the current velocity we're moving
00301   vector3d target_vel_xya; //!< the velocity that was requested
00302   vector3d last_target_vel_xya; //!< the velocity that was last sent to motion
00303 };
00304 
00305 /* struct LegState{
00306    long attr,reserved;
00307    point3d pos;
00308    double angles[3];
00309    };
00310    
00311    struct HeadState{
00312    long attr,reserved;
00313    vector3d target;
00314    double angles[3];
00315    };
00316    
00317    struct BodyState{
00318    BodyPosition pos;
00319    LegState leg[4];
00320    HeadState head;
00321    }; 
00322 */
00323 
00324 /*! @file
00325  * @brief Describes CMPackWalkMC, a MotionCommand for walking around
00326  * @author CMU RoboSoccer 2001-2002 (Creator)
00327  * @author ejt (ported)
00328  * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
00329  * 
00330  * @verbinclude CMPack_license.txt
00331  */
00332 
00333 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:37 2016 by Doxygen 1.6.3