#!/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)