417 lines
21 KiB
Python
Executable File
417 lines
21 KiB
Python
Executable File
#!/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)
|