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

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