Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
HolonomicMotionModel.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_HolonomicMotionModel_h_ 00003 #define INCLUDED_HolonomicMotionModel_h_ 00004 00005 #include "Shared/ParticleFilter.h" 00006 #include "Shared/LocalizationParticle.h" 00007 #include "Shared/get_time.h" 00008 #include "Shared/zignor.h" 00009 #include <cmath> 00010 00011 //! the main function -- to avoid numeric issues, treats paths that would result in a radius over 1e6 long as a straight line 00012 /*! see HolonomicMotionModel class notes for more information on the math involved */ 00013 void computeHolonomicMotion(float xvel, float yvel, float avel, float time, float& xpos, float& ypos, float& apos); 00014 00015 //! This class can model the path of a holonomic robot in two dimensions (x and y) 00016 /*! This class can also model a non-holonomic robot, just don't use the y parameter! 00017 * 00018 * The static function computeMotion() does the main computational grunt work. 00019 * <table cellspacing=0 cellpadding=0 width="434" class="figures" align="center" border="0"><tr> 00020 * <td class="figure"><img src="MotionModel.png"><div style="padding:10px;"> 00021 * Illustration of the equations used. Constant angular and linear velocities result in a 00022 * circular arc as shown. The radius of the arc is directly determined by the ratio of the 00023 * linear speed to the angular speed. The final position after time @f$ \Delta t @f$ will be 00024 * @f$ \delta @f$ along a bearing @f$ \theta/2 @f$, where @f$ \theta @f$ is the angle which has 00025 * been turned during the interval of interest. 00026 * </div></td></tr></table> 00027 * 00028 * You can call setPosition() to initialize the starting location or if the robot has been moved 00029 * by some external means. You should call setVelocity() whenever the robot's velocity changes. 00030 * The class will internally store the position and time at which the velocity change happened 00031 * so that later calls to getPosition() will return points along the new path. 00032 * 00033 * This class can be used as a motion model in the ParticleFilter class, or independently if 00034 * desired for general purpose motion estimation. (see the standalone computeHolonomicMotion()) 00035 * 00036 * However, if you are looking for a motion model for ParticleFiltering, it may be more convenient 00037 * to use DeadReckoningBehavior (found in Behaviors/Services) because you can start the 00038 * behavior and it will subscribe to automatically receive LocomotionEvents from then on. 00039 * If using HolonomicMotionModel directly with the particle filter, you 00040 * would need to call setVelocity() yourself anytime the robot changes direction. 00041 * 00042 * Variance parameters only come into play with updateMotion(), which is called on 00043 * collections of particles. The other functions all return "ideal" motion calculations. 00044 * Be aware that when using in a particle filter, the position is reset on particle updates 00045 * (updateMotion()). In other words, the position returned by motion model is the 00046 * offset achieved since the last particle update, @e not the position relative to 00047 * world's origin, nor any other fixed point. 00048 * 00049 * Caveat: acceleration is not handled by this model. That would be a nice addition... 00050 * 00051 * @see computeHolonomicMotion() 00052 */ 00053 template<typename ParticleT> 00054 class HolonomicMotionModel : public ParticleFilter<ParticleT>::MotionModel { 00055 public: 00056 typedef typename ParticleFilter<ParticleT>::MotionModel::particle_collection particle_collection; 00057 00058 //! constructor, with default noise parameters (xvar=yvar=50, avar=0.15f) 00059 HolonomicMotionModel() 00060 : ParticleFilter<ParticleT>::MotionModel(), 00061 xvel(0), yvel(0), avel(0), prevtime(get_time()), posx (0), posy(0), posa(0), 00062 xvar(.25), yvar(.25), avar(.25), crossAxis(.05f), crossAngle(.001f) {} 00063 00064 //! constructor, with noise parameters (pass 0's to make it an "ideal" motion model) 00065 /*! Variance parameters only come into play with updateMotion(), which is called on 00066 * collections of particles. The other functions all return "ideal" motion calculations. */ 00067 HolonomicMotionModel(float xVariance, float yVariance, float aVariance) 00068 : ParticleFilter<ParticleT>::MotionModel(), 00069 xvel(0), yvel(0), avel(0), prevtime(get_time()), posx (0), posy(0), posa(0), 00070 xvar(xVariance), yvar(yVariance), avar(aVariance), 00071 crossAxis((xVariance+yVariance)/10), crossAngle(.001f) {} 00072 00073 //! called by the particle filter when the current position of each particle should be updated 00074 /*! This will reset the motion model to set the origin at the current location after the particles 00075 * are updated, so that the next call to updateMotion() will supply the particles with the 00076 * displacement which occurred since the last update */ 00077 virtual void updateMotion(particle_collection& particles) { 00078 unsigned int curt = get_time(); 00079 if(curt==prevtime) 00080 return; // no time has passed! 00081 float dt = (curt - prevtime)/1000.f; 00082 if(xvar==0 && yvar==0 && avar==0) { 00083 // if we're using a noiseless motion model, can be a bit faster and avoid all the random number generation 00084 computeHolonomicMotion(xvel,yvel,avel,dt, posx,posy,posa); 00085 for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it) { 00086 float c = std::cos(it->theta); 00087 float s = std::sin(it->theta); 00088 it->x += posx*c - posy*s; 00089 it->y += posx*s + posy*c; 00090 it->theta += posa; 00091 } 00092 } else { 00093 // otherwise have to do the noise generation too... 00094 for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it) { 00095 posx=posy=posa=0; 00096 // this factor normalizes across update rates 00097 // (integrating many small updates otherwise yields lower variance in position than fewer large updates...) 00098 float norm=1/std::sqrt(dt); 00099 float xv=xvel*(1+DRanNormalZig32()*xvar*norm) + (yvel*DRanNormalZig32()*crossAxis*norm); 00100 float yv=yvel*(1+DRanNormalZig32()*yvar*norm) + (xvel*DRanNormalZig32()*crossAxis*norm); 00101 float av=avel*(1+DRanNormalZig32()*avar*norm) + ((xvel+yvel)*DRanNormalZig32()*crossAngle*norm); 00102 computeHolonomicMotion(xv,yv,av,dt, posx,posy,posa); 00103 float c = std::cos(it->theta); 00104 float s = std::sin(it->theta); 00105 it->x += posx*c - posy*s; 00106 it->y += posx*s + posy*c; 00107 it->theta += posa; 00108 } 00109 } 00110 posx=posy=posa=0; 00111 prevtime=curt; 00112 } 00113 00114 //! stores the current position into the arguments (based on get_time() vs the time the position was last set) 00115 void getPosition(float& outx, float& outy, float& outa) const { 00116 outx=posx; 00117 outy=posy; 00118 outa=posa; 00119 float dt = (get_time() - prevtime)/1000.f; 00120 computeHolonomicMotion(xvel,yvel,avel,dt, outx,outy,outa); 00121 } 00122 00123 //! stores the current position into the arguments (based on curtime vs the time the position was last set) 00124 void getPosition(float& outx, float& outy, float& outa, unsigned int curtime) const { 00125 // store position of last velocity change: 00126 outx=posx; 00127 outy=posy; 00128 outa=posa; 00129 // how much time has passed since then? 00130 float dt = (curtime - prevtime)/1000.f; 00131 // compute current position along path 00132 computeHolonomicMotion(xvel,yvel,avel,dt, outx,outy,outa); 00133 } 00134 00135 //! sets the current position to the specified values and updates the timestamp to the current time 00136 void setPosition(float x, float y, float angle) { setPosition(x,y,angle,get_time()); } 00137 00138 //! sets the current position to the specified values and updates the timestamp to the specified time 00139 void setPosition(float x, float y, float angle, unsigned int curtime) { 00140 posx = x; 00141 posy = y; 00142 posa = angle; 00143 prevtime = curtime; 00144 } 00145 00146 //! stores the current velocity into the arguments (no noise is added, this just echos the values passed to setVelocity()) 00147 void getVelocity(float& outxv, float& outyv, float& outav) const { 00148 outxv=xvel; 00149 outyv=yvel; 00150 outav=avel; 00151 } 00152 00153 //! sets the current velocity to the specified values and updates the position and timestamp to the current time 00154 void setVelocity(float xv, float yv, float av) { setVelocity(xv,yv,av,get_time()); } 00155 00156 //! sets the current velocity to the specified values and updates the position and timestamp to the specified time 00157 void setVelocity(float xv, float yv, float av, unsigned int curtime) { 00158 //std::cerr << "setVelocity("<<xv<<','<<yv<<','<<av<<','<<curtime<<')'<<std::endl; 00159 // first update current position 00160 float dt = (curtime - prevtime)/1000.f; 00161 computeHolonomicMotion(xvel,yvel,avel,dt, posx,posy,posa); 00162 // now store specified velocity 00163 xvel = xv; // forward velocity 00164 yvel = yv; // sideways speed 00165 avel = av; // turning speed 00166 prevtime = curtime; // time of last event 00167 00168 //cout << "Posx: " << posx << " Posy: " << posy << " Posangle: " << posa << endl; 00169 //cout << "PrevX: " << xvel << endl; 00170 //cout << "PrevY: " << yvel << endl; 00171 //cout << "PrevTime: " << prevtime << endl; 00172 } 00173 00174 //! allows you to change the variance parameters (#xvar, #yvar, #avar) 00175 void setVariance(float xv, float yv, float av) { 00176 xvar=xv; yvar=yv; avar=av; 00177 } 00178 //! allows you to change the cross-variance parameters (#crossAxis, #crossAngle) 00179 void setCrossVariance(float axis, float angle) { 00180 crossAxis=axis; 00181 crossAngle=angle; 00182 } 00183 00184 float getXVariance() const { return xvar; } //!< accessor for #xvar 00185 float getYVariance() const { return yvar; } //!< accessor for #yvar 00186 float getAVariance() const { return avar; } //!< accessor for #avar 00187 float getAxisCrossVariance() const { return crossAxis; } //!< accessor for crossAxis 00188 float getAngleCrossVariance() const { return crossAngle; } //!< accessor for crossAngle 00189 00190 protected: 00191 float xvel; //!< current x velocity 00192 float yvel; //!< current y velocity 00193 float avel; //!< current angular velocity 00194 unsigned int prevtime; //!< time (in milliseconds) that the position was last set 00195 00196 float posx; //!< x position at which #prevtime was set 00197 float posy; //!< y position at which #prevtime was set 00198 float posa; //!< orientation at which #prevtime was set 00199 00200 float xvar; //!< variance of x velocities as ratio of x speed, used when updating particle list (updateMotion()) 00201 float yvar; //!< variance of y velocities as ratio of y speed, used when updating particle list (updateMotion()) 00202 float avar; //!< variance of angular velocities as ratio of angular speed, used when updating particle list (updateMotion()) 00203 float crossAxis; //!< cross variance of x speed on y speed and vice versa 00204 float crossAngle; //!< cross variance of x,y speed on angular speed 00205 }; 00206 00207 /*! @file 00208 * @brief Defines HolonomicMotionModel, which can model the path of a holonomic robot 00209 * @author Ethan Tira-Thompson (ejt) (Creator) 00210 * 00211 * $Author: ejt $ 00212 * $Name: tekkotsu-4_0 $ 00213 * $Revision: 1.5 $ 00214 * $State: Exp $ 00215 * $Date: 2007/11/10 22:58:09 $ 00216 */ 00217 00218 #endif |
Tekkotsu v4.0 |
Generated Thu Nov 22 00:54:53 2007 by Doxygen 1.5.4 |