git commit -m "first commit"
This commit is contained in:
3123
navigations/sbpl/matlab/mprim/all_file.mprim
Executable file
3123
navigations/sbpl/matlab/mprim/all_file.mprim
Executable file
File diff suppressed because it is too large
Load Diff
316
navigations/sbpl/matlab/mprim/genmprim.m
Executable file
316
navigations/sbpl/matlab/mprim/genmprim.m
Executable file
@@ -0,0 +1,316 @@
|
||||
% /*
|
||||
% * Copyright (c) 2008, Maxim Likhachev
|
||||
% * All rights reserved.
|
||||
% *
|
||||
% * Redistribution and use in source and binary forms, with or without
|
||||
% * modification, are permitted provided that the following conditions are met:
|
||||
% *
|
||||
% * * Redistributions of source code must retain the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer.
|
||||
% * * Redistributions in binary form must reproduce the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer in the
|
||||
% * documentation and/or other materials provided with the distribution.
|
||||
% * * Neither the name of the Carnegie Mellon University nor the names of its
|
||||
% * contributors may be used to endorse or promote products derived from
|
||||
% * this software without specific prior written permission.
|
||||
% *
|
||||
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% * POSSIBILITY OF SUCH DAMAGE.
|
||||
% */
|
||||
|
||||
function[] = genmprim(outfilename)
|
||||
|
||||
%
|
||||
%generates motion primitives and saves them into file
|
||||
%
|
||||
%written by Maxim Likhachev
|
||||
%---------------------------------------------------
|
||||
%
|
||||
|
||||
%defines
|
||||
|
||||
LINESEGMENT_MPRIMS = 1; %set the desired type of motion primitives
|
||||
UNICYCLE_MPRIMS = 0;
|
||||
|
||||
|
||||
|
||||
if LINESEGMENT_MPRIMS == 1
|
||||
resolution = 0.01;
|
||||
numberofangles = 32; %preferably a power of 2, definitely multiple of 8
|
||||
numberofprimsperangle = 16;
|
||||
|
||||
%multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1;
|
||||
backwardcostmult = 5;
|
||||
forwardandturncostmult = 1;
|
||||
sidestepcostmult = 50;
|
||||
turninplacecostmult = 50;
|
||||
|
||||
%note, what is shown x,y,theta changes (not absolute numbers)
|
||||
|
||||
%0 degreees
|
||||
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(2,:) = [4 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(3,:) = [8 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(4,:) = [6 2 0 sidestepcostmult];
|
||||
basemprimendpts0_c(5,:) = [6 -2 0 sidestepcostmult];
|
||||
basemprimendpts0_c(6,:) = [2 3 0 sidestepcostmult];
|
||||
basemprimendpts0_c(7,:) = [2 -3 0 sidestepcostmult];
|
||||
basemprimendpts0_c(8,:) = [-5 0 0 backwardcostmult];
|
||||
%1/32 theta change
|
||||
basemprimendpts0_c(9,:) = [6 2 1 forwardandturncostmult];
|
||||
basemprimendpts0_c(10,:) = [6 -2 -1 forwardandturncostmult];
|
||||
%2/32 theta change
|
||||
basemprimendpts0_c(11,:) = [4 3 2 forwardandturncostmult];
|
||||
basemprimendpts0_c(12,:) = [4 -3 -2 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts0_c(13,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts0_c(14,:) = [0 0 -1 turninplacecostmult];
|
||||
basemprimendpts0_c(15,:) = [0 0 3 turninplacecostmult];
|
||||
basemprimendpts0_c(16,:) = [0 0 -3 turninplacecostmult];
|
||||
|
||||
%45 degrees
|
||||
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
|
||||
basemprimendpts45_c(2,:) = [3 3 0 forwardcostmult];
|
||||
basemprimendpts45_c(3,:) = [6 6 0 forwardcostmult];
|
||||
basemprimendpts45_c(4,:) = [2 6 0 sidestepcostmult];
|
||||
basemprimendpts45_c(5,:) = [6 2 0 sidestepcostmult];
|
||||
basemprimendpts45_c(6,:) = [0 4 0 sidestepcostmult];
|
||||
basemprimendpts45_c(7,:) = [4 0 0 sidestepcostmult];
|
||||
basemprimendpts45_c(8,:) = [-4 -4 0 backwardcostmult];
|
||||
%1/32 theta change
|
||||
basemprimendpts45_c(9,:) = [2 6 1 forwardandturncostmult];
|
||||
basemprimendpts45_c(10,:) = [6 2 -1 forwardandturncostmult];
|
||||
%2/32 theta change
|
||||
basemprimendpts45_c(11,:) = [1 5 2 forwardandturncostmult];
|
||||
basemprimendpts45_c(12,:) = [5 1 -2 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts45_c(13,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts45_c(14,:) = [0 0 -1 turninplacecostmult];
|
||||
basemprimendpts45_c(15,:) = [0 0 3 turninplacecostmult];
|
||||
basemprimendpts45_c(16,:) = [0 0 -3 turninplacecostmult];
|
||||
|
||||
%22.5 degrees
|
||||
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(2,:) = [4 2 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(3,:) = [6 3 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(4,:) = [4 4 0 sidestepcostmult];
|
||||
basemprimendpts22p5_c(5,:) = [6 2 0 sidestepcostmult];
|
||||
basemprimendpts22p5_c(6,:) = [0 3 0 sidestepcostmult];
|
||||
basemprimendpts22p5_c(7,:) = [4 -1 0 sidestepcostmult];
|
||||
basemprimendpts22p5_c(8,:) = [-4 -2 0 backwardcostmult];
|
||||
%1/32 theta change
|
||||
basemprimendpts22p5_c(9,:) = [4 4 1 forwardandturncostmult];
|
||||
basemprimendpts22p5_c(10,:) = [6 2 -1 forwardandturncostmult];
|
||||
%2/32 theta change
|
||||
basemprimendpts22p5_c(11,:) = [2 4 2 forwardandturncostmult];
|
||||
basemprimendpts22p5_c(12,:) = [6 0 -2 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts22p5_c(13,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts22p5_c(14,:) = [0 0 -1 turninplacecostmult];
|
||||
basemprimendpts22p5_c(15,:) = [0 0 3 turninplacecostmult];
|
||||
basemprimendpts22p5_c(16,:) = [0 0 -3 turninplacecostmult];
|
||||
|
||||
%11.25 degrees
|
||||
basemprimendpts11p25_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts11p25_c(1,:) = [3 1 0 forwardcostmult];
|
||||
basemprimendpts11p25_c(2,:) = [6 2 0 forwardcostmult];
|
||||
basemprimendpts11p25_c(3,:) = [9 3 0 forwardcostmult];
|
||||
basemprimendpts11p25_c(4,:) = [4 3 0 sidestepcostmult];
|
||||
basemprimendpts11p25_c(5,:) = [6 0 0 sidestepcostmult];
|
||||
basemprimendpts11p25_c(6,:) = [1 3 0 sidestepcostmult];
|
||||
basemprimendpts11p25_c(7,:) = [3 -2 0 sidestepcostmult];
|
||||
basemprimendpts11p25_c(8,:) = [-6 -2 0 backwardcostmult];
|
||||
%1/32 theta change
|
||||
basemprimendpts11p25_c(9,:) = [4 3 1 forwardandturncostmult];
|
||||
basemprimendpts11p25_c(10,:) = [6 0 -1 forwardandturncostmult];
|
||||
%2/32 theta change
|
||||
basemprimendpts11p25_c(11,:) = [2 4 2 forwardandturncostmult];
|
||||
basemprimendpts11p25_c(12,:) = [5 -1 -2 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts11p25_c(13,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts11p25_c(14,:) = [0 0 -1 turninplacecostmult];
|
||||
basemprimendpts11p25_c(15,:) = [0 0 3 turninplacecostmult];
|
||||
basemprimendpts11p25_c(16,:) = [0 0 -3 turninplacecostmult];
|
||||
|
||||
%33.75 degrees
|
||||
basemprimendpts33p75_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts33p75_c(1,:) = [3 2 0 forwardcostmult];
|
||||
basemprimendpts33p75_c(2,:) = [6 4 0 forwardcostmult];
|
||||
basemprimendpts33p75_c(3,:) = [9 6 0 forwardcostmult];
|
||||
basemprimendpts33p75_c(4,:) = [4 5 0 sidestepcostmult];
|
||||
basemprimendpts33p75_c(5,:) = [6 2 0 sidestepcostmult];
|
||||
basemprimendpts33p75_c(6,:) = [0 4 0 sidestepcostmult];
|
||||
basemprimendpts33p75_c(7,:) = [3 -2 0 sidestepcostmult];
|
||||
basemprimendpts33p75_c(8,:) = [-6 -4 0 backwardcostmult];
|
||||
%1/32 theta change
|
||||
basemprimendpts33p75_c(9,:) = [4 5 1 forwardandturncostmult];
|
||||
basemprimendpts33p75_c(10,:) = [6 2 -1 forwardandturncostmult];
|
||||
%2/32 theta change
|
||||
basemprimendpts33p75_c(11,:) = [1 5 2 forwardandturncostmult];
|
||||
basemprimendpts33p75_c(12,:) = [3 -2 -2 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts33p75_c(13,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts33p75_c(14,:) = [0 0 -1 turninplacecostmult];
|
||||
basemprimendpts33p75_c(15,:) = [0 0 3 turninplacecostmult];
|
||||
basemprimendpts33p75_c(16,:) = [0 0 -3 turninplacecostmult];
|
||||
|
||||
|
||||
elseif UNICYCLE_MPRIMS == 1
|
||||
fprintf(1, 'ERROR: unsupported mprims type\n');
|
||||
return;
|
||||
else
|
||||
fprintf(1, 'ERROR: undefined mprims type\n');
|
||||
return;
|
||||
end;
|
||||
|
||||
|
||||
fout = fopen(outfilename, 'w');
|
||||
|
||||
|
||||
%write the header
|
||||
fprintf(fout, 'resolution_m: %f\n', resolution);
|
||||
fprintf(fout, 'numberofangles: %d\n', numberofangles);
|
||||
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
|
||||
|
||||
%iterate over angles
|
||||
for angleind = 1:numberofangles
|
||||
|
||||
figure(1);
|
||||
hold off;
|
||||
|
||||
text(0, 0, int2str(angleind));
|
||||
|
||||
%iterate over primitives
|
||||
for primind = 1:numberofprimsperangle
|
||||
fprintf(fout, 'primID: %d\n', primind-1);
|
||||
fprintf(fout, 'startangle_c: %d\n', angleind-1);
|
||||
|
||||
%current angle
|
||||
currentangle = (angleind-1)*2*pi/numberofangles;
|
||||
currentangle_36000int = round((angleind-1)*36000/numberofangles);
|
||||
|
||||
%compute which template to use
|
||||
if (rem(currentangle_36000int, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts0_c(primind,:);
|
||||
angle = currentangle;
|
||||
elseif (rem(currentangle_36000int, 4500) == 0)
|
||||
basemprimendpts_c = basemprimendpts45_c(primind,:);
|
||||
angle = currentangle - 45*pi/180;
|
||||
elseif (rem(currentangle_36000int-7875, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 78.75*pi/180;
|
||||
fprintf(1, '78p75\n');
|
||||
elseif (rem(currentangle_36000int-6750, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
|
||||
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
|
||||
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
|
||||
angle = currentangle - 67.5*pi/180;
|
||||
fprintf(1, '67p5\n');
|
||||
elseif (rem(currentangle_36000int-5625, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 56.25*pi/180;
|
||||
fprintf(1, '56p25\n');
|
||||
elseif (rem(currentangle_36000int-3375, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
angle = currentangle - 33.75*pi/180;
|
||||
fprintf(1, '33p75\n');
|
||||
elseif (rem(currentangle_36000int-2250, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
angle = currentangle - 22.5*pi/180;
|
||||
fprintf(1, '22p5\n');
|
||||
elseif (rem(currentangle_36000int-1125, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
angle = currentangle - 11.25*pi/180;
|
||||
fprintf(1, '11p25\n');
|
||||
else
|
||||
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
|
||||
return;
|
||||
end;
|
||||
|
||||
%now figure out what action will be
|
||||
baseendpose_c = basemprimendpts_c(1:3);
|
||||
additionalactioncostmult = basemprimendpts_c(4);
|
||||
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
|
||||
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
|
||||
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
|
||||
endpose_c = [endx_c endy_c endtheta_c];
|
||||
|
||||
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
|
||||
|
||||
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
|
||||
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
end;
|
||||
|
||||
%generate intermediate poses (remember they are w.r.t 0,0 (and not
|
||||
%centers of the cells)
|
||||
numofsamples = 10;
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if LINESEGMENT_MPRIMS == 1
|
||||
startpt = [0 0 currentangle];
|
||||
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
|
||||
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
for iind = 1:numofsamples
|
||||
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
|
||||
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
|
||||
0];
|
||||
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
|
||||
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
|
||||
end;
|
||||
end;
|
||||
|
||||
%write out
|
||||
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
|
||||
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
|
||||
for interind = 1:size(intermcells_m, 1)
|
||||
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
|
||||
end;
|
||||
|
||||
plot(intermcells_m(:,1), intermcells_m(:,2));
|
||||
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
|
||||
hold on;
|
||||
|
||||
end;
|
||||
grid;
|
||||
pause;
|
||||
end;
|
||||
|
||||
fclose('all');
|
||||
279
navigations/sbpl/matlab/mprim/genmprim_unicycle.m
Executable file
279
navigations/sbpl/matlab/mprim/genmprim_unicycle.m
Executable file
@@ -0,0 +1,279 @@
|
||||
% /*
|
||||
% * Copyright (c) 2008, Maxim Likhachev
|
||||
% * All rights reserved.
|
||||
% *
|
||||
% * Redistribution and use in source and binary forms, with or without
|
||||
% * modification, are permitted provided that the following conditions are met:
|
||||
% *
|
||||
% * * Redistributions of source code must retain the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer.
|
||||
% * * Redistributions in binary form must reproduce the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer in the
|
||||
% * documentation and/or other materials provided with the distribution.
|
||||
% * * Neither the name of the Carnegie Mellon University nor the names of its
|
||||
% * contributors may be used to endorse or promote products derived from
|
||||
% * this software without specific prior written permission.
|
||||
% *
|
||||
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% * POSSIBILITY OF SUCH DAMAGE.
|
||||
% */
|
||||
function[] = genmprim_unicycle(outfilename)
|
||||
|
||||
%
|
||||
%generates motion primitives and saves them into file
|
||||
%
|
||||
%written by Maxim Likhachev
|
||||
%---------------------------------------------------
|
||||
%
|
||||
|
||||
%defines
|
||||
|
||||
UNICYCLE_MPRIM_16DEGS = 1;
|
||||
|
||||
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
resolution = 0.025;
|
||||
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
|
||||
numberofprimsperangle = 5;
|
||||
|
||||
%multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1;
|
||||
backwardcostmult = 5;
|
||||
forwardandturncostmult = 2;
|
||||
sidestepcostmult = 10;
|
||||
turninplacecostmult = 5;
|
||||
|
||||
%note, what is shown x,y,theta changes (not absolute numbers)
|
||||
|
||||
%0 degreees
|
||||
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
|
||||
basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
%basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
%basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
|
||||
%45 degrees
|
||||
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
|
||||
basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
|
||||
basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
|
||||
basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
%basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
%basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
|
||||
%22.5 degrees
|
||||
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%x aligned with the heading of the robot, angles are positive
|
||||
%counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
|
||||
basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
%basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
%basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
|
||||
else
|
||||
fprintf(1, 'ERROR: undefined mprims type\n');
|
||||
return;
|
||||
end;
|
||||
|
||||
|
||||
fout = fopen(outfilename, 'w');
|
||||
|
||||
|
||||
%write the header
|
||||
fprintf(fout, 'resolution_m: %f\n', resolution);
|
||||
fprintf(fout, 'numberofangles: %d\n', numberofangles);
|
||||
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
|
||||
|
||||
%iterate over angles
|
||||
for angleind = 1:numberofangles
|
||||
|
||||
figure(1);
|
||||
hold off;
|
||||
|
||||
text(0, 0, int2str(angleind));
|
||||
|
||||
%iterate over primitives
|
||||
for primind = 1:numberofprimsperangle
|
||||
fprintf(fout, 'primID: %d\n', primind-1);
|
||||
fprintf(fout, 'startangle_c: %d\n', angleind-1);
|
||||
|
||||
%current angle
|
||||
currentangle = (angleind-1)*2*pi/numberofangles;
|
||||
currentangle_36000int = round((angleind-1)*36000/numberofangles);
|
||||
|
||||
%compute which template to use
|
||||
if (rem(currentangle_36000int, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts0_c(primind,:);
|
||||
angle = currentangle;
|
||||
elseif (rem(currentangle_36000int, 4500) == 0)
|
||||
basemprimendpts_c = basemprimendpts45_c(primind,:);
|
||||
angle = currentangle - 45*pi/180;
|
||||
elseif (rem(currentangle_36000int-7875, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 78.75*pi/180;
|
||||
fprintf(1, '78p75\n');
|
||||
elseif (rem(currentangle_36000int-6750, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
|
||||
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
|
||||
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
|
||||
angle = currentangle - 67.5*pi/180;
|
||||
fprintf(1, '67p5\n');
|
||||
elseif (rem(currentangle_36000int-5625, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 56.25*pi/180;
|
||||
fprintf(1, '56p25\n');
|
||||
elseif (rem(currentangle_36000int-3375, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
angle = currentangle - 33.75*pi/180;
|
||||
fprintf(1, '33p75\n');
|
||||
elseif (rem(currentangle_36000int-2250, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
angle = currentangle - 22.5*pi/180;
|
||||
fprintf(1, '22p5\n');
|
||||
elseif (rem(currentangle_36000int-1125, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
angle = currentangle - 11.25*pi/180;
|
||||
fprintf(1, '11p25\n');
|
||||
else
|
||||
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
|
||||
return;
|
||||
end;
|
||||
|
||||
%now figure out what action will be
|
||||
baseendpose_c = basemprimendpts_c(1:3);
|
||||
additionalactioncostmult = basemprimendpts_c(4);
|
||||
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
|
||||
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
|
||||
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
|
||||
endpose_c = [endx_c endy_c endtheta_c];
|
||||
|
||||
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
|
||||
|
||||
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
|
||||
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
end;
|
||||
|
||||
%generate intermediate poses (remember they are w.r.t 0,0 (and not
|
||||
%centers of the cells)
|
||||
numofsamples = 10;
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
startpt = [0 0 currentangle];
|
||||
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
|
||||
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
|
||||
for iind = 1:numofsamples
|
||||
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
|
||||
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
|
||||
0];
|
||||
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
|
||||
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
|
||||
|
||||
end;
|
||||
else %unicycle-based move forward or backward
|
||||
R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
|
||||
sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
|
||||
S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
|
||||
l = S(1);
|
||||
tvoverrv = S(2);
|
||||
rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
|
||||
tv = tvoverrv*rv;
|
||||
|
||||
if l < 0
|
||||
fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
|
||||
l = 0;
|
||||
end;
|
||||
%compute rv
|
||||
%rv = baseendpose_c(3)*2*pi/numberofangles;
|
||||
%compute tv
|
||||
%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
|
||||
%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
|
||||
%tv = (tvx + tvy)/2.0;
|
||||
%generate samples
|
||||
for iind = 1:numofsamples
|
||||
dt = (iind-1)/(numofsamples-1);
|
||||
|
||||
%dtheta = rv*dt + startpt(3);
|
||||
%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
% dtheta];
|
||||
|
||||
if(dt*tv < l)
|
||||
intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
|
||||
startpt(2) + dt*tv*sin(startpt(3)) ...
|
||||
startpt(3)];
|
||||
else
|
||||
dtheta = rv*(dt - l/tv) + startpt(3);
|
||||
intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
dtheta];
|
||||
end;
|
||||
end;
|
||||
%correct
|
||||
errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
|
||||
endpt(2) - intermcells_m(numofsamples,2)];
|
||||
fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
|
||||
interpfactor = [0:1/(numofsamples-1):1];
|
||||
intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
|
||||
intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
|
||||
end;
|
||||
end;
|
||||
|
||||
%write out
|
||||
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
|
||||
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
|
||||
for interind = 1:size(intermcells_m, 1)
|
||||
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
|
||||
end;
|
||||
|
||||
plot(intermcells_m(:,1), intermcells_m(:,2));
|
||||
axis([-0.3 0.3 -0.3 0.3]);
|
||||
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
|
||||
hold on;
|
||||
|
||||
end;
|
||||
grid;
|
||||
pause;
|
||||
end;
|
||||
|
||||
fclose('all');
|
||||
741
navigations/sbpl/matlab/mprim/genmprim_unicycle_circular_non_uniform.m
Executable file
741
navigations/sbpl/matlab/mprim/genmprim_unicycle_circular_non_uniform.m
Executable file
@@ -0,0 +1,741 @@
|
||||
function[] = genmprim_unicycle_circular_non_uniform (outfilename)
|
||||
% Generates circular motion primitives with min turning radius and saves them into a file.
|
||||
% For each angle from the list_of_angles array generates a set of 4 forward moving motion primitives
|
||||
% and 4 symmetric backward moving motion primitives.
|
||||
%
|
||||
% This function takes one optional argument - 'outfilename', which specifies
|
||||
% the location of the output file where motion primitives data is saved.
|
||||
% If this argiment is omitted, the file name is hardcoded.
|
||||
%
|
||||
% The script builds the following types of motion primitives with unique ids for each
|
||||
% angle from the list_of_angles array:
|
||||
%
|
||||
% if has_turn_in_place_prims == 0
|
||||
%
|
||||
% 1 - short forward
|
||||
% 2 - long forward
|
||||
% 3 - forward and turn left
|
||||
% 4 - forward and turn right
|
||||
% 5 - short backward
|
||||
% 6 - long backward
|
||||
% 7 - backward and turn left
|
||||
% 8 - backward and turn right
|
||||
%
|
||||
% if has_turn_in_place_prims == 1
|
||||
%
|
||||
% 1 - short forward
|
||||
% 2 - long forward
|
||||
% 3 - forward and turn left
|
||||
% 4 - forward and turn right
|
||||
% 5 - turn in place forward
|
||||
% 6 - short backward
|
||||
% 7 - long backward
|
||||
% 8 - backward and turn left
|
||||
% 9 - backward and turn right
|
||||
% 10 - turn in place backward
|
||||
%
|
||||
%
|
||||
% Configurarion parameters:
|
||||
% resolution grid resolution
|
||||
% errmin max acceptable error for the distance between the endpoint of the motion primitive and the grid node.
|
||||
% rmin_m min acceptable turning radius im meters
|
||||
% xmin min x coordinate for the endpoint of the circular motion primitive
|
||||
% ymin min y coordinate for the endpoint of the circular motion primitive
|
||||
% xmax max x coordinate for the endpoint of the circular motion primitive
|
||||
% ymax max y coordinate for the endpoint of the circular motion primitive
|
||||
% countmax max number of different acceptable (by errmin and rmax) primitives of different lengths to calculate before choosing the best among them.
|
||||
%
|
||||
% alfa the angle satisfying the condition: alfa = atan(0.5)= 26.56505118 degrees, 0.46364761 radians
|
||||
% numberofangles number of angles for which motion primitives are generated
|
||||
% number_of_base_prims_per_angle number of base (forward moving) primitives per angle
|
||||
% numberofbaseangles number of base angles: 0, alfa and 45
|
||||
% degrees. All other angles are rotations or reflections of these base angles.
|
||||
% has_backward_prims can be 1 or 0, if 0, backward moving primitives are not generated.
|
||||
% list_if_angles list of angles for which sets of motion primitives are built
|
||||
% degrees: 0 26.5651 45.00 63.4349 90.00 116.5651 135.00 153.4349 180.00 206.5651 225.00 243.4349 270.00 296.5651 315.00 333.4349 360.00
|
||||
% radians: 0 0.4538 0.7854 1.1170 1.5708 2.0246 2.3562 2.6878 3.1416 3.5954 3.9270 4.2586 4.7124 5.1662 5.4978 5.8294 6.2832
|
||||
%
|
||||
% Cost multipliers (multiplier is used as costmult*cost)
|
||||
% forwardcostmult cost multiplier for the forward motion
|
||||
% backwardcostmult cost multiplier for the backward motion
|
||||
% forwardandturncostmult cost multiplier for the forward-and-turn motion
|
||||
% backwardandturncostmult cost multiplier for the backward-and-turn motion
|
||||
%
|
||||
% This script prints 3 coordinates for each motion primitive: x, y, theta
|
||||
%%%%%
|
||||
|
||||
fprintf(1, 'genmprim_unicycle_circular_non_uniform\n');
|
||||
|
||||
% configuration parameters
|
||||
resolution = 0.1; %0.025; %0.1;
|
||||
errmin = 0.05; %0.05;
|
||||
rmin_m = 3.0; %1.0;
|
||||
rmin_c = rmin_m/resolution;
|
||||
fprintf(1, 'resolution=%f rmin_m=%f meters rmin_c=%f cells errmin=%f\n', resolution, rmin_m, rmin_c, errmin);
|
||||
|
||||
if nargin < 1
|
||||
%outputFile = 0;
|
||||
basefilename = 'non_uniform_';
|
||||
outfilename = generate_file_name(basefilename, resolution, rmin_m, errmin);
|
||||
fprintf(1, 'outfilename = %s\n', outfilename);
|
||||
end
|
||||
|
||||
xmin = 0;
|
||||
ymin = 0;
|
||||
xmax = 40;
|
||||
ymax = 40;
|
||||
countmax = 40;
|
||||
interm_spacing_m = resolution/2; % Approximate spacing of intermediate poses in meters
|
||||
interm_spacing_c = interm_spacing_m/resolution;
|
||||
interm_spacing_rad = 0.05; % Approximate spacing of intermediate poses in radians
|
||||
fprintf(1, 'xmin=%f xmax=%f ymin=%f ymax=%f\n', xmin, xmax, ymin, ymax);
|
||||
fprintf(1, 'countmax=%d interm_spacing_m=%f meters interm_spacing_rad=%f radians\n', countmax, interm_spacing_m, interm_spacing_rad);
|
||||
|
||||
numberofangles = 16;
|
||||
%number_of_base_prims_per_angle = 4;
|
||||
%max_number_of_prims_per_angle = 8;
|
||||
numberofbaseangles = 3; % 0, alfa and 45 degrees
|
||||
has_backward_prims = 1;
|
||||
has_turn_in_place_prims = 1;
|
||||
number_of_base_prims_per_angle = 4 + has_turn_in_place_prims;
|
||||
max_number_of_prims_per_angle = 2 * number_of_base_prims_per_angle; %this is only needed for arrays memory allocations
|
||||
numberofprimsperangle = (1 + has_backward_prims) * number_of_base_prims_per_angle; % This is the total number of primitives per angle
|
||||
|
||||
totalnumberofprimitives = numberofprimsperangle*numberofangles;
|
||||
fprintf(1, 'numberofangles=%d number_of_base_prims_per_angle=%d\n', numberofangles, number_of_base_prims_per_angle);
|
||||
fprintf(1, 'has_backward_prims=%d\n', has_backward_prims);
|
||||
fprintf(1, 'has_turn_in_place_prims=%d\n', has_turn_in_place_prims);
|
||||
fprintf(1, 'numberofprimsperangle=%d totalnumberofprimitives=%d\n', numberofprimsperangle, totalnumberofprimitives);
|
||||
|
||||
start_x = 0.0;
|
||||
start_y = 0.0;
|
||||
|
||||
%multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1;
|
||||
backwardcostmult = 5;
|
||||
forwardandturncostmult = 2;
|
||||
backwardandturncostmult = 3;
|
||||
turninplacecostmult = 5;
|
||||
|
||||
costmult = []
|
||||
if (has_turn_in_place_prims)
|
||||
costmult = [forwardcostmult, forwardcostmult, forwardandturncostmult, forwardandturncostmult, turninplacecostmult, backwardcostmult, backwardcostmult, backwardandturncostmult, backwardandturncostmult, turninplacecostmult];
|
||||
else
|
||||
costmult = [forwardcostmult, forwardcostmult, forwardandturncostmult, forwardandturncostmult, tbackwardcostmult, backwardcostmult, backwardandturncostmult, backwardandturncostmult];
|
||||
end;
|
||||
|
||||
% list of angles increments in discrete units for the end pose of the primitive relative to the start pose
|
||||
prim_angle_increments = [0, 0, 1, -1];
|
||||
|
||||
alfa = 26.56505118;
|
||||
alfa1 = alfa*100000000;
|
||||
beta = 90 - alfa;
|
||||
beta1 = beta*100000000;
|
||||
imax = has_backward_prims + 1;
|
||||
fprintf(1, 'alfa1=%f beta=%f beta1=%f imax=%d\n', alfa1, beta, beta1, imax);
|
||||
|
||||
%list_of_angles (degrees): 0 26.5651 45.00 63.4349 90.00 116.5651 135.00 153.4349 180.00 206.5651 225.00 243.4349 270.00 296.5651 315.00 333.4349 360.00
|
||||
%list_of_angles (radians): 0 0.4538 0.7854 1.1170 1.5708 2.0246 2.3562 2.6878 3.1416 3.5954 3.9270 4.2586 4.7124 5.1662 5.4978 5.8294 6.2832
|
||||
fprintf(1, '*******************************\n');
|
||||
list_of_angles = zeros(1, numberofangles+1);
|
||||
list_of_angles(1:2:17) = (0:45:360);
|
||||
list_of_angles([2 6 10 14]) = list_of_angles([1 5 9 13]) + alfa;
|
||||
list_of_angles([4 8 12 16]) = list_of_angles([5 9 13 17]) - alfa;
|
||||
list_of_angles
|
||||
list_of_angles = list_of_angles .* pi/180;
|
||||
list_of_angles
|
||||
fprintf(1, '*******************************\n');
|
||||
|
||||
fout = fopen(outfilename, 'w');
|
||||
%write the header
|
||||
fprintf(fout, 'resolution_m: %f\n', resolution);
|
||||
fprintf(fout, 'min_turning_radius_m: %f\n', rmin_m);
|
||||
fprintf(fout, 'numberofangles: %d\n', numberofangles);
|
||||
for angleind = 1:numberofangles
|
||||
fprintf(fout, 'angle:%d %.8f\n', angleind-1, list_of_angles(angleind));
|
||||
end;
|
||||
fprintf(fout, 'totalnumberofprimitives: %d\n', totalnumberofprimitives);
|
||||
|
||||
% Note, what is shown x,y,theta changes (not absolute numbers) x aligned
|
||||
% with the heading of the robot, angles are positive and counterclockwise
|
||||
|
||||
% Initialize arrays to store acceptable circular primitives before choosing the best among them.
|
||||
% We only need 3 base sets of primitives for angles 0, alfa and 45 degrees.
|
||||
primdata = zeros(numberofbaseangles, max_number_of_prims_per_angle, countmax, 8); % startx, starty, starttheta, endx, endy, endtheta, tvoverrv, rv
|
||||
primcount = zeros(numberofbaseangles, max_number_of_prims_per_angle);
|
||||
basemprimendpts_c = zeros(numberofbaseangles, numberofprimsperangle, 6); % x_c, y_c, theta_c, costmult, tvoverrv, rv
|
||||
|
||||
% Calculate sets of valid primitives with ids 2,3,4 for base angles 0, alfa and 45 degrees.
|
||||
% Primitives with id=1 (short forward) will be calculated by re-scaling primitives with id=2 (long forward).
|
||||
for angleind=1:3
|
||||
for primind=2:4
|
||||
if (angleind == 1 && primind == 4)
|
||||
gen_base_prims(angleind, primind, xmin, xmax, -ymax, -ymin);
|
||||
else
|
||||
gen_base_prims(angleind, primind, xmin, xmax, ymin, ymax);
|
||||
end;
|
||||
end;
|
||||
end;
|
||||
|
||||
fprintf(1, '\n');
|
||||
fprintf(1, 'find best primitives start:\n');
|
||||
find_best_prims();
|
||||
|
||||
fprintf(1, '\n++++++++++++++++++\n');
|
||||
fprintf(1, 'start writing primitives to file:\n');
|
||||
fprintf(1, '\n');
|
||||
|
||||
h1f = figure(1);
|
||||
h1a = axes('parent',h1f);
|
||||
hold on;
|
||||
|
||||
s = 2.0; %0.6;
|
||||
xxmin = -s;
|
||||
xxmax = s;
|
||||
yymin = -s;
|
||||
yymax = s;
|
||||
xt = 5*resolution;
|
||||
yt = 5*resolution;
|
||||
|
||||
axis equal;
|
||||
axis([xxmin xxmax yymin yymax]);
|
||||
%axis([-0.5 0.5 -0.5 0.5]);
|
||||
set(gca,'XTick', xxmin:xt:xxmax);
|
||||
set(gca,'YTick',yymin:yt:yymax);
|
||||
grid on;
|
||||
|
||||
primposes = [];
|
||||
primsamples = [];
|
||||
primendangle = [];
|
||||
|
||||
% Iterate over angles
|
||||
for angleind = 1:numberofangles
|
||||
fprintf(1, '***************\n');
|
||||
fprintf(1, 'angleind=%d\n', angleind-1);
|
||||
|
||||
currentangle = list_of_angles(angleind);
|
||||
currentangle_36000int = round(currentangle*18000000000/pi);
|
||||
|
||||
fprintf(1, '--------------------\n');
|
||||
fprintf(1, 'currentangle=%f currentangle_36000int=%d\n', currentangle, currentangle_36000int);
|
||||
|
||||
numofsamples0 = zeros(numberofprimsperangle);
|
||||
endPoses_c = zeros(numberofprimsperangle, 3); % x_c, y_c, theta_c
|
||||
turningradiuses = zeros(numberofprimsperangle);
|
||||
intermposes_m = [];
|
||||
intermcells_m_orig = []; %zeros(numofsamples, 3); %x,y,theta
|
||||
|
||||
for primind = 1:numberofprimsperangle %number_of_base_prims_per_angle %[1,3,4]
|
||||
|
||||
fprintf(1, '+++++++++++++++++++++++++++\n');
|
||||
fprintf(1, 'primind=%d\n', primind-1);
|
||||
|
||||
%compute which template to use
|
||||
if (rem(currentangle_36000int, 9000000000) == 0)
|
||||
mprimendpts_c = basemprimendpts_c(1, primind,:);
|
||||
angle = currentangle;
|
||||
turning_radius = mprimendpts_c(5);
|
||||
rvel = mprimendpts_c(6);
|
||||
fprintf(1, '0000 angle=%f\n', angle);
|
||||
elseif (rem(currentangle_36000int, 4500000000) == 0)
|
||||
mprimendpts_c = basemprimendpts_c(3, primind,:);
|
||||
angle = currentangle - 45*pi/180;
|
||||
turning_radius = mprimendpts_c(5);
|
||||
rvel = mprimendpts_c(6);
|
||||
fprintf(1, '4500 angle=%f\n', angle);
|
||||
elseif (rem(currentangle_36000int-beta1, 9000000000) == 0) %6400
|
||||
mprimendpts_c = basemprimendpts_c(2, primind,:);
|
||||
mprimendpts_c(1) = basemprimendpts_c(2, primind,2); %reeverse x and y
|
||||
mprimendpts_c(2) = basemprimendpts_c(2, primind,1);
|
||||
mprimendpts_c(3) = -basemprimendpts_c(2, primind,3); %reverse the angle as well
|
||||
angle = currentangle - beta*pi/180;
|
||||
turning_radius = -mprimendpts_c(5);
|
||||
rvel = -mprimendpts_c(6);
|
||||
fprintf(1, '90-alfa angle=%f\n', angle);
|
||||
elseif (rem(currentangle_36000int-alfa1, 9000000000) == 0)
|
||||
mprimendpts_c = basemprimendpts_c(2, primind,:);
|
||||
angle = currentangle - alfa*pi/180;
|
||||
turning_radius = mprimendpts_c(5);
|
||||
rvel = mprimendpts_c(6);
|
||||
fprintf(1, 'alfa angle=%f\n', angle);
|
||||
else
|
||||
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
|
||||
return;
|
||||
end;
|
||||
|
||||
[res, turning_radius_center] = generate_intermediate_poses(angleind, primind, currentangle, angle, rvel, turning_radius, mprimendpts_c);
|
||||
|
||||
for ii = 1:numofsamples
|
||||
intermposes_m(primind, ii, :) = [intermcells_m(primind,ii,1)*resolution intermcells_m(primind,ii,2)*resolution intermcells_m(primind,ii,3)]; % x, y, theta
|
||||
%intermposes_m_orig(primind, iind, :) = [sgn*intermcells_m_orig(iind,1) sgn*intermcells_m_orig(iind,2) intermcells_m(iind,3)]; % x, y, theta
|
||||
primposes(angleind, primind, ii, :) = intermposes_m(primind, ii, :);
|
||||
end;
|
||||
%pause;
|
||||
|
||||
end; %for primind = 1:numberofprimsperangle
|
||||
|
||||
%output to the mprim file and plot
|
||||
for primind = 1:numberofprimsperangle
|
||||
fprintf(fout, 'primID: %d\n', primind-1);
|
||||
fprintf(fout, 'startangle_c: %d\n', angleind-1);
|
||||
fprintf(fout, 'endpose_c: %d %d %d\n', endPoses_c(primind,1), endPoses_c(primind,2), endPoses_c(primind,3));
|
||||
|
||||
additionalcostmult = costmult(primind);
|
||||
|
||||
fprintf(fout, 'additionalactioncostmult: %d\n', additionalcostmult);
|
||||
fprintf(fout, 'turning_radius: %.4f\n', turningradiuses(primind));
|
||||
|
||||
numofsamples = numofsamples0(primind);
|
||||
|
||||
fprintf(fout, 'intermediateposes: %d\n', numofsamples);
|
||||
for interind = 1:numofsamples
|
||||
fprintf(fout, '%.4f %.4f %.4f\n', intermposes_m(primind,interind,1), intermposes_m(primind,interind,2), intermposes_m(primind,interind,3));
|
||||
end;
|
||||
plot(squeeze(intermposes_m(primind, 1:numofsamples, 1)), squeeze(intermposes_m(primind, 1:numofsamples, 2)), 'r');
|
||||
|
||||
dl = 0.0;
|
||||
if (primind == 3)
|
||||
dl = 0.05;
|
||||
elseif (primind == 4)
|
||||
dl = -0.05;
|
||||
end;
|
||||
slabel = [int2str(angleind), '-', int2str(primind)];
|
||||
%text(intermposes_m(primind,numofsamples,1)+dl, intermposes_m(primind,numofsamples,2)+dl, slabel);
|
||||
|
||||
end; %primid
|
||||
%pause;
|
||||
end; %angleid
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%******************************
|
||||
|
||||
fprintf(1, '!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n');
|
||||
fclose('all');
|
||||
|
||||
function[] = gen_base_prims(angle_ind, prim_ind, xmin0, xmax0, ymin0, ymax0)
|
||||
|
||||
fprintf(1, '\ngen_base_prims: angle_ind=%d prim_ind=%d\n', angle_ind, prim_ind);
|
||||
angle_incr = prim_angle_increments(prim_ind);
|
||||
start_theta = list_of_angles(angle_ind);
|
||||
if (angle_ind == 1 && angle_incr == -1)
|
||||
end_theta = -list_of_angles(2);
|
||||
else
|
||||
end_theta = list_of_angles(angle_ind + angle_incr);
|
||||
end;
|
||||
|
||||
for i=1:imax
|
||||
count = 0;
|
||||
if (i == 1)
|
||||
xmn0 = xmin0;
|
||||
xmx0 = xmax0;
|
||||
ymn0 = ymin0;
|
||||
ymx0 = ymax0;
|
||||
prm_ind = prim_ind;
|
||||
else
|
||||
xmn0 = -xmax0;
|
||||
xmx0 = -xmin0;
|
||||
ymn0 = -ymax0;
|
||||
ymx0 = -ymin0;
|
||||
prm_ind = prim_ind + number_of_base_prims_per_angle; %4;
|
||||
end;
|
||||
basemprimendpts_c(angle_ind, prm_ind, 3) = angle_incr;
|
||||
|
||||
fprintf(1, '\nangle_ind=%d prm_ind=%d\n', angle_ind, prm_ind);
|
||||
fprintf(1, 'start_theta=%f end_theta=%f basemprimendpts_c(3)=%f angle_incr=%d\n', start_theta, end_theta, basemprimendpts_c(angle_ind,prim_ind,3), angle_incr);
|
||||
|
||||
%fprintf(1,'xmn0=%f xmx0=%f\n', xmn0,xmx0);
|
||||
%fprintf(1,'ymn0=%f ymx0=%f\n', ymn0,ymx0);
|
||||
|
||||
for endx_c = xmn0:xmx0
|
||||
for endy_c = ymn0:ymx0
|
||||
prim_endx_c = endx_c;
|
||||
prim_endy_c = endy_c;
|
||||
|
||||
if (prim_ind == 3 || prim_ind == 4)
|
||||
R = [sin(end_theta) - sin(start_theta); cos(start_theta) - cos(end_theta)];
|
||||
RT = R';
|
||||
B = [prim_endx_c - start_x; prim_endy_c - start_y];
|
||||
tvoverrv = pinv(RT*R)*RT*B;
|
||||
if (abs(tvoverrv) < rmin_c)
|
||||
continue;
|
||||
end;
|
||||
|
||||
rv = end_theta - start_theta;
|
||||
tv = rv*tvoverrv;
|
||||
err = abs(R*tvoverrv - B);
|
||||
errxy = norm(err);
|
||||
|
||||
if (errxy < errmin)
|
||||
fprintf(1, 'count=%d endx_c=%d endy_c=%d errxy=%f tvoverrv=%f rv=%f tv=%f prim_endx_c=%f prim_endy_c=%f\n', count, endx_c, endy_c, errxy, tvoverrv, rv, tv, prim_endx_c, prim_endy_c);
|
||||
end
|
||||
else
|
||||
yy = prim_endx_c*tan(end_theta);
|
||||
errxy = abs(prim_endy_c - yy);
|
||||
tvoverrv = sqrt(prim_endx_c*prim_endx_c + yy*yy);
|
||||
rv = 0.0;
|
||||
tv = tvoverrv;
|
||||
% if (errxy < 2*errmin)
|
||||
% fprintf(1, 'count=%d endx_c=%d endy_c=%d errxy=%f tvoverrv=%f rv=%f tv=%f prim_endx_c=%f prim_endy_c=%f\n', count, endx_c, endy_c, errxy, tvoverrv, rv, tv, prim_endx_c, prim_endy_c);
|
||||
% end
|
||||
end;
|
||||
|
||||
if (errxy > errmin)
|
||||
continue;
|
||||
end;
|
||||
|
||||
if (count+1 > countmax)
|
||||
fprintf(1, 'endy: count = %d, countmax = %d\n', count, countmax);
|
||||
break;
|
||||
end;
|
||||
count = count + 1;
|
||||
|
||||
primdata(angle_ind, prm_ind, count, 1) = start_x;
|
||||
primdata(angle_ind, prm_ind, count, 2) = start_y;
|
||||
primdata(angle_ind, prm_ind, count, 3) = start_theta;
|
||||
primdata(angle_ind, prm_ind, count, 4) = prim_endx_c;
|
||||
primdata(angle_ind, prm_ind, count, 5) = prim_endy_c;
|
||||
primdata(angle_ind, prm_ind, count, 6) = end_theta;
|
||||
primdata(angle_ind, prm_ind, count, 7) = tvoverrv;
|
||||
primdata(angle_ind, prm_ind, count, 8) = rv;
|
||||
%pause;
|
||||
end; %for end_y
|
||||
|
||||
if (count+1 > countmax)
|
||||
fprintf(1, 'endx: count = %d, countmax = %d\n', count, countmax);
|
||||
break;
|
||||
end;
|
||||
|
||||
end; %for end_x
|
||||
primcount(angle_ind, prm_ind) = count;
|
||||
fprintf(1, 'count=%f\n', primcount(angle_ind, prm_ind));
|
||||
|
||||
end %for i=1:imax
|
||||
end
|
||||
|
||||
function[] = find_best_prims()
|
||||
% First find the motion primitive for angle_ind = 1 and prim_ind = 4
|
||||
% with minimal turning radius among selected set of primitives that
|
||||
% satisfy the given min turning radius and epsilon conditions.
|
||||
|
||||
[rmin0, countmin] = min(abs(primdata(1, 4, 1:primcount(1, 4), 7)));
|
||||
fprintf(1, 'countmin=%f rmin0=%f\n', countmin, rmin0);
|
||||
|
||||
%Iterate over all saved motion primitives for all angles and for each
|
||||
%angle find a set of primitives with turning radius closest to the min radius found above.
|
||||
|
||||
rrm = rmin0;
|
||||
%rrm = 30.0;
|
||||
%rad = [];
|
||||
|
||||
for angle_id=1:numberofbaseangles
|
||||
|
||||
fprintf(1, '************* angle_id=%f *********\n', angle_id);
|
||||
|
||||
i1 = number_of_base_prims_per_angle + 3; %7;
|
||||
i2 = number_of_base_prims_per_angle + 4; %8;
|
||||
|
||||
for prim_ind = [3,4,i1,i2] %4:-1:3
|
||||
if (has_backward_prims == 0 && prim_ind > number_of_base_prims_per_angle) %>4
|
||||
continue;
|
||||
end
|
||||
|
||||
fprintf(1, '------------- prim_ind=%f -----------\n', prim_ind);
|
||||
|
||||
% [dmin, countmin] = min(abs(abs(primdata(angle_id, prim_ind, 1:primcount(angle_id, prim_ind), 7)) - rmin0));
|
||||
% if (angle_id == 1)
|
||||
% rrm = rmin0;
|
||||
% elseif (prim_ind == 3)
|
||||
% rrm = rad(angle_id, 4);
|
||||
% else
|
||||
% rrm = rad(angle_id-1, 3);
|
||||
% end;
|
||||
fprintf(1, 'rrm=%f\n', rrm);
|
||||
|
||||
fprintf(1, 'primcount(angle_id, prim_ind)=%d\n', primcount(angle_id, prim_ind));
|
||||
|
||||
[dmin, countmin] = min(abs(abs(primdata(angle_id, prim_ind, 1:primcount(angle_id, prim_ind), 7)) - rrm));
|
||||
fprintf(1, 'countmin=%f dmin=%f\n', countmin, dmin);
|
||||
|
||||
tvoverrv = primdata(angle_id, prim_ind, countmin, 7);
|
||||
%rad(angle_id, prim_ind) = abs(tvoverrv);
|
||||
|
||||
rv = primdata(angle_id, prim_ind, countmin, 6) - primdata(angle_id, prim_ind, countmin, 3);
|
||||
tv = rv*tvoverrv;
|
||||
fprintf(1, 'tvoverrv=%f rv=%f tv=%f\n', tvoverrv, rv, tv);
|
||||
|
||||
basemprimendpts_c(angle_id, prim_ind, 1) = primdata(angle_id, prim_ind, countmin, 4);
|
||||
basemprimendpts_c(angle_id, prim_ind, 2) = primdata(angle_id, prim_ind, countmin, 5);
|
||||
basemprimendpts_c(angle_id, prim_ind, 5) = primdata(angle_id, prim_ind, countmin, 7);
|
||||
basemprimendpts_c(angle_id, prim_ind, 6) = primdata(angle_id, prim_ind, countmin, 8);
|
||||
|
||||
fprintf(1, 'basemprimendpts_c=%f %f %f %f %f %f\n', basemprimendpts_c(angle_id, prim_ind, :)); %x, y, angle_incr, costmult,tvoverrv, rv
|
||||
end;
|
||||
|
||||
for i = 1:imax
|
||||
%prim_ind = 2; %long forward
|
||||
prim_ind = 2 + (i-1)*number_of_base_prims_per_angle; %4;
|
||||
fprintf(1, '------------- prim_ind=%f -----------\n', prim_ind);
|
||||
i1 = prim_ind+1;
|
||||
i2 = prim_ind+2;
|
||||
|
||||
x = (basemprimendpts_c(angle_id, i1, 1) + basemprimendpts_c(angle_id, i2, 1))/2;
|
||||
y = (basemprimendpts_c(angle_id, i1, 2) + basemprimendpts_c(angle_id, i2, 2))/2;
|
||||
fprintf(1, 'x=%f y=%f primcount=%d\n', x, y, primcount(angle_id, prim_ind));
|
||||
|
||||
[dmin, countmin] = min(sqrt((primdata(angle_id, prim_ind, 1:primcount(angle_id, prim_ind), 4)-x).^2 + (primdata(angle_id, prim_ind, 1:primcount(angle_id, prim_ind), 5)-y).^2));
|
||||
|
||||
fprintf(1, 'angle_id=%d prim_ind=%d countmin=%d dmin=%f\n', angle_id, prim_ind, countmin, dmin);
|
||||
basemprimendpts_c(angle_id, prim_ind, 1) = primdata(angle_id, prim_ind, countmin, 4);
|
||||
basemprimendpts_c(angle_id, prim_ind, 2) = primdata(angle_id, prim_ind, countmin, 5);
|
||||
fprintf(1, 'basemprimendpts_c=%f %f %f %f %f %f\n', basemprimendpts_c(angle_id, prim_ind, :));
|
||||
|
||||
%prim_ind = 1; %short forward
|
||||
i1 = prim_ind;
|
||||
prim_ind = prim_ind - 1;
|
||||
fprintf(1, '------------- prim_ind=%f -----------\n', prim_ind);
|
||||
x = basemprimendpts_c(angle_id, i1, 1);
|
||||
y = basemprimendpts_c(angle_id, i1, 2);
|
||||
d = nonzero_min(abs(x), abs(y));
|
||||
basemprimendpts_c(angle_id, prim_ind, 1) = x/d;
|
||||
basemprimendpts_c(angle_id, prim_ind, 2) = y/d;
|
||||
fprintf(1, 'basemprimendpts_c=%f %f %f %f %f %f\n', basemprimendpts_c(angle_id, prim_ind, :));
|
||||
|
||||
%prim_ind = 5; %short forward
|
||||
if (has_turn_in_place_prims == 1)
|
||||
prim_ind = 5 + (i-1)*number_of_base_prims_per_angle;
|
||||
fprintf(1, '------------- prim_ind=%f -----------\n', prim_ind);
|
||||
basemprimendpts_c(angle_id, prim_ind, 1) = 0.0;
|
||||
basemprimendpts_c(angle_id, prim_ind, 2) = 0.0;
|
||||
if (prim_ind == 5)
|
||||
i1 = prim_ind-2;
|
||||
else
|
||||
i1 = prim_ind-1;
|
||||
end;
|
||||
basemprimendpts_c(angle_id, prim_ind, 3) = basemprimendpts_c(angle_id, i1, 3);
|
||||
basemprimendpts_c(angle_id, prim_ind, 5) = 0.0;
|
||||
basemprimendpts_c(angle_id, prim_ind, 6) = basemprimendpts_c(angle_id, i1, 6);
|
||||
fprintf(1, 'basemprimendpts_c=%f %f %f %f %f %f\n', basemprimendpts_c(angle_id, prim_ind, :));
|
||||
end
|
||||
end;
|
||||
end;
|
||||
end
|
||||
|
||||
%generate intermediate poses (remember they are w.r.t 0,0 (and not centers of the cells)
|
||||
function[res, turning_radius_center] = generate_intermediate_poses(angleind, primind, currentangle, angle, rvel, turning_radius, mprimendpts_c)
|
||||
|
||||
baseendpose_c = mprimendpts_c(1:3);
|
||||
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
|
||||
|
||||
fprintf(1,'\n');
|
||||
fprintf(1, 'generate_intermediate_poses start\n');
|
||||
fprintf(1, 'baseendpose_c: %f %f %f endtheta_c=%d\n', baseendpose_c, endtheta_c);
|
||||
startpt = [0 0 currentangle];
|
||||
|
||||
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
|
||||
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
|
||||
if (angleind == 1 && endtheta_c == -1)
|
||||
end_angle = -list_of_angles(2);
|
||||
else
|
||||
end_angle = list_of_angles(endtheta_c + 1);
|
||||
end;
|
||||
endpt = [endx_c endy_c end_angle];
|
||||
|
||||
dist_m = sqrt(sum((endpt(1:2) - startpt(1:2)).^2));
|
||||
dist_rad = abs(currentangle - end_angle);
|
||||
fprintf(1, 'currentangle=%f end_angle=%f\n', currentangle, end_angle);
|
||||
|
||||
if (endx_c == 0 && endy_c == 0)
|
||||
numofsamples = abs(round(dist_rad/interm_spacing_rad));
|
||||
else
|
||||
numofsamples = floor(dist_m/interm_spacing_c) + 1;
|
||||
end;
|
||||
%numofsamples = 1 + max(round(dist_m/interm_spacing_m), abs(round(dist_rad/interm_spacing_rad)));
|
||||
fprintf(1, 'endx_c=%f endy_c=%f endtheta_c=%d turning_radius=%f rvel=%f transformation angle=%f end_angle=%f numofsamples=%d dist_m=%f dist_rad=%f\n', endx_c, endy_c, endtheta_c, turning_radius, rvel, angle*180/pi, end_angle, numofsamples, dist_m, dist_rad);
|
||||
numofsamples0(primind) = numofsamples;
|
||||
|
||||
if (baseendpose_c(3) == 0) %move forward/backward
|
||||
for iind = 1:numofsamples
|
||||
dt = (iind-1)/(numofsamples-1);
|
||||
intermcells_m(primind, iind,:) = [startpt(1) + (endpt(1) - startpt(1))*dt ...
|
||||
startpt(2) + (endpt(2) - startpt(2))*dt ...
|
||||
rem(startpt(3) + (endpt(3) - startpt(3))*dt, 2*pi)];
|
||||
end;
|
||||
res = correct_poses();
|
||||
turning_radius_center = 0.0;
|
||||
elseif (endx_c == 0 && endy_c == 0) %turn in place
|
||||
for iind = 1:numofsamples
|
||||
dt = (iind-1)/(numofsamples-1);
|
||||
dtheta = rvel*dt + startpt(3);
|
||||
intermcells_m(primind, iind,:) = [0.0 0.0 dtheta];
|
||||
end;
|
||||
res = correct_poses();
|
||||
turning_radius_center = 0.0;
|
||||
else
|
||||
for iind = 1:numofsamples
|
||||
dt = (iind-1)/(numofsamples-1);
|
||||
|
||||
dtheta = rvel*dt + startpt(3);
|
||||
x = startpt(1) + turning_radius*(sin(dtheta) - sin(startpt(3)));
|
||||
y = startpt(2) + turning_radius*(cos(startpt(3)) - cos(dtheta));
|
||||
intermcells_m(primind, iind,:) = [x y dtheta];
|
||||
% fprintf(1, 'iind=%d dt=%f x=%f y=%f dtheta=%f %f %f\n', iind, dt, intermcells_m(iind,1), intermcells_m(iind,2), dtheta, x, y);
|
||||
end;
|
||||
res = correct_poses();
|
||||
[turning_radius_center, arc_len_m, turn_angle] = average_turning_radius();
|
||||
end;
|
||||
%pause;
|
||||
|
||||
k = primind;
|
||||
endPoses_c(k, :) = [round(endx_c) round(endy_c) round(endtheta_c)];
|
||||
turningradiuses(k) = turning_radius*resolution;
|
||||
turningradiuses_center(k) = turning_radius_center;
|
||||
|
||||
primsamples(angleind, k) = numofsamples;
|
||||
primendangle(angleind, k) = endtheta_c;
|
||||
%pause;
|
||||
|
||||
function[turn_rad_aver, arc_len_m, turn_angle] = average_turning_radius()
|
||||
|
||||
turn_angle = 0.0;
|
||||
turn_rad_aver = 0.0;
|
||||
arc_len_m = 0.0;
|
||||
for ind = 1:numofsamples
|
||||
if (ind > 1)
|
||||
dx = intermcells_m(primind, ind,1) - intermcells_m(primind, ind-1,1);
|
||||
dy = intermcells_m(primind, ind,2) - intermcells_m(primind, ind-1,2);
|
||||
ds = sqrt(dx*dx + dy*dy);
|
||||
dth = (intermcells_m(primind, ind,3) - intermcells_m(primind, ind-1,3));
|
||||
arc_len_m = arc_len_m + ds;
|
||||
turn_angle = turn_angle + dth;
|
||||
turn_rad = ds/dth;
|
||||
turn_rad_aver = turn_rad_aver + abs(turn_rad);
|
||||
%fprintf(1, 'ind=%d ds=%f dth=%f turn_rad=%f\n', ind, ds, dth, turn_rad);
|
||||
end;
|
||||
end
|
||||
turn_rad_aver = turn_rad_aver/(numofsamples - 1);
|
||||
turn_angle = abs(turn_angle);
|
||||
fprintf('arc_len_m = %f turn_angle = %f turn_rad_aver = %f\n', arc_len_m, turn_angle, turn_rad_aver);
|
||||
end
|
||||
|
||||
%correct intermediate poses using interpolation to place the end point of the motion primitive exactly to the grid node.
|
||||
function[status] = correct_poses()
|
||||
|
||||
status = true;
|
||||
%error = [endpt(1) - intermcells_m(primind, numofsamples,1) endpt(2) - intermcells_m(primind, numofsamples,2) endpt(3) - intermcells_m(primind, numofsamples,3)];
|
||||
errx = endpt(1) - intermcells_m(primind, numofsamples,1);
|
||||
erry = endpt(2) - intermcells_m(primind, numofsamples,2);
|
||||
errth = endpt(3) - intermcells_m(primind, numofsamples,3);
|
||||
|
||||
fprintf(1, '********************\n');
|
||||
fprintf(1, 'correction:\n');
|
||||
fprintf(1,'numofsamples = %d\n', numofsamples);
|
||||
fprintf(1, 'startpt(1)=%f startpt(2)=%f startpt(3)=%f\n', startpt(1), startpt(2), startpt(3));
|
||||
fprintf(1, 'endpt(1)=%f endpt(2)=%f endpt(3)=%f\n', endpt(1), endpt(2), endpt(3));
|
||||
fprintf(1, 'mprimendpts_c(1)=%f mprimendpts_c(2)=%f mprimendpts_c(3)=%f\n', mprimendpts_c(1), mprimendpts_c(2), mprimendpts_c(3));
|
||||
|
||||
fprintf(1, 'error: dx %f dy %f dtheta %f\n', errx, erry, errth);
|
||||
|
||||
R0 = [sin(end_angle) - sin(startpt(3)); cos(startpt(3)) - cos(end_angle)];
|
||||
RT0 = R0';
|
||||
B0 = [endpt(1) - start_x; endpt(2) - start_y];
|
||||
tr = abs(pinv(RT0*R0)*RT0*B0);
|
||||
fprintf(1, 'tr = %f\n', tr);
|
||||
|
||||
if (tr > 0 && tr < rmin_m)
|
||||
fprintf(1, 'WARNING: After interpolation turning radius is less than the specified min!\n');
|
||||
status = false;
|
||||
end;
|
||||
|
||||
interpfactor = 0:1/(numofsamples-1):1;
|
||||
|
||||
%save original motion for display purposes only
|
||||
for i=1:numofsamples
|
||||
intermcells_m_orig(primind, i,1) = intermcells_m(primind, i,1);
|
||||
intermcells_m_orig(primind, i,2) = intermcells_m(primind, i,2);
|
||||
intermcells_m_orig(primind, i,3) = intermcells_m(primind, i,3);
|
||||
end;
|
||||
|
||||
%correct intermideate poses using interpolation
|
||||
for i=1:numofsamples
|
||||
intermcells_m(primind, i,1) = intermcells_m(primind, i,1) + errx*interpfactor(i);
|
||||
intermcells_m(primind, i,2) = intermcells_m(primind, i,2) + erry*interpfactor(i);
|
||||
end;
|
||||
end
|
||||
end
|
||||
|
||||
function d = nonzero_min(x,y)
|
||||
if (x == 0)
|
||||
d = y;
|
||||
elseif (y == 0)
|
||||
d = x;
|
||||
else
|
||||
d = min(x, y);
|
||||
end;
|
||||
end
|
||||
|
||||
function[x1,y1,theta1] = rotate(x,y,theta,theta0)
|
||||
x1 = x * cos(theta0) - y * sin(theta0);
|
||||
y1 = x * sin(theta0) + y * cos(theta0);
|
||||
|
||||
r = [cos(theta/2), 0.0, 0.0, sin(theta/2)];
|
||||
q = [cos(theta0/2), 0.0, 0.0, sin(theta0/2)];
|
||||
|
||||
f(1) = (r(1) * q(1)) - (r(2) * q(2)) - (r(3) * q(3)) - (r(4) * q(4));
|
||||
f(2) = (r(1) * q(2)) + (r(2) * q(1)) - (r(3) * q(4)) + (r(4) * q(3));
|
||||
f(3) = (r(1) * q(3)) + (r(2) * q(4)) + (r(3) * q(1)) - (r(4) * q(2));
|
||||
f(4) = (r(1) * q(4)) - (r(2) * q(3)) + (r(3) * q(2)) + (r(4) * q(1));
|
||||
theta1 = 2*atan2(f(4), f(1));
|
||||
end
|
||||
|
||||
function[x1,y1,theta1] = rotate_clockwise(x,y,theta,theta0)
|
||||
|
||||
r = [cos(theta/2), 0.0, 0.0, sin(theta/2)];
|
||||
q = [cos(theta0/2), 0.0, 0.0, sin(theta0/2)];
|
||||
q(4) = -q(4);
|
||||
theta00 = 2*atan2(q(4), q(1));
|
||||
|
||||
x1 = x * cos(theta00) - y * sin(theta00);
|
||||
y1 = x * sin(theta00) + y * cos(theta00);
|
||||
|
||||
f(1) = (r(1) * q(1)) - (r(2) * q(2)) - (r(3) * q(3)) - (r(4) * q(4));
|
||||
f(2) = (r(1) * q(2)) + (r(2) * q(1)) - (r(3) * q(4)) + (r(4) * q(3));
|
||||
f(3) = (r(1) * q(3)) + (r(2) * q(4)) + (r(3) * q(1)) - (r(4) * q(2));
|
||||
f(4) = (r(1) * q(4)) - (r(2) * q(3)) + (r(3) * q(2)) + (r(4) * q(1));
|
||||
theta1 = 2*atan2(f(4), f(1));
|
||||
end
|
||||
|
||||
function[x1,y1,theta1] = add_origin_and_turn(x,y,theta, x0,y0,theta0)
|
||||
[x1,y1,theta1] = rotate(x,y,theta,theta0);
|
||||
x1 = x1 + x0;
|
||||
y1 = y1 + y0;
|
||||
end
|
||||
|
||||
function[x1,y1,theta1] = remove_origin_and_turn(x,y,theta, x0,y0,theta0)
|
||||
x2 = x - x0;
|
||||
y2 = y - y0;
|
||||
[x1,y1,theta1] = rotate_clockwise(x2,y2,theta,theta0);
|
||||
end
|
||||
|
||||
function[x2,y2,theta2] = transform(x0, y0, theta0, start_theta)
|
||||
[x1,y1,theta1] = add_origin_and_turn(1.0,0.0,0.0, x0,y0,theta0);
|
||||
x2 = x1 - cos(start_theta);
|
||||
y2 = y1 - sin(start_theta);
|
||||
theta2 = theta1;
|
||||
end
|
||||
|
||||
function[outfilename] = generate_file_name(base_name, res, rmin_m, errmin)
|
||||
res_s = num2str(res, 2);
|
||||
res_s = strrep(res_s, '.', '');
|
||||
rmin_s = num2str(rmin_m, 2);
|
||||
rmin_s = strrep(rmin_s, '.', '');
|
||||
err_s = num2str(errmin, 2);
|
||||
err_s = strrep(err_s, '.', '');
|
||||
outfilename1 = strcat(base_name, 'res');
|
||||
outfilename2 = strcat(outfilename1, res_s);
|
||||
outfilename1 = strcat(outfilename2, '_rad');
|
||||
outfilename2 = strcat(outfilename1, rmin_s);
|
||||
outfilename1 = strcat(outfilename2, '_err');
|
||||
outfilename2 = strcat(outfilename1, err_s);
|
||||
outfilename = strcat(outfilename2, '.mprim');
|
||||
end
|
||||
|
||||
end
|
||||
286
navigations/sbpl/matlab/mprim/genmprim_unicycleplussideways.m
Executable file
286
navigations/sbpl/matlab/mprim/genmprim_unicycleplussideways.m
Executable file
@@ -0,0 +1,286 @@
|
||||
% /*
|
||||
% * Copyright (c) 2008, Maxim Likhachev
|
||||
% * All rights reserved.
|
||||
% *
|
||||
% * Redistribution and use in source and binary forms, with or without
|
||||
% * modification, are permitted provided that the following conditions are met:
|
||||
% *
|
||||
% * * Redistributions of source code must retain the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer.
|
||||
% * * Redistributions in binary form must reproduce the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer in the
|
||||
% * documentation and/or other materials provided with the distribution.
|
||||
% * * Neither the name of the Carnegie Mellon University nor the names of its
|
||||
% * contributors may be used to endorse or promote products derived from
|
||||
% * this software without specific prior written permission.
|
||||
% *
|
||||
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% * POSSIBILITY OF SUCH DAMAGE.
|
||||
% */
|
||||
function[] = genmprim_unicycleplussideways(outfilename)
|
||||
|
||||
%
|
||||
%generates motion primitives and saves them into file
|
||||
%
|
||||
%written by Maxim Likhachev
|
||||
%---------------------------------------------------
|
||||
%
|
||||
|
||||
%defines
|
||||
|
||||
UNICYCLE_MPRIM_16DEGS = 1;
|
||||
|
||||
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
resolution = 0.025;
|
||||
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
|
||||
numberofprimsperangle = 9;
|
||||
|
||||
%multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1;
|
||||
backwardcostmult = 5;
|
||||
forwardandturncostmult = 3;
|
||||
sidestepcostmult = 2;
|
||||
turninplacecostmult = 1;
|
||||
|
||||
%note, what is shown x,y,theta *changes* (that is, dx,dy,dtheta and not absolute numbers)
|
||||
|
||||
%0 degreees
|
||||
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
|
||||
basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts0_c(8,:) = [0 1 0 sidestepcostmult];
|
||||
basemprimendpts0_c(9,:) = [0 -1 0 sidestepcostmult];
|
||||
|
||||
%45 degrees
|
||||
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
|
||||
basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
|
||||
basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
|
||||
basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts45_c(8,:) = [-1 1 0 sidestepcostmult];
|
||||
basemprimendpts45_c(9,:) = [1 -1 0 sidestepcostmult];
|
||||
|
||||
%22.5 degrees
|
||||
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
|
||||
basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts22p5_c(8,:) = [-1 2 0 sidestepcostmult];
|
||||
basemprimendpts22p5_c(9,:) = [1 -2 0 sidestepcostmult];
|
||||
|
||||
else
|
||||
fprintf(1, 'ERROR: undefined mprims type\n');
|
||||
return;
|
||||
end;
|
||||
|
||||
|
||||
fout = fopen(outfilename, 'w');
|
||||
|
||||
|
||||
%write the header
|
||||
fprintf(fout, 'resolution_m: %f\n', resolution);
|
||||
fprintf(fout, 'numberofangles: %d\n', numberofangles);
|
||||
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
|
||||
|
||||
%iterate over angles
|
||||
for angleind = 1:numberofangles
|
||||
|
||||
figure(1);
|
||||
hold off;
|
||||
|
||||
text(0, 0, int2str(angleind));
|
||||
|
||||
%iterate over primitives
|
||||
for primind = 1:numberofprimsperangle
|
||||
fprintf(fout, 'primID: %d\n', primind-1);
|
||||
fprintf(fout, 'startangle_c: %d\n', angleind-1);
|
||||
|
||||
%current angle
|
||||
currentangle = (angleind-1)*2*pi/numberofangles;
|
||||
currentangle_36000int = round((angleind-1)*36000/numberofangles);
|
||||
|
||||
%compute which template to use
|
||||
if (rem(currentangle_36000int, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts0_c(primind,:);
|
||||
angle = currentangle;
|
||||
fprintf(1, '90\n');
|
||||
elseif (rem(currentangle_36000int, 4500) == 0)
|
||||
basemprimendpts_c = basemprimendpts45_c(primind,:);
|
||||
angle = currentangle - 45*pi/180;
|
||||
fprintf(1, '45\n');
|
||||
elseif (rem(currentangle_36000int-7875, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 78.75*pi/180;
|
||||
fprintf(1, '78p75\n');
|
||||
elseif (rem(currentangle_36000int-6750, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
|
||||
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
|
||||
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
|
||||
angle = currentangle - 67.5*pi/180;
|
||||
fprintf(1, '67p5\n');
|
||||
elseif (rem(currentangle_36000int-5625, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 56.25*pi/180;
|
||||
fprintf(1, '56p25\n');
|
||||
elseif (rem(currentangle_36000int-3375, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
angle = currentangle - 33.75*pi/180;
|
||||
fprintf(1, '33p75\n');
|
||||
elseif (rem(currentangle_36000int-2250, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
angle = currentangle - 22.5*pi/180;
|
||||
fprintf(1, '22p5\n');
|
||||
elseif (rem(currentangle_36000int-1125, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
angle = currentangle - 11.25*pi/180;
|
||||
fprintf(1, '11p25\n');
|
||||
else
|
||||
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
|
||||
return;
|
||||
end;
|
||||
|
||||
%now figure out what action will be
|
||||
baseendpose_c = basemprimendpts_c(1:3);
|
||||
additionalactioncostmult = basemprimendpts_c(4);
|
||||
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
|
||||
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
|
||||
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
|
||||
endpose_c = [endx_c endy_c endtheta_c];
|
||||
|
||||
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
|
||||
|
||||
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
|
||||
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
end;
|
||||
|
||||
%generate intermediate poses (remember they are w.r.t 0,0 (and not
|
||||
%centers of the cells)
|
||||
numofsamples = 10;
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
startpt = [0 0 currentangle];
|
||||
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
|
||||
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
|
||||
for iind = 1:numofsamples
|
||||
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
|
||||
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
|
||||
0];
|
||||
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
|
||||
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
|
||||
end;
|
||||
else %unicycle-based move forward or backward
|
||||
R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
|
||||
sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
|
||||
S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
|
||||
l = S(1);
|
||||
tvoverrv = S(2);
|
||||
rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
|
||||
tv = tvoverrv*rv;
|
||||
|
||||
if l < 0
|
||||
fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
|
||||
l = 0;
|
||||
end;
|
||||
%compute rv
|
||||
%rv = baseendpose_c(3)*2*pi/numberofangles;
|
||||
%compute tv
|
||||
%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
|
||||
%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
|
||||
%tv = (tvx + tvy)/2.0;
|
||||
%generate samples
|
||||
for iind = 1:numofsamples
|
||||
dt = (iind-1)/(numofsamples-1);
|
||||
|
||||
%dtheta = rv*dt + startpt(3);
|
||||
%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
% dtheta];
|
||||
|
||||
if(dt*tv < l)
|
||||
intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
|
||||
startpt(2) + dt*tv*sin(startpt(3)) ...
|
||||
startpt(3)];
|
||||
else
|
||||
dtheta = rv*(dt - l/tv) + startpt(3);
|
||||
intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
dtheta];
|
||||
end;
|
||||
end;
|
||||
%correct
|
||||
errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
|
||||
endpt(2) - intermcells_m(numofsamples,2)];
|
||||
fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
|
||||
interpfactor = [0:1/(numofsamples-1):1];
|
||||
intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
|
||||
intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
|
||||
end;
|
||||
end;
|
||||
|
||||
%write out
|
||||
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
|
||||
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
|
||||
for interind = 1:size(intermcells_m, 1)
|
||||
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
|
||||
end;
|
||||
|
||||
plot(intermcells_m(:,1), intermcells_m(:,2));
|
||||
axis([-0.3 0.3 -0.3 0.3]);
|
||||
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
|
||||
hold on;
|
||||
|
||||
end;
|
||||
grid;
|
||||
pause;
|
||||
end;
|
||||
|
||||
fclose('all');
|
||||
296
navigations/sbpl/matlab/mprim/genmprim_unicycleplussidewaysplusbackturn.m
Executable file
296
navigations/sbpl/matlab/mprim/genmprim_unicycleplussidewaysplusbackturn.m
Executable file
@@ -0,0 +1,296 @@
|
||||
% /*
|
||||
% * Copyright (c) 2008, Maxim Likhachev
|
||||
% * All rights reserved.
|
||||
% *
|
||||
% * Redistribution and use in source and binary forms, with or without
|
||||
% * modification, are permitted provided that the following conditions are met:
|
||||
% *
|
||||
% * * Redistributions of source code must retain the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer.
|
||||
% * * Redistributions in binary form must reproduce the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer in the
|
||||
% * documentation and/or other materials provided with the distribution.
|
||||
% * * Neither the name of the Carnegie Mellon University nor the names of its
|
||||
% * contributors may be used to endorse or promote products derived from
|
||||
% * this software without specific prior written permission.
|
||||
% *
|
||||
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% * POSSIBILITY OF SUCH DAMAGE.
|
||||
% */
|
||||
function[] = genmprim_unicycleplussidewaysplusbackturn(outfilename)
|
||||
|
||||
%
|
||||
%generates motion primitives and saves them into file
|
||||
%
|
||||
%written by Maxim Likhachev
|
||||
%---------------------------------------------------
|
||||
%
|
||||
|
||||
%defines
|
||||
|
||||
UNICYCLE_MPRIM_16DEGS = 1;
|
||||
|
||||
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
resolution = 0.025;
|
||||
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
|
||||
numberofprimsperangle = 11;
|
||||
|
||||
%multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1;
|
||||
backwardcostmult = 5;
|
||||
forwardandturncostmult = 3;
|
||||
sidestepcostmult = 2;
|
||||
turninplacecostmult = 1;
|
||||
|
||||
%note, what is shown x,y,theta *changes* (that is, dx,dy,dtheta and not absolute numbers)
|
||||
|
||||
%0 degreees
|
||||
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
|
||||
basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts0_c(8,:) = [0 1 0 sidestepcostmult];
|
||||
basemprimendpts0_c(9,:) = [0 -1 0 sidestepcostmult];
|
||||
%1/16 theta change going backward
|
||||
basemprimendpts0_c(10,:) = [-8 -1 1 backwardcostmult];
|
||||
basemprimendpts0_c(11,:) = [-8 1 -1 backwardcostmult];
|
||||
|
||||
%45 degrees
|
||||
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
|
||||
basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
|
||||
basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
|
||||
basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts45_c(8,:) = [-1 1 0 sidestepcostmult];
|
||||
basemprimendpts45_c(9,:) = [1 -1 0 sidestepcostmult];
|
||||
%1/16 theta change going back
|
||||
basemprimendpts45_c(10,:) = [-5 -7 1 backwardcostmult];
|
||||
basemprimendpts45_c(11,:) = [-7 -5 -1 backwardcostmult];
|
||||
|
||||
%22.5 degrees
|
||||
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
|
||||
basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts22p5_c(8,:) = [-1 2 0 sidestepcostmult];
|
||||
basemprimendpts22p5_c(9,:) = [1 -2 0 sidestepcostmult];
|
||||
%1/16 theta change going back
|
||||
basemprimendpts22p5_c(10,:) = [-5 -4 1 backwardcostmult];
|
||||
basemprimendpts22p5_c(11,:) = [-7 -2 -1 backwardcostmult];
|
||||
|
||||
|
||||
else
|
||||
fprintf(1, 'ERROR: undefined mprims type\n');
|
||||
return;
|
||||
end;
|
||||
|
||||
|
||||
fout = fopen(outfilename, 'w');
|
||||
|
||||
|
||||
%write the header
|
||||
fprintf(fout, 'resolution_m: %f\n', resolution);
|
||||
fprintf(fout, 'numberofangles: %d\n', numberofangles);
|
||||
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
|
||||
|
||||
%iterate over angles
|
||||
for angleind = 1:numberofangles
|
||||
|
||||
figure(1);
|
||||
hold off;
|
||||
|
||||
text(0, 0, int2str(angleind));
|
||||
|
||||
%iterate over primitives
|
||||
for primind = 1:numberofprimsperangle
|
||||
fprintf(fout, 'primID: %d\n', primind-1);
|
||||
fprintf(fout, 'startangle_c: %d\n', angleind-1);
|
||||
|
||||
%current angle
|
||||
currentangle = (angleind-1)*2*pi/numberofangles;
|
||||
currentangle_36000int = round((angleind-1)*36000/numberofangles);
|
||||
|
||||
%compute which template to use
|
||||
if (rem(currentangle_36000int, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts0_c(primind,:);
|
||||
angle = currentangle;
|
||||
fprintf(1, '90\n');
|
||||
elseif (rem(currentangle_36000int, 4500) == 0)
|
||||
basemprimendpts_c = basemprimendpts45_c(primind,:);
|
||||
angle = currentangle - 45*pi/180;
|
||||
fprintf(1, '45\n');
|
||||
elseif (rem(currentangle_36000int-7875, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 78.75*pi/180;
|
||||
fprintf(1, '78p75\n');
|
||||
elseif (rem(currentangle_36000int-6750, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
|
||||
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
|
||||
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
|
||||
angle = currentangle - 67.5*pi/180;
|
||||
fprintf(1, '67p5\n');
|
||||
elseif (rem(currentangle_36000int-5625, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 56.25*pi/180;
|
||||
fprintf(1, '56p25\n');
|
||||
elseif (rem(currentangle_36000int-3375, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
angle = currentangle - 33.75*pi/180;
|
||||
fprintf(1, '33p75\n');
|
||||
elseif (rem(currentangle_36000int-2250, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
angle = currentangle - 22.5*pi/180;
|
||||
fprintf(1, '22p5\n');
|
||||
elseif (rem(currentangle_36000int-1125, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
angle = currentangle - 11.25*pi/180;
|
||||
fprintf(1, '11p25\n');
|
||||
else
|
||||
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
|
||||
return;
|
||||
end;
|
||||
|
||||
%now figure out what action will be
|
||||
baseendpose_c = basemprimendpts_c(1:3);
|
||||
additionalactioncostmult = basemprimendpts_c(4);
|
||||
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
|
||||
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
|
||||
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
|
||||
endpose_c = [endx_c endy_c endtheta_c];
|
||||
|
||||
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
|
||||
|
||||
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
|
||||
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
end;
|
||||
|
||||
%generate intermediate poses (remember they are w.r.t 0,0 (and not
|
||||
%centers of the cells)
|
||||
numofsamples = 10;
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
startpt = [0 0 currentangle];
|
||||
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
|
||||
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
|
||||
for iind = 1:numofsamples
|
||||
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
|
||||
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
|
||||
0];
|
||||
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
|
||||
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
|
||||
end;
|
||||
else %unicycle-based move forward or backward
|
||||
R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
|
||||
sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
|
||||
S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
|
||||
l = S(1);
|
||||
tvoverrv = S(2);
|
||||
rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
|
||||
tv = tvoverrv*rv;
|
||||
|
||||
if ((l < 0 & tv > 0) | (l > 0 & tv < 0))
|
||||
fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
|
||||
l = 0;
|
||||
end;
|
||||
%compute rv
|
||||
%rv = baseendpose_c(3)*2*pi/numberofangles;
|
||||
%compute tv
|
||||
%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
|
||||
%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
|
||||
%tv = (tvx + tvy)/2.0;
|
||||
%generate samples
|
||||
for iind = 1:numofsamples
|
||||
dt = (iind-1)/(numofsamples-1);
|
||||
|
||||
%dtheta = rv*dt + startpt(3);
|
||||
%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
% dtheta];
|
||||
|
||||
if(abs(dt*tv) < abs(l))
|
||||
intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
|
||||
startpt(2) + dt*tv*sin(startpt(3)) ...
|
||||
startpt(3)];
|
||||
else
|
||||
dtheta = rv*(dt - l/tv) + startpt(3);
|
||||
intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
dtheta];
|
||||
end;
|
||||
end;
|
||||
%correct
|
||||
errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
|
||||
endpt(2) - intermcells_m(numofsamples,2)];
|
||||
fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
|
||||
interpfactor = [0:1/(numofsamples-1):1];
|
||||
intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
|
||||
intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
|
||||
end;
|
||||
end;
|
||||
|
||||
%write out
|
||||
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
|
||||
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
|
||||
for interind = 1:size(intermcells_m, 1)
|
||||
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
|
||||
end;
|
||||
|
||||
plot(intermcells_m(:,1), intermcells_m(:,2));
|
||||
axis([-0.3 0.3 -0.3 0.3]);
|
||||
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
|
||||
hold on;
|
||||
|
||||
end;
|
||||
grid;
|
||||
pause;
|
||||
end;
|
||||
|
||||
fclose('all');
|
||||
@@ -0,0 +1,306 @@
|
||||
% /*
|
||||
% * Copyright (c) 2008, Maxim Likhachev
|
||||
% * All rights reserved.
|
||||
% *
|
||||
% * Redistribution and use in source and binary forms, with or without
|
||||
% * modification, are permitted provided that the following conditions are met:
|
||||
% *
|
||||
% * * Redistributions of source code must retain the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer.
|
||||
% * * Redistributions in binary form must reproduce the above copyright
|
||||
% * notice, this list of conditions and the following disclaimer in the
|
||||
% * documentation and/or other materials provided with the distribution.
|
||||
% * * Neither the name of the Carnegie Mellon University nor the names of its
|
||||
% * contributors may be used to endorse or promote products derived from
|
||||
% * this software without specific prior written permission.
|
||||
% *
|
||||
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% * POSSIBILITY OF SUCH DAMAGE.
|
||||
% */
|
||||
function[] = genmprim_unicycleplussidewaysplusbackturnplusdiag(outfilename)
|
||||
|
||||
%
|
||||
%generates motion primitives and saves them into file
|
||||
%
|
||||
%written by Maxim Likhachev
|
||||
%---------------------------------------------------
|
||||
%
|
||||
|
||||
%defines
|
||||
|
||||
UNICYCLE_MPRIM_16DEGS = 1;
|
||||
|
||||
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
resolution = 0.025;
|
||||
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
|
||||
numberofprimsperangle = 13;
|
||||
|
||||
%multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1;
|
||||
backwardcostmult = 1;
|
||||
forwardandturncostmult = 20; % 3;
|
||||
sidestepcostmult = 2;
|
||||
turninplacecostmult = 20;
|
||||
forwarddiagcostmult = 1; %move forward slightly to the left/right without changing heading
|
||||
|
||||
%note, what is shown x,y,theta *changes* (that is, dx,dy,dtheta and not absolute numbers)
|
||||
|
||||
%0 degreees
|
||||
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
|
||||
basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
|
||||
basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts0_c(8,:) = [0 1 0 sidestepcostmult];
|
||||
basemprimendpts0_c(9,:) = [0 -1 0 sidestepcostmult];
|
||||
%1/16 theta change going backward
|
||||
basemprimendpts0_c(10,:) = [-8 -1 1 backwardcostmult];
|
||||
basemprimendpts0_c(11,:) = [-8 1 -1 backwardcostmult];
|
||||
%forward diagonal
|
||||
basemprimendpts0_c(12,:) = [8 1 0 forwarddiagcostmult];
|
||||
basemprimendpts0_c(13,:) = [8 -1 0 forwarddiagcostmult];
|
||||
|
||||
%45 degrees
|
||||
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
|
||||
basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
|
||||
basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
|
||||
basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts45_c(8,:) = [-1 1 0 sidestepcostmult];
|
||||
basemprimendpts45_c(9,:) = [1 -1 0 sidestepcostmult];
|
||||
%1/16 theta change going back
|
||||
basemprimendpts45_c(10,:) = [-5 -7 1 backwardcostmult];
|
||||
basemprimendpts45_c(11,:) = [-7 -5 -1 backwardcostmult];
|
||||
%forward diagonal
|
||||
basemprimendpts45_c(12,:) = [5 7 0 forwarddiagcostmult];
|
||||
basemprimendpts45_c(13,:) = [7 5 0 forwarddiagcostmult];
|
||||
|
||||
%22.5 degrees
|
||||
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
|
||||
%angles are positive counterclockwise
|
||||
%0 theta change
|
||||
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
|
||||
basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
|
||||
%1/16 theta change
|
||||
basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
|
||||
basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
|
||||
%turn in place
|
||||
basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
|
||||
basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
|
||||
%sideways maintaining the same heading
|
||||
basemprimendpts22p5_c(8,:) = [-1 2 0 sidestepcostmult];
|
||||
basemprimendpts22p5_c(9,:) = [1 -2 0 sidestepcostmult];
|
||||
%1/16 theta change going back
|
||||
basemprimendpts22p5_c(10,:) = [-5 -4 1 backwardcostmult];
|
||||
basemprimendpts22p5_c(11,:) = [-7 -2 -1 backwardcostmult];
|
||||
%forward diagonal
|
||||
basemprimendpts22p5_c(12,:) = [5 4 0 forwarddiagcostmult];
|
||||
basemprimendpts22p5_c(13,:) = [7 2 0 forwarddiagcostmult];
|
||||
|
||||
|
||||
else
|
||||
fprintf(1, 'ERROR: undefined mprims type\n');
|
||||
return;
|
||||
end;
|
||||
|
||||
|
||||
fout = fopen(outfilename, 'w');
|
||||
|
||||
|
||||
%write the header
|
||||
fprintf(fout, 'resolution_m: %f\n', resolution);
|
||||
fprintf(fout, 'numberofangles: %d\n', numberofangles);
|
||||
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
|
||||
|
||||
%iterate over angles
|
||||
for angleind = 1:numberofangles
|
||||
|
||||
figure(1);
|
||||
hold off;
|
||||
|
||||
text(0, 0, int2str(angleind));
|
||||
|
||||
%iterate over primitives
|
||||
for primind = 1:numberofprimsperangle
|
||||
fprintf(fout, 'primID: %d\n', primind-1);
|
||||
fprintf(fout, 'startangle_c: %d\n', angleind-1);
|
||||
|
||||
%current angle
|
||||
currentangle = (angleind-1)*2*pi/numberofangles;
|
||||
currentangle_36000int = round((angleind-1)*36000/numberofangles);
|
||||
|
||||
%compute which template to use
|
||||
if (rem(currentangle_36000int, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts0_c(primind,:);
|
||||
angle = currentangle;
|
||||
fprintf(1, '90\n');
|
||||
elseif (rem(currentangle_36000int, 4500) == 0)
|
||||
basemprimendpts_c = basemprimendpts45_c(primind,:);
|
||||
angle = currentangle - 45*pi/180;
|
||||
fprintf(1, '45\n');
|
||||
elseif (rem(currentangle_36000int-7875, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 78.75*pi/180;
|
||||
fprintf(1, '78p75\n');
|
||||
elseif (rem(currentangle_36000int-6750, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
|
||||
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
|
||||
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
|
||||
angle = currentangle - 67.5*pi/180;
|
||||
fprintf(1, '67p5\n');
|
||||
elseif (rem(currentangle_36000int-5625, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
|
||||
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
|
||||
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
|
||||
angle = currentangle - 56.25*pi/180;
|
||||
fprintf(1, '56p25\n');
|
||||
elseif (rem(currentangle_36000int-3375, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
|
||||
angle = currentangle - 33.75*pi/180;
|
||||
fprintf(1, '33p75\n');
|
||||
elseif (rem(currentangle_36000int-2250, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
|
||||
angle = currentangle - 22.5*pi/180;
|
||||
fprintf(1, '22p5\n');
|
||||
elseif (rem(currentangle_36000int-1125, 9000) == 0)
|
||||
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
|
||||
angle = currentangle - 11.25*pi/180;
|
||||
fprintf(1, '11p25\n');
|
||||
else
|
||||
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
|
||||
return;
|
||||
end;
|
||||
|
||||
%now figure out what action will be
|
||||
baseendpose_c = basemprimendpts_c(1:3);
|
||||
additionalactioncostmult = basemprimendpts_c(4);
|
||||
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
|
||||
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
|
||||
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
|
||||
endpose_c = [endx_c endy_c endtheta_c];
|
||||
|
||||
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
|
||||
|
||||
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
|
||||
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
end;
|
||||
|
||||
%generate intermediate poses (remember they are w.r.t 0,0 (and not
|
||||
%centers of the cells)
|
||||
numofsamples = 10;
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if UNICYCLE_MPRIM_16DEGS == 1
|
||||
startpt = [0 0 currentangle];
|
||||
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
|
||||
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
|
||||
intermcells_m = zeros(numofsamples,3);
|
||||
if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
|
||||
for iind = 1:numofsamples
|
||||
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
|
||||
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
|
||||
0];
|
||||
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
|
||||
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
|
||||
end;
|
||||
else %unicycle-based move forward or backward
|
||||
R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
|
||||
sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
|
||||
S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
|
||||
l = S(1);
|
||||
tvoverrv = S(2);
|
||||
rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
|
||||
tv = tvoverrv*rv;
|
||||
|
||||
if ((l < 0 & tv > 0) | (l > 0 & tv < 0))
|
||||
fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
|
||||
l = 0;
|
||||
end;
|
||||
%compute rv
|
||||
%rv = baseendpose_c(3)*2*pi/numberofangles;
|
||||
%compute tv
|
||||
%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
|
||||
%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
|
||||
%tv = (tvx + tvy)/2.0;
|
||||
%generate samples
|
||||
for iind = 1:numofsamples
|
||||
dt = (iind-1)/(numofsamples-1);
|
||||
|
||||
%dtheta = rv*dt + startpt(3);
|
||||
%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
% dtheta];
|
||||
|
||||
if(abs(dt*tv) < abs(l))
|
||||
intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
|
||||
startpt(2) + dt*tv*sin(startpt(3)) ...
|
||||
startpt(3)];
|
||||
else
|
||||
dtheta = rv*(dt - l/tv) + startpt(3);
|
||||
intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
|
||||
startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
|
||||
dtheta];
|
||||
end;
|
||||
end;
|
||||
%correct
|
||||
errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
|
||||
endpt(2) - intermcells_m(numofsamples,2)];
|
||||
fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
|
||||
interpfactor = [0:1/(numofsamples-1):1];
|
||||
intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
|
||||
intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
|
||||
end;
|
||||
end;
|
||||
|
||||
%write out
|
||||
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
|
||||
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
|
||||
for interind = 1:size(intermcells_m, 1)
|
||||
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
|
||||
end;
|
||||
|
||||
plot(intermcells_m(:,1), intermcells_m(:,2));
|
||||
axis([-0.3 0.3 -0.3 0.3]);
|
||||
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
|
||||
hold on;
|
||||
|
||||
end;
|
||||
grid;
|
||||
pause;
|
||||
end;
|
||||
|
||||
fclose('all');
|
||||
2643
navigations/sbpl/matlab/mprim/mprim_unic_sideback.mprim
Executable file
2643
navigations/sbpl/matlab/mprim/mprim_unic_sideback.mprim
Executable file
File diff suppressed because it is too large
Load Diff
3123
navigations/sbpl/matlab/mprim/mprim_unic_sidebackdiag.mprim
Executable file
3123
navigations/sbpl/matlab/mprim/mprim_unic_sidebackdiag.mprim
Executable file
File diff suppressed because it is too large
Load Diff
5723
navigations/sbpl/matlab/mprim/non_uniform_res0025_rad1_err005.mprim
Executable file
5723
navigations/sbpl/matlab/mprim/non_uniform_res0025_rad1_err005.mprim
Executable file
File diff suppressed because it is too large
Load Diff
5307
navigations/sbpl/matlab/mprim/non_uniform_res01_rad3_err005.mprim
Executable file
5307
navigations/sbpl/matlab/mprim/non_uniform_res01_rad3_err005.mprim
Executable file
File diff suppressed because it is too large
Load Diff
1683
navigations/sbpl/matlab/mprim/pr2.mprim
Executable file
1683
navigations/sbpl/matlab/mprim/pr2.mprim
Executable file
File diff suppressed because it is too large
Load Diff
3843
navigations/sbpl/matlab/mprim/pr2_10cm.mprim
Executable file
3843
navigations/sbpl/matlab/mprim/pr2_10cm.mprim
Executable file
File diff suppressed because it is too large
Load Diff
3123
navigations/sbpl/matlab/mprim/pr2_all_2.5cm_20turncost.mprim
Executable file
3123
navigations/sbpl/matlab/mprim/pr2_all_2.5cm_20turncost.mprim
Executable file
File diff suppressed because it is too large
Load Diff
1203
navigations/sbpl/matlab/mprim/pr2_unicycle_10cm.mprim
Executable file
1203
navigations/sbpl/matlab/mprim/pr2_unicycle_10cm.mprim
Executable file
File diff suppressed because it is too large
Load Diff
2163
navigations/sbpl/matlab/mprim/pr2sides.mprim
Executable file
2163
navigations/sbpl/matlab/mprim/pr2sides.mprim
Executable file
File diff suppressed because it is too large
Load Diff
1683
navigations/sbpl/matlab/mprim/unicycle_backonlystraight.mprim
Executable file
1683
navigations/sbpl/matlab/mprim/unicycle_backonlystraight.mprim
Executable file
File diff suppressed because it is too large
Load Diff
1203
navigations/sbpl/matlab/mprim/unicycle_noturninplace.mprim
Executable file
1203
navigations/sbpl/matlab/mprim/unicycle_noturninplace.mprim
Executable file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user