update
This commit is contained in:
@@ -1,280 +0,0 @@
|
||||
% /*
|
||||
% * 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_highcost_5cm(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.05;
|
||||
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
|
||||
numberofprimsperangle = 7;
|
||||
|
||||
%multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1;
|
||||
backwardcostmult = 40;
|
||||
forwardandturncostmult = 2;
|
||||
sidestepcostmult = 10;
|
||||
turninplacecostmult = 20;
|
||||
|
||||
%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');
|
||||
@@ -1,416 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# Copyright (c) 2016, David Conner (Christopher Newport University)
|
||||
# Based on genmprim_unicycle.m
|
||||
# Copyright (c) 2008, Maxim Likhachev
|
||||
# All rights reserved.
|
||||
# converted by libermate utility (https://github.com/awesomebytes/libermate)
|
||||
#
|
||||
# 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.
|
||||
|
||||
import numpy as np
|
||||
import rospkg
|
||||
|
||||
# if available import pylab (from matlibplot)
|
||||
matplotlib_found = False
|
||||
try:
|
||||
import matplotlib.pylab as plt
|
||||
|
||||
matplotlib_found = True
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
|
||||
def matrix_size(mat, elem=None):
|
||||
if not elem:
|
||||
return mat.shape
|
||||
else:
|
||||
return mat.shape[int(elem) - 1]
|
||||
|
||||
|
||||
def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
|
||||
visualize = matplotlib_found and visualize # Plot the primitives
|
||||
|
||||
# Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c,
|
||||
# baseendpose_c, additionalactioncostmult, fout, numofsamples,
|
||||
# basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename,
|
||||
# numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult,
|
||||
# rotation_angle, basemprimendpts_c, forwardandturncostmult,
|
||||
# forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult,
|
||||
# interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt,
|
||||
# currentangle, numberofprimsperangle, resolution, currentangle_36000int,
|
||||
# l, iind, errorxy, interind, endy_c, angleind, endpt
|
||||
# Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text,
|
||||
# int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen,
|
||||
# round, size
|
||||
# %
|
||||
# %generates motion primitives and saves them into file
|
||||
# %
|
||||
# %written by Maxim Likhachev
|
||||
# %---------------------------------------------------
|
||||
# %
|
||||
# %defines
|
||||
UNICYCLE_MPRIM_16DEGS = 1.0
|
||||
if UNICYCLE_MPRIM_16DEGS == 1.0:
|
||||
resolution = 0.05
|
||||
numberofangles = 16
|
||||
# %preferably a power of 2, definitely multiple of 8
|
||||
numberofprimsperangle = 7
|
||||
# %multipliers (multiplier is used as costmult*cost)
|
||||
forwardcostmult = 1.0
|
||||
backwardcostmult = 40.0
|
||||
forwardandturncostmult = 2.0
|
||||
# sidestepcostmult = 10.0
|
||||
turninplacecostmult = 20.0
|
||||
# %note, what is shown x,y,theta changes (not absolute numbers)
|
||||
# %0 degreees
|
||||
basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
|
||||
# %x,y,theta,costmult
|
||||
# %x aligned with the heading of the robot, angles are positive
|
||||
# %counterclockwise
|
||||
# %0 theta change
|
||||
basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
|
||||
basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
|
||||
basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
|
||||
# %1/16 theta change
|
||||
basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
|
||||
basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
|
||||
# %turn in place
|
||||
basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
|
||||
basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
|
||||
# %45 degrees
|
||||
basemprimendpts45_c = np.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[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
|
||||
basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
|
||||
basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
|
||||
# %1/16 theta change
|
||||
basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
|
||||
basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
|
||||
# %turn in place
|
||||
basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
|
||||
basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
|
||||
# %22.5 degrees
|
||||
basemprimendpts22p5_c = np.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[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
|
||||
basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
|
||||
basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
|
||||
# %1/16 theta change
|
||||
basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
|
||||
basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
|
||||
# %turn in place
|
||||
basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
|
||||
basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
|
||||
else:
|
||||
print('ERROR: undefined mprims type\n')
|
||||
return []
|
||||
|
||||
fout = open(outfilename, 'w')
|
||||
# %write the header
|
||||
fout.write('resolution_m: %f\n' % (resolution))
|
||||
fout.write('numberofangles: %d\n' % (numberofangles))
|
||||
fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles))
|
||||
# %iterate over angles
|
||||
for angleind in np.arange(1.0, (numberofangles) + 1):
|
||||
currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
|
||||
currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles)
|
||||
if visualize:
|
||||
if separate_plots:
|
||||
fig = plt.figure(angleind)
|
||||
plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0))
|
||||
else:
|
||||
fig = plt.figure(1)
|
||||
|
||||
plt.axis('equal')
|
||||
plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution])
|
||||
ax = fig.add_subplot(1, 1, 1)
|
||||
major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution)
|
||||
minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution)
|
||||
ax.set_xticks(major_ticks)
|
||||
ax.set_xticks(minor_ticks, minor=True)
|
||||
ax.set_yticks(major_ticks)
|
||||
ax.set_yticks(minor_ticks, minor=True)
|
||||
ax.grid(which='minor', alpha=0.5)
|
||||
ax.grid(which='major', alpha=0.9)
|
||||
|
||||
# %iterate over primitives
|
||||
for primind in np.arange(1.0, (numberofprimsperangle) + 1):
|
||||
fout.write('primID: %d\n' % (primind - 1))
|
||||
fout.write('startangle_c: %d\n' % (angleind - 1))
|
||||
# %current angle
|
||||
# %compute which template to use
|
||||
if (currentangle_36000int % 9000) == 0:
|
||||
basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :]
|
||||
angle = currentangle
|
||||
elif (currentangle_36000int % 4500) == 0:
|
||||
basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :]
|
||||
angle = currentangle - 45.0 * np.pi / 180.0
|
||||
|
||||
# commented out because basemprimendpts33p75_c is undefined
|
||||
# elif ((currentangle_36000int - 7875) % 9000) == 0:
|
||||
# basemprimendpts_c = (
|
||||
# 1 * basemprimendpts33p75_c[primind, :]
|
||||
# ) # 1* to force deep copy to avoid reference update below
|
||||
# basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
|
||||
# # %reverse x and y
|
||||
# basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
|
||||
# basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
|
||||
# # %reverse the angle as well
|
||||
# angle = currentangle - (78.75 * np.pi) / 180.0
|
||||
# print('78p75\n')
|
||||
|
||||
elif ((currentangle_36000int - 6750) % 9000) == 0:
|
||||
basemprimendpts_c = (
|
||||
1 * basemprimendpts22p5_c[int(primind) - 1, :]
|
||||
) # 1* to force deep copy to avoid reference update below
|
||||
basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
|
||||
# %reverse x and y
|
||||
basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
|
||||
basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
|
||||
# %reverse the angle as well
|
||||
# print(
|
||||
# '%d : %d %d %d onto %d %d %d\n'
|
||||
# % (
|
||||
# primind - 1,
|
||||
# basemprimendpts22p5_c[int(primind) - 1, 0],
|
||||
# basemprimendpts22p5_c[int(primind) - 1, 1],
|
||||
# basemprimendpts22p5_c[int(primind) - 1, 2],
|
||||
# basemprimendpts_c[0],
|
||||
# basemprimendpts_c[1],
|
||||
# basemprimendpts_c[2],
|
||||
# )
|
||||
# )
|
||||
angle = currentangle - (67.5 * np.pi) / 180.0
|
||||
print('67p5\n')
|
||||
|
||||
# commented out because basemprimendpts11p25_c is undefined
|
||||
# elif ((currentangle_36000int - 5625) % 9000) == 0:
|
||||
# basemprimendpts_c = (
|
||||
# 1 * basemprimendpts11p25_c[primind, :]
|
||||
# ) # 1* to force deep copy to avoid reference update below
|
||||
# basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
|
||||
# # %reverse x and y
|
||||
# basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
|
||||
# basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
|
||||
# # %reverse the angle as well
|
||||
# angle = currentangle - (56.25 * np.pi) / 180.0
|
||||
# print('56p25\n')
|
||||
|
||||
# commented out because basemprimendpts33p75_c is undefined
|
||||
# elif ((currentangle_36000int - 3375) % 9000) == 0:
|
||||
# basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
|
||||
# angle = currentangle - (33.75 * np.pi) / 180.0
|
||||
# print('33p75\n')
|
||||
|
||||
elif ((currentangle_36000int - 2250) % 9000) == 0:
|
||||
basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
|
||||
angle = currentangle - (22.5 * np.pi) / 180.0
|
||||
print('22p5\n')
|
||||
|
||||
# commented out because basemprimendpts11p25_c is undefined
|
||||
# elif ((currentangle_36000int - 1125) % 9000) == 0:
|
||||
# basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
|
||||
# angle = currentangle - (11.25 * np.pi) / 180.0
|
||||
# print('11p25\n')
|
||||
|
||||
else:
|
||||
print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int)
|
||||
return []
|
||||
|
||||
# %now figure out what action will be
|
||||
baseendpose_c = basemprimendpts_c[0:3]
|
||||
additionalactioncostmult = basemprimendpts_c[3]
|
||||
endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
|
||||
endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
|
||||
endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
|
||||
endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
|
||||
print("endpose_c=", endpose_c)
|
||||
print(('rotation angle=%f\n' % (angle * 180.0 / np.pi)))
|
||||
# if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
|
||||
# %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
|
||||
|
||||
# %generate intermediate poses (remember they are w.r.t 0,0 (and not
|
||||
# %centers of the cells)
|
||||
numofsamples = 10
|
||||
intermcells_m = np.zeros((numofsamples, 3))
|
||||
if UNICYCLE_MPRIM_16DEGS == 1.0:
|
||||
startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
|
||||
endpt = np.array(
|
||||
np.hstack(
|
||||
(
|
||||
(endpose_c[0] * resolution),
|
||||
(endpose_c[1] * resolution),
|
||||
(
|
||||
((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
|
||||
/ numberofangles
|
||||
),
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
print("startpt =", startpt)
|
||||
print("endpt =", endpt)
|
||||
intermcells_m = np.zeros((numofsamples, 3))
|
||||
if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
|
||||
# %turn in place or move forward
|
||||
for iind in np.arange(1.0, (numofsamples) + 1):
|
||||
fraction = float(iind - 1) / (numofsamples - 1)
|
||||
intermcells_m[int(iind) - 1, :] = np.array(
|
||||
(
|
||||
startpt[0] + (endpt[0] - startpt[0]) * fraction,
|
||||
startpt[1] + (endpt[1] - startpt[1]) * fraction,
|
||||
0,
|
||||
)
|
||||
)
|
||||
rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
|
||||
intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
|
||||
# print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
|
||||
|
||||
else:
|
||||
# %unicycle-based move forward or backward (http://sbpl.net/node/53)
|
||||
R = np.array(
|
||||
np.vstack(
|
||||
(
|
||||
np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
|
||||
np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
|
||||
l = S[0]
|
||||
tvoverrv = S[1]
|
||||
rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
|
||||
tv = tvoverrv * rv
|
||||
|
||||
# print "R=\n",R
|
||||
# print "Rpi=\n",np.linalg.pinv(R)
|
||||
# print "S=\n",S
|
||||
# print "l=",l
|
||||
# print "tvoverrv=",tvoverrv
|
||||
# print "rv=",rv
|
||||
# print "tv=",tv
|
||||
|
||||
if l < 0.0:
|
||||
print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l)))
|
||||
l = 0.0
|
||||
|
||||
# %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 in np.arange(1, numofsamples + 1):
|
||||
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[int(iind) - 1, :] = np.array(
|
||||
np.hstack(
|
||||
(
|
||||
startpt[0] + dt * tv * np.cos(startpt[2]),
|
||||
startpt[1] + dt * tv * np.sin(startpt[2]),
|
||||
startpt[2],
|
||||
)
|
||||
)
|
||||
)
|
||||
else:
|
||||
dtheta = rv * (dt - l / tv) + startpt[2]
|
||||
intermcells_m[int(iind) - 1, :] = np.array(
|
||||
np.hstack(
|
||||
(
|
||||
startpt[0]
|
||||
+ l * np.cos(startpt[2])
|
||||
+ tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
|
||||
startpt[1]
|
||||
+ l * np.sin(startpt[2])
|
||||
- tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
|
||||
dtheta,
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
# %correct
|
||||
errorxy = np.array(
|
||||
np.hstack(
|
||||
(
|
||||
endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
|
||||
endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
|
||||
)
|
||||
)
|
||||
)
|
||||
# print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
|
||||
interpfactor = np.array(
|
||||
np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
|
||||
)
|
||||
|
||||
# print "intermcells_m=",intermcells_m
|
||||
# print "interp'=",interpfactor.conj().T
|
||||
|
||||
intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
|
||||
intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
|
||||
|
||||
# %write out
|
||||
fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
|
||||
fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
|
||||
fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
|
||||
for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
|
||||
fout.write(
|
||||
'%.4f %.4f %.4f\n'
|
||||
% (
|
||||
intermcells_m[int(interind) - 1, 0],
|
||||
intermcells_m[int(interind) - 1, 1],
|
||||
intermcells_m[int(interind) - 1, 2],
|
||||
)
|
||||
)
|
||||
|
||||
if visualize:
|
||||
plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
|
||||
plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
|
||||
# if (visualize):
|
||||
# plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time
|
||||
|
||||
fout.close()
|
||||
if visualize:
|
||||
# plt.waitforbuttonpress() # hold until buttom pressed
|
||||
plt.show() # Keep windows open until the program is terminated
|
||||
return []
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
rospack = rospkg.RosPack()
|
||||
outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
|
||||
genmprim_unicycle(outfilename, visualize=True)
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user