Skip to content

Commit

Permalink
Merge branch 'noah_pose_estimation' of https://github.com/blair-robot…
Browse files Browse the repository at this point in the history 
…-project/robot2017 into iroc_code
  • Loading branch information
Noah Gleason committed Oct 20, 2017
2 parents f96580b + 383bcc9 commit 8674e84
Show file tree
Hide file tree
Showing 4 changed files with 129 additions and 90 deletions.
2 changes: 1 addition & 1 deletion Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 = 30. / 12.;
//200 in: 29.96
//50 in: 34.2

Expand Down
2 changes: 1 addition & 1 deletion R scripts/drawMP.R
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, center
#This is the angle for the vector pointing towards the new position of each
#wheel.
#To understand why this formula is correct, overlay isoclese triangles on the sectors
vectorTheta <- perpendicular-theta/2
vectorTheta <- perpendicular+theta/2

#The is the length of the vector pointing towards the new position of each
#wheel divided by the radius of the turning circle.
Expand Down
211 changes: 125 additions & 86 deletions R scripts/poseEstimationChecker.R
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,34 +2,9 @@
rad2deg <- function(rad) {(rad * 180) / (pi)}
deg2rad <- function(deg) {(deg * pi) / (180)}

#Get the angle between two points
angleBetween <- function(leftX, leftY, rightX, rightY){
deltaX <- leftX-rightX
deltaY <- leftY-rightY
if (identical(deltaX, 0)){
ans <- pi/2
} else {
#Pretend it's first quadrant because we manually determine quadrants
ans <- atan(abs(deltaY/deltaX))
}
if (deltaY > 0){
if (deltaX > 0){
#If it's actually quadrant 1
return(ans)
}else {
#quadrant 2
return(pi - ans)
}
return(ans)
} else {
if (deltaX > 0){
#quadrant 4
return(-ans)
}else {
#quadrant 3
return(-(pi - ans))
}
}
#Find the distance between the end position of two things
dist <- function(x1, y1, x2, y2){
return(sqrt((x1[length(x1)] - x2[length(x2)])^2+(y1[length(y1)] - y2[length(y2)])^2))
}

#Calculate the effective wheelbase for a given delta left, right, and angle
Expand All @@ -43,8 +18,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)

Expand Down Expand Up @@ -73,24 +48,21 @@ 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')
abline(a=avgWheelbase, b=0, col='green')

return(avgWheelbase)
}

#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
Expand All @@ -108,9 +80,8 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
avgMoved <- (deltaLeft+deltaRight)/2

#The angle of the movement vector
angle <- rawAngle[i-1]-(deltaTheta/2)

#Eli's approach
angle <- rawAngle[i-1]+(deltaTheta/2)

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)
Expand All @@ -126,27 +97,41 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
if (deltaTheta/deltaTime < angularVelThreshRad){
out[i,] <- c(x, y, NA,NA,NA,NA, timeMillis[i],NA)
} else {
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)
out[i,] <- c(x, y, x+wheelbase/2*cos(rawAngle[i]+pi/2), y+wheelbase/2*sin(rawAngle[i]+pi/2), x+wheelbase/2*cos(rawAngle[i]-pi/2), y+wheelbase/2*sin(rawAngle[i]-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) {
Expand All @@ -162,39 +147,93 @@ 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(rawAngle[i]+pi/2), y+actualWheelbase/2*sin(rawAngle[i]+pi/2), x+actualWheelbase/2*cos(rawAngle[i]-pi/2), y+actualWheelbase/2*sin(rawAngle[i]-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)

wheelRadius <- fakeWheelbase/2

#Set up output array
out <- array(dim=c(length(timeMillis), 8))
colnames(out) <- c("X","Y","leftX","leftY","rightX","rightY","angle","time")
out[1,] <- c(leftPos[1],rightPos[1],wheelRadius*cos(startingAngle+pi/2), wheelRadius*sin(startingAngle+pi/2), wheelRadius*cos(startingAngle-pi/2), wheelRadius*sin(startingAngle-pi/2),startingAngle,timeMillis[1])

#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 <- out[i-1,7]

#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){
mag <- avgMoved
} else {
#If turning, calculate the radius and use that to get the magnitude of the turn
mag <- 2*(avgMoved/theta)*sin(theta/2)
}

angle <- perpendicular + (theta/2)
x <- out[i-1,1]+mag*cos(angle)
y <- out[i-1,2]+mag*sin(angle)
newHeading <- perpendicular + theta
out[i,] <- c(x,y,x+wheelRadius*cos(newHeading+pi/2), y+wheelRadius*sin(newHeading+pi/2), x+wheelRadius*cos(newHeading-pi/2), y+wheelRadius*sin(newHeading-pi/2), newHeading, 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(encOnly)
return(out)
}

#Make all the graphs, for wheelbase and all 3 algorithms, using the average effective wheelbase for encoder-only
doEverything <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst, actualWheelbase){
avg <- plotWheelVsVel(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst)
equalScrubPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad)
ignoreWorstPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, actualWheelbase)
encoderOnlyPoseEstimation(leftPos=leftPos, rightPos=rightPos, startingAngleDegrees=rawAngleDegrees[1], timeMillis=timeMillis, fakeWheelbase=avg)
return(avg)
}
4 changes: 2 additions & 2 deletions RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -157,7 +157,7 @@ private static double[] calcEliVector(double left, double right, double deltaThe
//This next part is too complicated to explain in comments. Read this wiki page instead:
// http://team449.shoutwiki.com/wiki/Pose_Estimation
double r = ((left+right)/2.)/deltaTheta;
double vectorAngle = lastAngle - deltaTheta/2.;
double vectorAngle = lastAngle + deltaTheta/2.;
double vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.);
vector[0] = vectorMagnitude * Math.cos(vectorAngle);
vector[1] = vectorMagnitude * Math.sin(vectorAngle);
Expand All @@ -183,7 +183,7 @@ private static double[] calcVector(double left, double right, double robotDiamet
} else {
r = robotDiameter / 2. * (left + right) / (left - right);
}
double vectorAngle = lastAngle - deltaTheta/2.;
double vectorAngle = lastAngle + deltaTheta/2.;
double vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.);
vector[0] = vectorMagnitude * Math.cos(vectorAngle);
vector[1] = vectorMagnitude * Math.sin(vectorAngle);
Expand Down

0 comments on commit 8674e84

Please sign in to comment.