Polish pose estimation R script

blair-robot-project · Oct 19, 2017 · eefa754 · eefa754
1 parent 7006f4a
commit eefa754
Show file tree

Hide file tree

Showing 2 changed files with 116 additions and 56 deletions.
diff --git a/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java b/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java
@@ -149,7 +149,7 @@ public static void main(String[] args) throws IOException {
 		//Calculated by driving each wheel n inches in opposite directions, then taking the angle moved, θ, and finding
 		// the circumference of a circle moved by the robot via C = 360 * n / θ
 		//You then find the diameter via C / π.
-		double balbasaurWheelbase = 33.3 / 12.;
+		double balbasaurWheelbase = 16. / 12.;
 		//200 in: 29.96
 		//50 in: 34.2
 

diff --git a/R scripts/poseEstimationChecker.R b/R scripts/poseEstimationChecker.R
@@ -43,8 +43,8 @@ smoothDerivative <- function(value, timeMillis, n){
   return(c(rep(0, ceiling(n/2)), smoothed, rep(0, floor(n/2))));
 }
 
-#Just kinda does everything
-plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst, actualWheelbase, fakeWheelbase){
+#Plot the effective wheelbase against a bunch of different things
+plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst){
   #Convert because degrees suuuuck
   rawAngle <- deg2rad(rawAngleDegrees)
 
@@ -74,23 +74,18 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
   #Plot wheelbase vs turn radius
   plot(x=combinedAngular[,3]/combinedAngular[,1], y=combinedAngular[,2]-2.0833, xlab="Turn Radius (feet)", ylab="Error in wheelbase diameter (feet)", main="Error in Wheelbase Diameter vs. Turn Radius")
   abline(a=avgWheelbase-2.0833, b=0, col='green')
+}
+
+#A pose estimation algorithm that assumes the left and right sides have equal scrub, in opposite directions
+equalScrubPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad){
+  #Convert because degrees suuuuck
+  rawAngle <- deg2rad(rawAngleDegrees)
 
-  #Eli's algorithm
+  #Set up output array
   out <- array(dim=c(length(timeMillis),8))
   colnames(out)<-c("X","Y","leftX","leftY","rightX","rightY","time","wheelbase")
   out[1,] <- c(leftPos[1],rightPos[1],NA,NA,NA,NA,timeMillis[1],NA)
 
-  #Noah's algorithm
-  noah <- array(dim=c(length(timeMillis),7))
-  colnames(noah) <- c("X","Y","leftX","leftY","rightX","rightY","time")
-  angle <- rawAngle[1]
-  noah[1,] <- c(leftPos[1],rightPos[1],actualWheelbase/2*cos(angle+pi/2), actualWheelbase/2*sin(angle+pi/2), actualWheelbase/2*cos(angle-pi/2), actualWheelbase/2*sin(angle-pi/2),timeMillis[1])
-
-  #Encoder-only algorithm
-  encOnly <- array(dim=c(length(timeMillis), 7))
-  colnames(encOnly) <- c("X","Y","leftX","leftY","rightX","rightY","time")
-  encOnly[1,] <- c(leftPos[1],rightPos[1],actualWheelbase/2*cos(angle+pi/2), actualWheelbase/2*sin(angle+pi/2), actualWheelbase/2*cos(angle-pi/2), actualWheelbase/2*sin(angle-pi/2),timeMillis[1])
-
   #Loop through each logged tic, calculating pose iteratively
   for(i in 2:length(timeMillis)){
     #Find change in time, in seconds
@@ -109,8 +104,7 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
 
     #The angle of the movement vector
     angle <- rawAngle[i-1]-(deltaTheta/2)
-
-    #Eli's approach
+
     if (deltaTheta == 0){
       #If we're driving straight, we know the magnitude is just the average of the two sides
       out[i,] <- c(out[i-1,1]+avgMoved*cos(angle),out[i-1,2]+avgMoved*sin(angle), NA, NA, NA, NA, timeMillis[i],NA)
@@ -129,24 +123,38 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
         out[i,] <- c(x, y, x+wheelbase/2*cos(angle+pi/2), y+wheelbase/2*sin(angle+pi/2), x+wheelbase/2*cos(angle-pi/2), y+wheelbase/2*sin(angle-pi/2), timeMillis[i],wheelbase)
       }
     }
+  }
+
+  #Plot results, with fake wheelbase, only showing points within 1 actual wheelbase of the path
+  plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], na.rm = TRUE)-3,max(out[,1],na.rm = TRUE)+3), ylim=c(min(out[,2], na.rm = TRUE)-3,max(out[,2], na.rm=TRUE)+3), xlab="X position (Feet)", ylab="Y position (Feet)", main="Equal Scrub Pose Estimation Algorithm", asp=1)
+  lines(out[,3], out[,4], col="Green")
+  lines(out[,5], out[,6], col="Red")
+
+  return(out)
+}
+
+#A pose estimation algorithm that ignores the worse encoder reading.
+ignoreWorstPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, actualWheelbase){
+  #Convert because degrees suuuuck
+  rawAngle <- deg2rad(rawAngleDegrees)
+
+  #Set up output array
+  out <- array(dim=c(length(timeMillis),7))
+  colnames(out) <- c("X","Y","leftX","leftY","rightX","rightY","time")
+  angle <- rawAngle[1]
+  out[1,] <- c(leftPos[1],rightPos[1],actualWheelbase/2*cos(angle+pi/2), actualWheelbase/2*sin(angle+pi/2), actualWheelbase/2*cos(angle-pi/2), actualWheelbase/2*sin(angle-pi/2),timeMillis[1])
+
+  #Loop through each logged tic, calculating pose iteratively
+  for(i in 2:length(timeMillis)){
 
-    #Encoder only
-    perpendicular <- angleBetween(leftX=encOnly[i-1,3],leftY=encOnly[i-1,4],rightX = encOnly[i-1,5],rightY = encOnly[i-1,6])
-    theta <- (deltaLeft - deltaRight)/fakeWheelbase
-    if(theta == 0){
-      x <- encOnly[i-1,1]+avgMoved*cos(perpendicular)
-      y <- encOnly[i-1,2]+avgMoved*sin(perpendicular)
-      encOnly[i,] <- c(x,y,x+fakeWheelbase/2*cos(perpendicular+pi/2), y+fakeWheelbase/2*sin(perpendicular+pi/2), x+fakeWheelbase/2*cos(perpendicular-pi/2), y+fakeWheelbase/2*sin(perpendicular-pi/2), timeMillis[i])
-    } else {
-      r <- fakeWheelbase / 2. * (deltaLeft + deltaRight) / (deltaLeft - deltaRight);
-      mag <- 2*r*sin(theta/2)
-      encAngle <- perpendicular - theta/2
-      x <- encOnly[i-1,1]+mag*cos(encAngle)
-      y <- encOnly[i-1,2]+mag*sin(encAngle)
-      encOnly[i,] <- c(x,y,x+fakeWheelbase/2*cos(perpendicular+pi/2), y+fakeWheelbase/2*sin(perpendicular+pi/2), x+fakeWheelbase/2*cos(perpendicular-pi/2), y+fakeWheelbase/2*sin(perpendicular-pi/2), timeMillis[i])
-    }
+    #Directly find change in position and angle
+    deltaLeft <- leftPos[i]-leftPos[i-1]
+    deltaRight <- rightPos[i]-rightPos[i-1]
+    deltaTheta <- rawAngle[i]-rawAngle[i-1]
 
-    #Noah's approach
+    #The angle of the movement vector
+    angle <- rawAngle[i-1]-(deltaTheta/2)
+
     #Take the side that slipped more and recalculate it from the wheelbase, change in angle, and other side's change
     if (deltaTheta < (deltaLeft - deltaRight) / actualWheelbase) {
       if (deltaLeft > 0) {
@@ -162,39 +170,91 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
       }
     }
 
-    #Recalculate average after recalculating one of the sides
+    #Calculate average after recalculating one of the sides
     avgMoved <- (deltaLeft+deltaRight)/2
 
     #If we're driving straight, the magnitude is just the average of the two sides
     if (deltaTheta == 0){
-        noah[i,] <- c(noah[i-1,1]+avgMoved*cos(angle),noah[i-1,2]+avgMoved*sin(angle), x+actualWheelbase/2*cos(angle+pi/2), y+actualWheelbase/2*sin(angle+pi/2), x+actualWheelbase/2*cos(angle-pi/2), y+actualWheelbase/2*sin(angle-pi/2), timeMillis[i])
+      out[i,] <- c(out[i-1,1]+avgMoved*cos(angle),out[i-1,2]+avgMoved*sin(angle), x+actualWheelbase/2*cos(angle+pi/2), y+actualWheelbase/2*sin(angle+pi/2), x+actualWheelbase/2*cos(angle-pi/2), y+actualWheelbase/2*sin(angle-pi/2), timeMillis[i])
     } else {
-        #If the sides moved the same distance but the angle changed, the radius is just the sector length over the angle
-        if (deltaLeft-deltaRight == 0){
-            r <- avgMoved/deltaTheta;
-        } else {
-            #If they moved a different distance, do a more complicated equation (may be the same as the other one, not doing the math yet)
-            r <- actualWheelbase / 2. * (deltaLeft + deltaRight) / (deltaLeft - deltaRight);
-        }
-        mag <- 2. * r * sin(deltaTheta / 2.);
-        
-        #Vector decomposition
-        x <- noah[i-1,1]+mag*cos(angle)
-        y <- noah[i-1,2]+mag*sin(angle)
-        noah[i,] <- c(x, y, x+actualWheelbase/2*cos(angle+pi/2), y+actualWheelbase/2*sin(angle+pi/2), x+actualWheelbase/2*cos(angle-pi/2), y+actualWheelbase/2*sin(angle-pi/2), timeMillis[i])
+      #If the sides moved the same distance but the angle changed, the radius is just the sector length over the angle
+      if (deltaLeft-deltaRight == 0){
+        r <- avgMoved/deltaTheta;
+      } else {
+        #If they moved a different distance, do a more complicated equation (may be the same as the other one, not doing the math yet)
+        r <- actualWheelbase / 2. * (deltaLeft + deltaRight) / (deltaLeft - deltaRight);
+      }
+      mag <- 2. * r * sin(deltaTheta / 2.);
+
+      #Vector decomposition
+      x <- out[i-1,1]+mag*cos(angle)
+      y <- out[i-1,2]+mag*sin(angle)
+      out[i,] <- c(x, y, x+actualWheelbase/2*cos(angle+pi/2), y+actualWheelbase/2*sin(angle+pi/2), x+actualWheelbase/2*cos(angle-pi/2), y+actualWheelbase/2*sin(angle-pi/2), timeMillis[i])
     }
   }
 
-  #Plot Eli results, with fake wheelbase, only showing points within 1 actual wheelbase of the path
-  plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], na.rm = TRUE)-actualWheelbase,max(out[,1],na.rm = TRUE)+actualWheelbase), ylim=c(min(out[,2], na.rm = TRUE)-actualWheelbase,max(out[,2], na.rm=TRUE)+actualWheelbase), xlab="X position (Feet)", ylab="Y position (Feet)", main="Eli's Pose Estimation Algorithm", asp=1)
+  #Plot results, with real wheelbase
+  plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], out[,3], out[,5]),max(out[,1], out[,3], out[,5])), ylim=c(min(out[,2], out[,4], out[,6]),max(out[,2], out[,4], out[,6])), xlab="X position (Feet)", ylab="Y position (Feet)", main="Ignore-Worst Pose Estimation Algorithm",asp=1)
   lines(out[,3], out[,4], col="Green")
   lines(out[,5], out[,6], col="Red")
 
-  #Plot Noah results, with real wheelbase
-  plot(noah[,1], noah[,2], t="l", xlim = c(min(noah[,1], noah[,3], noah[,5]),max(noah[,1], noah[,3], noah[,5])), ylim=c(min(noah[,2], noah[,4], noah[,6]),max(noah[,2], noah[,4], noah[,6])), xlab="X position (Feet)", ylab="Y position (Feet)", main="Noah's Pose Estimation Algorithm",asp=1)
-  #plot(noah[,1],noah[,2],t="l")
-  lines(noah[,3], noah[,4], col="Green")
-  lines(noah[,5], noah[,6], col="Red")
+  return(out)
+}
+
+#A pose estimation algorithm that only uses the encoders
+encoderOnlyPoseEstimation <- function(leftPos, rightPos, startingAngleDegrees, timeMillis, fakeWheelbase){
+  #Convert because degrees suuuuck
+  startingAngle <- deg2rad(startingAngleDegrees)
+
+  #Set up output array
+  out <- array(dim=c(length(timeMillis), 7))
+  colnames(out) <- c("X","Y","leftX","leftY","rightX","rightY","time")
+  out[1,] <- c(leftPos[1],rightPos[1],fakeWheelbase/2*cos(startingAngle+pi/2), fakeWheelbase/2*sin(startingAngle+pi/2), fakeWheelbase/2*cos(startingAngle-pi/2), fakeWheelbase/2*sin(startingAngle-pi/2),timeMillis[1])
 
-  return(encOnly)
+  #Loop through each logged tic, calculating pose iteratively
+  for(i in 2:length(timeMillis)){
+
+    #Directly find change in position and angle
+    deltaLeft <- leftPos[i]-leftPos[i-1]
+    deltaRight <- rightPos[i]-rightPos[i-1]
+
+    #Average
+    avgMoved <- (deltaLeft+deltaRight)/2
+
+    #Points in the direction the robot is facing at the start of tic
+    perpendicular <- angleBetween(leftX=out[i-1,3],leftY=out[i-1,4],rightX = out[i-1,5],rightY = out[i-1,6])-pi/2
+
+    #The angle of the sector the path is tracing
+    theta <- (deltaLeft - deltaRight)/fakeWheelbase
+
+    #If not turning, magnitude is just the average moved
+    if(theta == 0){
+      x <- out[i-1,1]+avgMoved*cos(perpendicular)
+      y <- out[i-1,2]+avgMoved*sin(perpendicular)
+      out[i,] <- c(x,y,x+fakeWheelbase/2*cos(perpendicular+pi/2), y+fakeWheelbase/2*sin(perpendicular+pi/2), x+fakeWheelbase/2*cos(perpendicular-pi/2), y+fakeWheelbase/2*sin(perpendicular-pi/2), timeMillis[i])
+    } else {
+      #If turning, calculate the radius and use that to get the magnitude of the turn
+      r <- avgMoved/theta;
+      mag <- 2*r*sin(theta/2)
+      angle <- perpendicular + theta/2
+      x <- out[i-1,1]+mag*cos(angle)
+      y <- out[i-1,2]+mag*sin(angle)
+      out[i,] <- c(x,y,x+fakeWheelbase/2*cos(angle+pi/2), y+fakeWheelbase/2*sin(angle+pi/2), x+fakeWheelbase/2*cos(angle-pi/2), y+fakeWheelbase/2*sin(angle-pi/2), timeMillis[i])
+    }
+  }
+
+  #Plot results, with fake wheelbase
+  plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], out[,3], out[,5]),max(out[,1], out[,3], out[,5])), ylim=c(min(out[,2], out[,4], out[,6]),max(out[,2], out[,4], out[,6])), xlab="X position (Feet)", ylab="Y position (Feet)", main="Encoder-only Pose Estimation Algorithm",asp=1)
+  lines(out[,3], out[,4], col="Green")
+  lines(out[,5], out[,6], col="Red")
+
+  return(out)
 }
+
+#Make all the graphs, for wheelbase and all 3 algorithms
+doEverything <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst, actualWheelbase, fakeWheelbase){
+  plotWheelVsVel(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst)
+  equalScrubPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad)
+  ignoreWorstPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, actualWheelbase)
+  return(encoderOnlyPoseEstimation(leftPos, rightPos, rawAngleDegrees[1], timeMillis, fakeWheelbase))
+}