Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

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 "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