00001
00002
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
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
00031
00032
00033
00034
00035
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
00046 if((dx == 0) && (dy == 0) && (da == 0)) return;
00047
00048
00049
00050
00051
00052
00053
00054
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
00060
00061
00062
00063
00064
00065
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
00074 #warning "Using innate (and possibly obsolete) walking motion model!"
00075
00076
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
00082 arc_length = sqrt(x_motion*x_motion + y_motion*y_motion);
00083
00084
00085
00086
00087 if(fabs(theta_motion) < 0.000001) {
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 {
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
00103
00104 chord_length = sqrt(xpos_before_angles*xpos_before_angles +
00105 ypos_before_angles*ypos_before_angles);
00106
00107
00108 total_motion_angle = atan2(y_motion, x_motion) +
00109 atan2(ypos_before_angles, xpos_before_angles) +
00110 p->pose.theta;
00111
00112
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
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
00122
00123
00124
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
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