280 lines
13 KiB
Matlab
Executable File
280 lines
13 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_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');
|