From 5fcb5112586091de7dd842ab238b114480be974c Mon Sep 17 00:00:00 2001 From: David Goodman Date: Fri, 2 Oct 2015 20:00:10 -0700 Subject: [PATCH] Fixed DubinsLength bug. --- angularMod.m | 7 ++++++ findDubinsLength.m | 28 ++++++++++++++-------- heading2Theta.m | 6 ++--- test/testFindDubinsLength.m | 46 ++++++++++++++++++++++++++++++++++++- wrapAngle.m | 5 ++++ 5 files changed, 79 insertions(+), 13 deletions(-) create mode 100644 angularMod.m create mode 100644 wrapAngle.m diff --git a/angularMod.m b/angularMod.m new file mode 100644 index 0000000..e723dbe --- /dev/null +++ b/angularMod.m @@ -0,0 +1,7 @@ +function [ m ] = angularMod(x,y) +%angularMod Modulo safe for angles. +% Behaves as expected for angular quantities (negatives?). + n = floor(x./y); + m = x - n.*y; +end + diff --git a/findDubinsLength.m b/findDubinsLength.m index b278dfc..117d3c8 100644 --- a/findDubinsLength.m +++ b/findDubinsLength.m @@ -12,6 +12,7 @@ % DUBINS_DEBUG = 0; DEBUG_VERBOSE = 0; % plots dubins trajectories +DEBUG_VVERBOSE = 0; % extra verbosity %============= Input Validation =============== if nargin < 1 error('No input arguments given!'); @@ -43,7 +44,7 @@ % TODO be sure we aren't using anonymous functions anywhere. They are slow. %rotm = @(theta) [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]'; -% TODO replace everywhere with wrapTo2Pi() +% TODO replace everywhere with wrapAngle() %wrap = @(theta) mod(theta,2*pi); @@ -76,8 +77,15 @@ % Case I, R-S-R theta = findHeadingFrom(c_rs,c_re); -L1 = norm(c_rs - c_re) + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta - pi/2) - wrapTo2Pi(x_s - pi/2))... - + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta - pi/2)); +if (DEBUG_VVERBOSE) +fprintf('norm(c_rs - c_re)=%.6f \nr*wrap1=%0.6f \nr*wrap2=%0.6f\n',... + norm(c_rs - c_re), r*wrapAngle(2*pi + wrapAngle(theta - pi/2) - wrapAngle(x_s - pi/2)),... + r*wrapAngle(2*pi + wrapAngle(x_e - pi/2) - wrapAngle(theta - pi/2))); +fprintf('r*wrap2: wrapAngle(x_e - pi/2)=%0.6f, wrapAngle(theta - pi/2)=%0.6f\n',... + wrapAngle(x_e - pi/2), wrapAngle(theta - pi/2)); +end +L1 = norm(c_rs - c_re) + r*wrapAngle(2*pi + wrapAngle(theta - pi/2) - wrapAngle(x_s - pi/2))... + + r*wrapAngle(2*pi + wrapAngle(x_e - pi/2) - wrapAngle(theta - pi/2)); if (DUBINS_DEBUG & DEBUG_VERBOSE) theta L1 @@ -87,8 +95,8 @@ len = norm(c_le - c_rs); theta = findHeadingFrom(c_rs,c_le); theta2 = theta - pi/2 + asin((2*r)/len); -L2 = sqrt(len^2 - 4*r^2)+r*wrapTo2Pi(2*pi + wrapTo2Pi(theta2) - wrapTo2Pi(x_s - pi/2))... - + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta2 + pi) - wrapTo2Pi(x_e + pi/2)); +L2 = sqrt(len^2 - 4*r^2)+r*wrapAngle(2*pi + wrapAngle(theta2) - wrapAngle(x_s - pi/2))... + + r*wrapAngle(2*pi + wrapAngle(theta2 + pi) - wrapAngle(x_e + pi/2)); if (DUBINS_DEBUG & DEBUG_VERBOSE) L2 end @@ -102,16 +110,16 @@ error('Error in case III'); end -L3 = sqrt(len^2 - 4*r^2) + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_s + pi/2) - wrapTo2Pi(theta + theta2))... - + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_e - pi/2) - wrapTo2Pi(theta + theta2 - pi)); +L3 = sqrt(len^2 - 4*r^2) + r*wrapAngle(2*pi + wrapAngle(x_s + pi/2) - wrapAngle(theta + theta2))... + + r*wrapAngle(2*pi + wrapAngle(x_e - pi/2) - wrapAngle(theta + theta2 - pi)); if (DUBINS_DEBUG & DEBUG_VERBOSE) L3 end % Case IV, L-S-L theta = findHeadingFrom(c_ls,c_le); -L4 = norm(c_ls - c_le) + r*wrapTo2Pi(2*pi + wrapTo2Pi(x_s + pi/2) - wrapTo2Pi(theta + pi/2))... - + r*wrapTo2Pi(2*pi + wrapTo2Pi(theta + pi/2) - wrapTo2Pi(x_e + pi/2)); +L4 = norm(c_ls - c_le) + r*wrapAngle(2*pi + wrapAngle(x_s + pi/2) - wrapAngle(theta + pi/2))... + + r*wrapAngle(2*pi + wrapAngle(theta + pi/2) - wrapAngle(x_e + pi/2)); if (DUBINS_DEBUG & DEBUG_VERBOSE) L4 end @@ -178,3 +186,5 @@ function plotScenario(p_s, x_s, p_e, x_e, c_rs, c_ls, c_re, c_le, r) M = [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]'; end + + diff --git a/heading2Theta.m b/heading2Theta.m index c7dfc28..06042c6 100644 --- a/heading2Theta.m +++ b/heading2Theta.m @@ -12,13 +12,13 @@ error('psi must be between 0 and 2*pi') end % ============================================================= -theta = mod(-(psi - pi/2),2*pi); +theta = angularMod(-(psi - pi/2),2*pi); end %% function heading2Theta() -%Note mod is actually: +%Note angle safe mod if: % -%function [m] = myMod(x,y) +%function [m] = mymod(x,y) % n = floor(x./y); % m = x - n.*y; %end diff --git a/test/testFindDubinsLength.m b/test/testFindDubinsLength.m index cb9e176..1a098f8 100644 --- a/test/testFindDubinsLength.m +++ b/test/testFindDubinsLength.m @@ -213,9 +213,53 @@ testsPassed = testsPassed + 1; end -fprintf('----------------------------\n'); +%% Boundaries when angle distance is small +%subplot(2,2,4); + +% Position +%findDubinsLength([450 0], 3.927, [350, -100], 3.927, 10.2) +startPosition = [450 0]; +startHeading = 3.92699 + 0.00001; % rad +endPosition = [350, -100]; +endHeading = 3.92699; % rad +q0 = [startPosition heading2Theta(startHeading)]; +q1 = [endPosition heading2Theta(endHeading)]; + +% Plotting +path = dubins(q0, q1, opts.TurnRadius, opts.DubinsStepSize); +%plot([startPosition(1) endPosition(1)], [startPosition(2) endPosition(2)],... +% 'ko', 'MarkerFaceColor', 'k') +L = findDubinsLength(startPosition, startHeading, endPosition, endHeading,... + opts.TurnRadius); +hold on; +%set(0,'currentFigure',fh) +%plot(path(1,1:end), path(2,1:end), 'Color', 'g'); +%title('Case IV: L-S-L'); +%yl = ylim(); +%text(0,yl(1)+5,sprintf('L = %.2f',L)); + +% Count length of path from DubinsPlot tool +Lexpected = 0; +for i=2:length(path) + l_i = sqrt((path(1,i) - path(1,i-1))^2 + (path(2,i) - path(2,i-1))^2); + Lexpected = Lexpected + l_i; +end % for + +% Results +result = L; +resultExpected = Lexpected; +theta_diff = endHeading - startHeading; +fprintf('Boundary (Line) Test: L = %.2f, CWdiff=%0.6f',result, theta_diff); +if (abs(result - resultExpected) > EPSILON_ERROR) + fprintf('\t-- FAILED: expected %.2f\n',resultExpected); +else + fprintf('\t-- PASS\n'); + testsPassed = testsPassed + 1; +end + %% Results +fprintf('----------------------------\n'); if (testsPassed ~= totalTests) fprintf('\nFAILED %i of %i tests\n\n',totalTests - testsPassed, totalTests); else diff --git a/wrapAngle.m b/wrapAngle.m new file mode 100644 index 0000000..2e1f043 --- /dev/null +++ b/wrapAngle.m @@ -0,0 +1,5 @@ +function [ theta ] = wrapAngle( theta ) +%wrapAngle Wraps an angle to 2*pi using angularMod. +% Uses modulo function safe for angles. + theta = angularMod(theta,2*pi); +end