Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
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 |