Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
CMPackWalkMC.hGo 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 |