Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
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 |