Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

afsMotionResample.cc

Go to the documentation of this file.
00001 /*
00002  * Motion resampling code for AIBO FastSLAM
00003  */
00004 
00005 #ifdef __cplusplus
00006 extern "C" {
00007 #endif
00008 
00009 #include "afsMotionResample.h"
00010 #include "afsParticle.h"
00011 #include "afsUtility.h"
00012 #include "Configuration.h"
00013 
00014 /* Needed for AIBO physical parameters */
00015 #include "../Maps/Configuration.h"
00016 
00017 #include <stdlib.h>
00018 #include <math.h>
00019 
00020 #ifdef UNIT_TEST
00021 #define USE_OWN_MOTION_MODEL
00022 #include <stdio.h>
00023 #include <time.h>
00024 #endif
00025 
00026 #ifndef USE_OWN_MOTION_MODEL
00027 #include "../Common/WalkMotionModel.h"
00028 #endif
00029 
00030 /* The resampling routine itself. See the header file for more details */
00031 /* This routine has its own motion model included so that AIBO FastSLAM
00032  * can be used on a standalone basis. In the context of the WorldModel2
00033  * project, it's best to use the common motion model used by the entire
00034  * codebase. Define USE_OWN_MOTION_MODEL if you'd like to use the local
00035  * motion model--and beware of it falling out of date! */
00036 void afsMotionResample(afsParticle *p,
00037       double dx, double dy, double da, unsigned int time)
00038 {
00039 #ifdef USE_OWN_MOTION_MODEL
00040   double x_motion, y_motion, theta_motion, arc_length;
00041   double xpos_before_angles, ypos_before_angles;
00042   double chord_length, total_motion_angle;
00043 #endif
00044 
00045   /* If there was no motion at all, just don't do anything. */
00046   if((dx == 0) && (dy == 0) && (da == 0)) return;
00047 
00048   /* There was motion. Since the robot's motion is highly unpredictable,
00049    * we add Gaussian noise to the motion commands. This approach has
00050    * drawbacks. Most of the rotational error on the AIBO comes from the
00051    * robot's very first and very last motions, when balancing while
00052    * starting and stopping can tip the robot in strange ways. On long
00053    * treks, the perturbation added here can result in large deviations,
00054    * which don't quite match the real behavior of the robot. */
00055   dx += normRand() * AFS_PERTURB_DX_VARIANCE + AFS_PERTURB_DX_MEAN;
00056   dy += normRand() * AFS_PERTURB_DY_VARIANCE + AFS_PERTURB_DY_MEAN;
00057   da += normRand() * AFS_PERTURB_DA_VARIANCE + AFS_PERTURB_DA_MEAN;
00058 
00059   /* The motion model code used here predicts the location of the
00060    * AIBO's heading pivot point, NOT the location of the AIBO's chest,
00061    * which is where all the other FastSLAM code tracks the AIBO's
00062    * location. This makes the math easier for the motion model but means
00063    * also that we'll have to convert the AIBO pose to pivot centric
00064    * coordinates before invoking it. Later, at the end of this routine,
00065    * we'll convert back. */
00066   p->pose.x -= AIBO_TURN_PIVOT_DIST * cos(p->pose.theta);
00067   p->pose.y -= AIBO_TURN_PIVOT_DIST * sin(p->pose.theta);
00068 
00069 #ifndef USE_OWN_MOTION_MODEL
00070 
00071   WalkMotionModel(&p->pose.x, &p->pose.y, &p->pose.theta, dx, dy, da, time);
00072 
00073 #else /* not USE_OWN_MOTION_MODEL */
00074 #warning "Using innate (and possibly obsolete) walking motion model!"
00075 
00076   /* Integrate dx, dy, and da */
00077   x_motion = dx * ((double) time)/1000.0;
00078   y_motion = dy * ((double) time)/1000.0;
00079   theta_motion = da * ((double) time)/1000.0;
00080 
00081   /* Compute linear distance */
00082   arc_length = sqrt(x_motion*x_motion + y_motion*y_motion);
00083 
00084   /* Compute relative motion along arc. This part pretends we start out going
00085    * straight forward, even though our dy might mean we're not. Don't worry,
00086    * it all gets sorted out. */
00087   if(fabs(theta_motion) < 0.000001) { /* avoids divide by zero */
00088     xpos_before_angles = arc_length;
00089     ypos_before_angles = 0;
00090   }
00091   else if(theta_motion < 0) {
00092     double hypotenuse = arc_length/theta_motion;
00093     xpos_before_angles = sin(theta_motion)*hypotenuse;
00094     ypos_before_angles = hypotenuse - cos(theta_motion)*hypotenuse;
00095   }
00096   else { /* theta_motion > 0 -- note negative sign */
00097     double hypotenuse = arc_length/theta_motion;
00098     xpos_before_angles = sin(theta_motion)*hypotenuse;
00099     ypos_before_angles = -(hypotenuse - cos(theta_motion)*hypotenuse);
00100   }
00101 
00102   /* Find the length of the chord between our current position and our
00103    * new position along the arc. */
00104   chord_length = sqrt(xpos_before_angles*xpos_before_angles +
00105           ypos_before_angles*ypos_before_angles);
00106 
00107   /* At last we construct the final position of the robot */
00108   total_motion_angle =  atan2(y_motion, x_motion) +
00109       atan2(ypos_before_angles, xpos_before_angles) +
00110       p->pose.theta;
00111 
00112   /* We place the new position in the particle */
00113   p->pose.x += cos(total_motion_angle) * chord_length;
00114   p->pose.y += sin(total_motion_angle) * chord_length;
00115   p->pose.theta += theta_motion;
00116 
00117   /* Make sure that the angle is in [0, 2pi). */
00118   while(p->pose.theta >= 2*M_PI) p->pose.theta -= 2*M_PI;
00119   while(p->pose.theta < 0)       p->pose.theta += 2*M_PI;
00120 
00121 #endif /* USE_OWN_MOTION_MODEL */
00122 
00123   /* Here we restore pose coordinates from the turn pivot point to the
00124    * neck point. */
00125   p->pose.x += AIBO_TURN_PIVOT_DIST * cos(p->pose.theta);
00126   p->pose.y += AIBO_TURN_PIVOT_DIST * sin(p->pose.theta);
00127 
00128   /* We are finished */
00129 }
00130 
00131 #ifdef UNIT_TEST
00132 
00133 int main(int argc, char **argv)
00134 {
00135   afsParticle p;
00136   int i;
00137 
00138   afsParticleInit(&p);
00139   srand(time(NULL));
00140 
00141   puts("hold off");
00142   puts("plot(0,0,'bx')");
00143   puts("hold on");
00144   puts("axis equal");
00145   for(i=0; i<10; ++i) {
00146     afsMotionResample(&p, 70, 0, 0, 1000);
00147     printf("plot (%f, %f, 'bx')\n", p.pose.x, p.pose.y);
00148   }
00149   for(i=0; i<10; ++i) {
00150     afsMotionResample(&p, -70, 0, 0, 1000);
00151     printf("plot (%f, %f, 'rx')\n", p.pose.x, p.pose.y);
00152   }
00153   for(i=0; i<10; ++i) {
00154     afsMotionResample(&p, 0, 30, 0, 1000);
00155     printf("plot (%f, %f, 'b*')\n", p.pose.x, p.pose.y);
00156   }
00157   for(i=0; i<10; ++i) {
00158     afsMotionResample(&p, 0, -30, 0, 1000);
00159     printf("plot (%f, %f, 'r*')\n", p.pose.x, p.pose.y);
00160   }
00161   for(i=0; i<20; ++i) {
00162     afsMotionResample(&p, 0, 30, 0.3, 1000);
00163     printf("plot (%f, %f, 'gx')\n", p.pose.x, p.pose.y);
00164   }
00165 
00166   return 0;
00167 }
00168 
00169 #endif
00170 
00171 #ifdef __cplusplus
00172 }
00173 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2