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

WalkMotionModel.cc

Go to the documentation of this file.
00001 /*
00002  * Walking motion model for the AIBO using walk routines from RoboSoccer.
00003  */
00004 
00005 /* This extern c thingy, usually reserved for header files, is here too so
00006  * that it's easy to compile unit test code for the C++ programs. Yep. */
00007 #ifdef __cplusplus
00008 extern "C" {
00009 #endif
00010 
00011 #include <stdlib.h>
00012 #include <math.h>
00013 
00014 /* The motion model routine itself. See the header file for more details */
00015 void WalkMotionModel(double *x, double *y, double *theta,
00016          double dx, double dy, double da, unsigned int time)
00017 {
00018   double x_motion, y_motion, theta_motion, arc_length;
00019   double xpos_before_angles, ypos_before_angles;
00020   double chord_length, total_motion_angle;
00021 
00022   /* If there was no motion at all, just don't do anything. */
00023   if((dx == 0) && (dy == 0) && (da == 0)) return;
00024 
00025   /* Integrate dx, dy, and da */
00026   x_motion = dx * ((double) time)/1000.0;
00027   y_motion = dy * ((double) time)/1000.0;
00028   theta_motion = da * ((double) time)/1000.0;
00029 
00030   /* Compute linear distance */
00031   arc_length = sqrt(x_motion*x_motion + y_motion*y_motion);
00032 
00033   /* Compute relative motion along arc. This part pretends we start out going
00034    * straight forward, even though our dy might mean we're not. Don't worry,
00035    * it all gets sorted out. */
00036   if(fabs(theta_motion) < 0.000001) { /* avoids divide by zero */
00037     xpos_before_angles = arc_length;
00038     ypos_before_angles = 0;
00039   }
00040   else if(theta_motion < 0) {
00041     double hypotenuse = arc_length/theta_motion;
00042     xpos_before_angles = sin(theta_motion)*hypotenuse;
00043     ypos_before_angles = hypotenuse - cos(theta_motion)*hypotenuse;
00044   }
00045   else { /* theta_motion > 0 -- note negative sign */
00046     double hypotenuse = arc_length/theta_motion;
00047     xpos_before_angles = sin(theta_motion)*hypotenuse;
00048     ypos_before_angles = -(hypotenuse - cos(theta_motion)*hypotenuse);
00049   }
00050 
00051   /* Find the length of the chord between our current position and our
00052    * new position along the arc. */
00053   chord_length = sqrt(xpos_before_angles*xpos_before_angles +
00054           ypos_before_angles*ypos_before_angles);
00055 
00056   /* At last we construct the final position of the robot */
00057   total_motion_angle =  atan2(y_motion, x_motion) +
00058       atan2(ypos_before_angles, xpos_before_angles) + *theta;
00059 
00060   /* We place the new position in the particle */
00061   *x += cos(total_motion_angle) * chord_length;
00062   *y += sin(total_motion_angle) * chord_length;
00063   *theta += theta_motion;
00064 
00065   /* Make sure that the angle is in [0, 2pi). */
00066   while(*theta >= 2*M_PI) *theta -= 2*M_PI;
00067   while(*theta < 0)       *theta += 2*M_PI;
00068 
00069   /* We are finished */
00070 }
00071 
00072 #ifdef __cplusplus
00073 }
00074 #endif

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