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

motionReshapeKludge.h

Go to the documentation of this file.
00001 /*
00002  * Inserted into code with the motion parameters dx, dy, da, and time,
00003  * this routine changes them to reflect actual motion data gathered
00004  * from the AIBO. A kludge.
00005  */
00006 /*
00007    Forward motion regression data for forward motion at 180:
00008 
00009 b = -32.0347
00010     162.5885
00011 
00012 bint = -50.9954  -13.0740
00013        160.2251  164.9518
00014 
00015 
00016    Left turn motion regression data for left turns at 1.008 (or 1.0236)
00017 
00018 b = -28.3579
00019      69.3368
00020 
00021 bint = -32.3540  -24.3618
00022         67.5766   71.0971
00023 
00024 
00025     Right turn motion regression data for right turns at 1.008
00026 
00027 b = -34.7600
00028      68.4120
00029 
00030 bint = -39.0989  -30.4211
00031         66.5712   70.2528
00032 
00033  */
00034 
00035   /* FIXME -- needs to not be tacked on -- FIXME */
00036 
00037   /* Based on the above regression data, we tune the AIBO walk parameters to
00038    * be more in accordance with actual motion. The data are too limited to
00039    * model even a narrow range of the AIBO's walking (indeed, sidestepping,
00040    * which is hideously inaccurate, is not even modeled at all! We try to
00041    * reason about the full range of AIBO's motion below, but for motion
00042    * parameters outside of those tested above, everything is purely
00043    * speculative. Let the pondering begin!
00044    *
00045    * One thing that is certain is that the AIBO's walk doesn't move the
00046    * dog anywhere until the walk parameters are above a certain value.
00047    * We can surmise that a good part of the x-intercept for forward motion
00048    * has to do with this phenomenon, since the robot has to accelerate up
00049    * to speed and decelerate to stop (linearly, thank heavens).
00050    *
00051    * Empirically, this minimum threshold for forward motion comes at about
00052    * 55. For turning left, it's about 0.2. For turning right, it's also
00053    * about 0.2. So, to achieve verisimilitude in our motion model, we'll
00054    * scale all the motion commands accordingly and hope for the best. We
00055    * don't have any motion data to find the x intercept for forward speed
00056    * other than 180 or rotational speeds other than 1, so I guess we'll just
00057    * guess. */
00058 #define RESCALE_DX_A  162.5885
00059 #define RESCALE_DX_B  -32.0347
00060 #define DX_MOT_THRESH 55
00061 
00062 #define RESCALE_DA_LEFT_A 69.3368*M_PI/180
00063 #define RESCALE_DA_LEFT_B -28.3579*M_PI/180
00064 #define DA_LEFT_MOT_THRESH  0.2
00065 
00066 #define RESCALE_DA_RIGHT_A  -68.4120*M_PI/180
00067 #define RESCALE_DA_RIGHT_B  34.7600*M_PI/180
00068 #define DA_RIGHT_MOT_THRESH -0.2
00069 
00070 /* First we rescale dx and da */
00071 if(dx > DX_MOT_THRESH)
00072   dx = RESCALE_DX_A * (dx - DX_MOT_THRESH) / (180.0 - DX_MOT_THRESH);
00073 else if(dx < -DX_MOT_THRESH)
00074   dx = RESCALE_DX_A * (dx + DX_MOT_THRESH) / (180.0 - DX_MOT_THRESH);
00075 else dx = 0.0;
00076 
00077 if(da > DA_LEFT_MOT_THRESH)
00078   da = RESCALE_DA_LEFT_A *
00079   (da - DA_LEFT_MOT_THRESH) / (1.008 - DA_LEFT_MOT_THRESH);
00080 else if(da < DA_RIGHT_MOT_THRESH)
00081   da = RESCALE_DA_RIGHT_A *
00082   (da - DA_RIGHT_MOT_THRESH) / (1.008 - DA_RIGHT_MOT_THRESH);
00083 else da = 0.0;
00084 
00085   /* Now, what to do about that X intercept... Since we don't have access to
00086    * the motion model from here (so, strictly speaking, all this tweaking is
00087    * illegitimate, but whatever), we can't tell it to reduce its final
00088    * estimates of forward or rotational motion by a certain amount. What we
00089    * can do, though, is reduce the amount of transit time passed to the
00090    * motion model.
00091    *
00092    * Moving forward, the robot "accelerates" at 162.6 * 3.8 = 617.8 "mm"/s^2.
00093    * This means it reaches the magic forward motion threshold of 55 "mm"/s
00094    * in 0.09 seconds. Likewise, rotating, it "accelerates" at 3.6 "rad"/s^2.
00095    * That means it reaches the threshold of 0.2 "rad"/s in 0.05 s. Since we
00096    * can only alter time one way, the strategy here is to lop off 0.09*2
00097    * seconds when going forward, 0.05*2 seconds when going sideways, and
00098    * 0.07 seconds otherwise. KLUUUUUUUUUDGEEE! */
00099      if((dx != 0.0) && (da == 0.0)) time = (time > 180 ? time - 180 : 0);
00100 else if((dx == 0.0) && (da != 0.0)) time = (time > 100 ? time - 100 : 0);
00101 else time = (time > 140 ? time - 140 : time = 0);

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