317 lines
15 KiB
Matlab
Executable File
317 lines
15 KiB
Matlab
Executable File
% /*
|
|
% * 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');
|