# HolonomicMotionModel.h

Go 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 "Localization/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 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_type particle_type;
00057   typedef typename ParticleFilter<ParticleT>::MotionModel::particle_collection particle_collection;
00058   typedef typename particle_collection::size_type index_t;
00059
00060   //! constructor, with default noise parameters (xvar=yvar=50, avar=0.15f)
00061   HolonomicMotionModel()
00062     : ParticleFilter<ParticleT>::MotionModel(),
00063     xvel(0), yvel(0), avel(0), prevtime(get_time()), posx (0), posy(0), posa(0),
00064     xvar(.25f), yvar(.25f), avar(.25f), crossAxis(.05f), crossAngle(.001f) {}
00065
00066   //! constructor, with noise parameters (pass 0's to make it an "ideal" motion model)
00067   /*! Variance parameters only come into play with updateMotion(), which is called on
00068    *  collections of particles.  The other functions all return "ideal" motion calculations. */
00069   HolonomicMotionModel(float xVariance, float yVariance, float aVariance)
00070     : ParticleFilter<ParticleT>::MotionModel(),
00071     xvel(0), yvel(0), avel(0), prevtime(get_time()), posx (0), posy(0), posa(0),
00072     xvar(xVariance), yvar(yVariance), avar(aVariance),
00073     crossAxis((xVariance+yVariance)/10), crossAngle(.001f)  {}
00074
00075   //! called by the particle filter when the current position of each particle should be updated
00076   /*! This will reset the motion model to set the origin at the current location after the particles
00077    *  are updated, so that the next call to updateMotion() will supply the particles with the
00078    *  displacement which occurred since the last update */
00079   virtual void updateMotion(particle_collection& particles, particle_type& estimate) {
00080     unsigned int curt = get_time();
00081     if(curt==prevtime)
00082       return; // no time has passed!
00083     float dt = (curt - prevtime)/1000.f;
00084     prevtime=curt;
00085     // update estimate using zero noise
00086     posx=posy=posa=0;
00087     computeHolonomicMotion(xvel,yvel,avel,dt, posx,posy,posa);
00088     float ec = std::cos(estimate.theta);
00089     float es = std::sin(estimate.theta);
00090     estimate.x += posx*ec - posy*es;
00091     estimate.y += posx*es + posy*ec;
00092     estimate.theta += posa;
00093
00094     // now update the particles
00095     if(xvar==0 && yvar==0 && avar==0) {
00096       // if we're using a noiseless motion model, can be a bit faster and avoid all the random number generation
00097       for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it) {
00098   float c = std::cos(it->theta);
00099   float s = std::sin(it->theta);
00100   it->x += posx*c - posy*s;
00101   it->y += posx*s + posy*c;
00102   it->theta += posa;
00103       }
00104     } else {
00105       // otherwise have to do the noise generation too...
00106       for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it) {
00107   posx=posy=posa=0;
00108   // this factor normalizes across update rates
00109   // (integrating many small updates otherwise yields lower variance in position than fewer large updates...)
00110   float norm=1/std::sqrt(dt);
00111   float xv=xvel*(1+(float)DRanNormalZig32()*xvar*norm) + (yvel*(float)DRanNormalZig32()*crossAxis*norm);
00112   float yv=yvel*(1+(float)DRanNormalZig32()*yvar*norm) + (xvel*(float)DRanNormalZig32()*crossAxis*norm);
00113   float av=avel*(1+(float)DRanNormalZig32()*avar*norm) + ((xvel+yvel)*(float)DRanNormalZig32()*crossAngle*norm);
00114   computeHolonomicMotion(xv,yv,av,dt, posx,posy,posa);
00115   float c = std::cos(it->theta);
00116   float s = std::sin(it->theta);
00117   it->x += posx*c - posy*s;
00118   it->y += posx*s + posy*c;
00119   it->theta += posa;
00120       }
00121     }
00122   }
00123
00124   //! stores the current position into the arguments (based on get_time() vs the time the position was last set)
00125   void getPosition(float& outx, float& outy, float& outa) const {
00126     outx=posx;
00127     outy=posy;
00128     outa=posa;
00129     float dt = (get_time() - prevtime)/1000.f;
00130     computeHolonomicMotion(xvel,yvel,avel,dt, outx,outy,outa);
00131   }
00132
00133 //! stores the current position into the arguments (based on curtime vs the time the position was last set)
00134 void getPosition(float& outx, float& outy, float& outa, unsigned int curtime) const {
00135   // store position of last velocity change:
00136   outx=posx;
00137   outy=posy;
00138   outa=posa;
00139   // how much time has passed since then?
00140   float dt = (curtime - prevtime)/1000.f;
00141   // compute current position along path
00142   computeHolonomicMotion(xvel,yvel,avel,dt, outx,outy,outa);
00143 }
00144
00145 //! sets the current position to the specified values and updates the timestamp to the current time
00146 void setPosition(float x, float y, float angle) { setPosition(x,y,angle,get_time()); }
00147
00148 //! sets the current position to the specified values and updates the timestamp to the specified time
00149 void setPosition(float x, float y, float angle, unsigned int curtime) {
00150   posx = x;
00151   posy = y;
00152   posa = angle;
00153   prevtime = curtime;
00154 }
00155
00156 //! stores the current velocity into the arguments (no noise is added, this just echos the values passed to setVelocity())
00157 void getVelocity(float& outxv, float& outyv, float& outav) const {
00158   outxv=xvel;
00159   outyv=yvel;
00160   outav=avel;
00161 }
00162
00163 //! sets the current velocity to the specified values and updates the position and timestamp to the current time
00164 void setVelocity(float xv, float yv, float av) { setVelocity(xv,yv,av,get_time()); }
00165
00166 //! sets the current velocity to the specified values and updates the position and timestamp to the specified time
00167 void setVelocity(float xv, float yv, float av, unsigned int curtime) {
00168   //std::cerr << "setVelocity("<<xv<<','<<yv<<','<<av<<','<<curtime<<')'<<std::endl;
00169   // first update current position
00170   float dt = (curtime - prevtime)/1000.f;
00171   computeHolonomicMotion(xvel,yvel,avel,dt, posx,posy,posa);
00172   // now store specified velocity
00173   xvel = xv; // forward velocity
00174   yvel = yv; // sideways speed
00175   avel = av; // turning speed
00176   prevtime = curtime;  // time of last event
00177
00178   //cout << "Posx: " << posx << " Posy: " << posy << " Posangle: " << posa << endl;
00179   //cout << "PrevX: " << xvel << endl;
00180   //cout << "PrevY: " << yvel << endl;
00181   //cout << "PrevTime: " << prevtime << endl;
00182 }
00183
00184 //! allows you to change the variance parameters (#xvar, #yvar, #avar)
00185 void setVariance(float xv, float yv, float av) {
00186   xvar=xv; yvar=yv; avar=av;
00187 }
00188 //! allows you to change the cross-variance parameters (#crossAxis, #crossAngle)
00189 void setCrossVariance(float axis, float angle) {
00190   crossAxis=axis;
00191   crossAngle=angle;
00192 }
00193
00194 float getXVariance() const { return xvar; } //!< accessor for #xvar
00195 float getYVariance() const { return yvar; } //!< accessor for #yvar
00196 float getAVariance() const { return avar; } //!< accessor for #avar
00197 float getAxisCrossVariance() const { return crossAxis; } //!< accessor for crossAxis
00198 float getAngleCrossVariance() const { return crossAngle; } //!< accessor for crossAngle
00199
00200 protected:
00201 float xvel; //!< current x velocity
00202 float yvel; //!< current y velocity
00203 float avel; //!< current angular velocity
00204 unsigned int prevtime; //!< time (in milliseconds) that the position was last set
00205
00206 float posx; //!< x position at which #prevtime was set
00207 float posy; //!< y position at which #prevtime was set
00208 float posa; //!< orientation at which #prevtime was set
00209
00210 float xvar; //!< variance of x velocities as ratio of x speed, used when updating particle list (updateMotion())
00211 float yvar; //!< variance of y velocities as ratio of y speed, used when updating particle list (updateMotion())
00212 float avar; //!< variance of angular velocities as ratio of angular speed, used when updating particle list (updateMotion())
00213 float crossAxis; //!< cross variance of x speed on y speed and vice versa
00214 float crossAngle; //!< cross variance of x,y speed on angular speed
00215 };
00216
00217 /*! @file
00218  * @brief Defines HolonomicMotionModel, which can model the path of a holonomic robot
00219  * @author Ethan Tira-Thompson (ejt) (Creator)
00220  */
00221
00222 #endif


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