genmprim_unicycle_highcost_5cm.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # Copyright (c) 2016, David Conner (Christopher Newport University)
4 # Based on genmprim_unicycle.m
5 # Copyright (c) 2008, Maxim Likhachev
6 # All rights reserved.
7 # converted by libermate utility (https://github.com/awesomebytes/libermate)
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above copyright
15 # notice, this list of conditions and the following disclaimer in the
16 # documentation and/or other materials provided with the distribution.
17 # * Neither the name of the Carnegie Mellon University nor the names of its
18 # contributors may be used to endorse or promote products derived from
19 # this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 import numpy as np
34 import rospkg
35 
36 # if available import pylab (from matlibplot)
37 matplotlib_found = False
38 try:
39  import matplotlib.pylab as plt
40  matplotlib_found = True
41 except ImportError:
42  pass
43 
44 
45 def matrix_size(mat, elem=None):
46  if not elem:
47  return mat.shape
48  else:
49  return mat.shape[int(elem)-1]
50 
51 
52 def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
53  visualize = matplotlib_found and visualize # Plot the primitives
54 
55  # 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
56  # Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text, int2str, basemprimendpts33p75_c, pause, axis, sin, pinv, basemprimendpts11p25_c, fprintf, fclose, rem, zeros, fopen, round, size
57  #%
58  #%generates motion primitives and saves them into file
59  #%
60  #%written by Maxim Likhachev
61  #%---------------------------------------------------
62  #%
63  #%defines
64  UNICYCLE_MPRIM_16DEGS = 1.
65  if UNICYCLE_MPRIM_16DEGS == 1.:
66  resolution = 0.05
67  numberofangles = 16
68  #%preferably a power of 2, definitely multiple of 8
69  numberofprimsperangle = 7
70  #%multipliers (multiplier is used as costmult*cost)
71  forwardcostmult = 1.
72  backwardcostmult = 40.
73  forwardandturncostmult = 2.
74  sidestepcostmult = 10.
75  turninplacecostmult = 20.
76  #%note, what is shown x,y,theta changes (not absolute numbers)
77  #%0 degreees
78  basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
79  #%x,y,theta,costmult
80  #%x aligned with the heading of the robot, angles are positive
81  #%counterclockwise
82  #%0 theta change
83  basemprimendpts0_c[0,:] = np.array(np.hstack(( 1., 0., 0., forwardcostmult)))
84  basemprimendpts0_c[1,:] = np.array(np.hstack(( 8., 0., 0., forwardcostmult)))
85  basemprimendpts0_c[2,:] = np.array(np.hstack((-1., 0., 0., backwardcostmult)))
86  #%1/16 theta change
87  basemprimendpts0_c[3,:] = np.array(np.hstack(( 8., 1., 1., forwardandturncostmult)))
88  basemprimendpts0_c[4,:] = np.array(np.hstack(( 8., -1., -1., forwardandturncostmult)))
89  #%turn in place
90  basemprimendpts0_c[5,:] = np.array(np.hstack(( 0., 0., 1., turninplacecostmult)))
91  basemprimendpts0_c[6,:] = np.array(np.hstack(( 0., 0., -1., turninplacecostmult)))
92  #%45 degrees
93  basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
94  #%x,y,theta,costmult (multiplier is used as costmult*cost)
95  #%x aligned with the heading of the robot, angles are positive
96  #%counterclockwise
97  #%0 theta change
98  basemprimendpts45_c[0,:] = np.array(np.hstack(( 1., 1., 0., forwardcostmult)))
99  basemprimendpts45_c[1,:] = np.array(np.hstack(( 6., 6., 0., forwardcostmult)))
100  basemprimendpts45_c[2,:] = np.array(np.hstack((-1., -1., 0., backwardcostmult)))
101  #%1/16 theta change
102  basemprimendpts45_c[3,:] = np.array(np.hstack(( 5., 7., 1., forwardandturncostmult)))
103  basemprimendpts45_c[4,:] = np.array(np.hstack(( 7., 5., -1., forwardandturncostmult)))
104  #%turn in place
105  basemprimendpts45_c[5,:] = np.array(np.hstack(( 0., 0., 1., turninplacecostmult)))
106  basemprimendpts45_c[6,:] = np.array(np.hstack(( 0., 0., -1., turninplacecostmult)))
107  #%22.5 degrees
108  basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
109  #%x,y,theta,costmult (multiplier is used as costmult*cost)
110  #%x aligned with the heading of the robot, angles are positive
111  #%counterclockwise
112  #%0 theta change
113  basemprimendpts22p5_c[0,:] = np.array(np.hstack(( 2., 1., 0., forwardcostmult)))
114  basemprimendpts22p5_c[1,:] = np.array(np.hstack(( 6., 3., 0., forwardcostmult)))
115  basemprimendpts22p5_c[2,:] = np.array(np.hstack((-2., -1., 0., backwardcostmult)))
116  #%1/16 theta change
117  basemprimendpts22p5_c[3,:] = np.array(np.hstack(( 5., 4., 1., forwardandturncostmult)))
118  basemprimendpts22p5_c[4,:] = np.array(np.hstack(( 7., 2., -1., forwardandturncostmult)))
119  #%turn in place
120  basemprimendpts22p5_c[5,:] = np.array(np.hstack(( 0., 0., 1., turninplacecostmult)))
121  basemprimendpts22p5_c[6,:] = np.array(np.hstack(( 0., 0., -1., turninplacecostmult)))
122  else:
123  print('ERROR: undefined mprims type\n')
124  return []
125 
126 
127  fout = open(outfilename, 'w')
128  #%write the header
129  fout.write('resolution_m: %f\n'%(resolution))
130  fout.write('numberofangles: %d\n'%(numberofangles))
131  fout.write('totalnumberofprimitives: %d\n'%(numberofprimsperangle*numberofangles))
132  #%iterate over angles
133  for angleind in np.arange(1., (numberofangles)+1):
134  currentangle = ((angleind-1)*2.*np.pi)/numberofangles
135  currentangle_36000int = np.round((angleind-1)*36000./numberofangles)
136  if (visualize):
137  if separate_plots:
138  fig = plt.figure(angleind)
139  plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int/100.))
140  else:
141  fig = plt.figure(1)
142 
143  plt.axis('equal')
144  plt.axis([-10*resolution,10*resolution,-10*resolution,10*resolution])
145  ax = fig.add_subplot(1, 1, 1)
146  major_ticks = np.arange(-8*resolution, 9*resolution, 4*resolution)
147  minor_ticks = np.arange(-8*resolution, 9*resolution, resolution)
148  ax.set_xticks(major_ticks)
149  ax.set_xticks(minor_ticks, minor=True)
150  ax.set_yticks(major_ticks)
151  ax.set_yticks(minor_ticks, minor=True)
152  ax.grid(which='minor', alpha=0.5)
153  ax.grid(which='major', alpha=0.9)
154 
155  #%iterate over primitives
156  for primind in np.arange(1., (numberofprimsperangle)+1):
157  fout.write('primID: %d\n'%(primind-1))
158  fout.write('startangle_c: %d\n'%(angleind-1))
159  #%current angle
160  #%compute which template to use
161  if (currentangle_36000int%9000) == 0:
162  basemprimendpts_c = basemprimendpts0_c[int(primind)-1,:]
163  angle = currentangle
164  elif (currentangle_36000int%4500) == 0:
165  basemprimendpts_c = basemprimendpts45_c[int(primind)-1,:]
166  angle = currentangle-45.*np.pi/180.
167 
168  elif ((currentangle_36000int-7875)%9000) == 0:
169  basemprimendpts_c = 1*basemprimendpts33p75_c[primind, :] # 1* to force deep copy to avoid reference update below
170  basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
171  #%reverse x and y
172  basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
173  basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
174  #%reverse the angle as well
175  angle = currentangle-(78.75*np.pi)/180.
176  print('78p75\n')
177 
178  elif ((currentangle_36000int-6750)%9000) == 0:
179  basemprimendpts_c = 1*basemprimendpts22p5_c[int(primind)-1,:] # 1* to force deep copy to avoid reference update below
180  basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind)-1, 1]
181  #%reverse x and y
182  basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind)-1, 0]
183  basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind)-1, 2]
184  #%reverse the angle as well
185  #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]))
186  angle = currentangle-(67.5*np.pi)/180.
187  print('67p5\n')
188 
189  elif ((currentangle_36000int-5625)%9000) == 0:
190  basemprimendpts_c = 1*basemprimendpts11p25_c[primind, :] # 1* to force deep copy to avoid reference update below
191  basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
192  #%reverse x and y
193  basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
194  basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
195  #%reverse the angle as well
196  angle = currentangle-(56.25*np.pi)/180.
197  print('56p25\n')
198 
199  elif ((currentangle_36000int-3375)%9000) == 0:
200  basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
201  angle = currentangle-(33.75*np.pi)/180.
202  print('33p75\n')
203 
204  elif ((currentangle_36000int-2250)%9000) == 0:
205  basemprimendpts_c = basemprimendpts22p5_c[int(primind)-1,:]
206  angle = currentangle-(22.5*np.pi)/180.
207  print('22p5\n')
208 
209  elif ((currentangle_36000int-1125)%9000) == 0:
210  basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
211  angle = currentangle-(11.25*np.pi)/180.
212  print('11p25\n')
213 
214  else:
215  print('ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int)
216  return []
217 
218 
219  #%now figure out what action will be
220  baseendpose_c = basemprimendpts_c[0:3]
221  additionalactioncostmult = basemprimendpts_c[3]
222  endx_c = np.round((baseendpose_c[0]*np.cos(angle))-(baseendpose_c[1]*np.sin(angle)))
223  endy_c = np.round((baseendpose_c[0]*np.sin(angle))+(baseendpose_c[1]*np.cos(angle)))
224  endtheta_c = np.fmod(angleind-1+baseendpose_c[2], numberofangles)
225  endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
226  print "endpose_c=",endpose_c
227  print( 'rotation angle=%f\n'% (angle*180./np.pi))
228  #if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
229  #%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
230 
231 
232  #%generate intermediate poses (remember they are w.r.t 0,0 (and not
233  #%centers of the cells)
234  numofsamples = 10
235  intermcells_m = np.zeros((numofsamples, 3))
236  if UNICYCLE_MPRIM_16DEGS == 1.:
237  startpt = np.array(np.hstack((0., 0., currentangle)))
238  endpt = np.array(np.hstack(((endpose_c[0]*resolution),
239  (endpose_c[1]*resolution),
240  (( (np.fmod(angleind-1+baseendpose_c[2], numberofangles))*2.*np.pi)/numberofangles))))
241 
242  print "startpt =",startpt
243  print "endpt =",endpt
244  intermcells_m = np.zeros((numofsamples, 3))
245  if np.logical_or(np.logical_and(endx_c == 0., endy_c == 0.), baseendpose_c[2] == 0.):
246  #%turn in place or move forward
247  for iind in np.arange(1., (numofsamples)+1):
248  fraction = float(iind-1)/(numofsamples - 1)
249  intermcells_m[int(iind)-1,:] = np.array((startpt[0]+(endpt[0]-startpt[0])*fraction, startpt[1]+(endpt[1]-startpt[1])*fraction, 0))
250  rotation_angle = baseendpose_c[2]*(2.*np.pi/numberofangles)
251  intermcells_m[int(iind)-1,2] = np.fmod(startpt[2]+rotation_angle*fraction, (2.*np.pi))
252  #print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
253 
254  else:
255  #%unicycle-based move forward or backward (http://sbpl.net/node/53)
256  R = np.array(np.vstack((np.hstack((np.cos(startpt[2]), np.sin(endpt[2])-np.sin(startpt[2]))),
257  np.hstack((np.sin(startpt[2]), -np.cos(endpt[2])+np.cos(startpt[2]))) )))
258 
259  S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0]-startpt[0], endpt[1]-startpt[1])) ))
260  l = S[0]
261  tvoverrv = S[1]
262  rv = (baseendpose_c[2]*2.*np.pi/numberofangles)+ l/tvoverrv
263  tv = tvoverrv*rv
264 
265  #print "R=\n",R
266  #print "Rpi=\n",np.linalg.pinv(R)
267  #print "S=\n",S
268  #print "l=",l
269  #print "tvoverrv=",tvoverrv
270  #print "rv=",rv
271  #print "tv=",tv
272 
273  if l<0.:
274  print('WARNING: l = %f < 0 -> bad action start/end points\n'%(l))
275  l = 0.
276 
277 
278  #%compute rv
279  #%rv = baseendpose_c(3)*2*pi/numberofangles;
280  #%compute tv
281  #%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
282  #%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
283  #%tv = (tvx + tvy)/2.0;
284  #%generate samples
285  for iind in np.arange(1, numofsamples+1):
286  dt = (iind-1)/(numofsamples-1)
287  #%dtheta = rv*dt + startpt(3);
288  #%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
289  #% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
290  #% dtheta];
291  if (dt*tv)<l:
292  intermcells_m[int(iind)-1,:] = np.array(np.hstack((startpt[0]+dt*tv*np.cos(startpt[2]),
293  startpt[1]+dt*tv*np.sin(startpt[2]),
294  startpt[2])))
295  else:
296  dtheta = rv*(dt-l/tv)+startpt[2]
297  intermcells_m[int(iind)-1,:] = np.array(np.hstack((startpt[0] + l*np.cos(startpt[2]) + tvoverrv*(np.sin(dtheta)-np.sin(startpt[2])),
298  startpt[1] + l*np.sin(startpt[2]) - tvoverrv*(np.cos(dtheta)-np.cos(startpt[2])),
299  dtheta)))
300 
301 
302  #%correct
303  errorxy = np.array(np.hstack((endpt[0] -intermcells_m[int(numofsamples)-1,0],
304  endpt[1] -intermcells_m[int(numofsamples)-1,1])))
305  #print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
306  interpfactor = np.array(np.hstack((np.arange(0., 1.+(1./(numofsamples)), 1./(numofsamples-1)))))
307 
308  #print "intermcells_m=",intermcells_m
309  #print "interp'=",interpfactor.conj().T
310 
311  intermcells_m[:,0] = intermcells_m[:,0]+errorxy[0]*interpfactor.conj().T
312  intermcells_m[:,1] = intermcells_m[:,1]+errorxy[1]*interpfactor.conj().T
313 
314  #%write out
315  fout.write('endpose_c: %d %d %d\n'% ( endpose_c[0], endpose_c[1], endpose_c[2]))
316  fout.write('additionalactioncostmult: %d\n'%( additionalactioncostmult))
317  fout.write('intermediateposes: %d\n'%(matrix_size(intermcells_m, 1.)))
318  for interind in np.arange(1., (matrix_size(intermcells_m, 1.))+1):
319  fout.write('%.4f %.4f %.4f\n'%( intermcells_m[int(interind)-1,0], intermcells_m[int(interind)-1,1], intermcells_m[int(interind)-1,2]))
320 
321  if (visualize):
322  plt.plot(intermcells_m[:,0], intermcells_m[:,1],linestyle="-",marker="o")
323  plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
324  plt.hold(True)
325  #if (visualize):
326  # plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time
327 
328  fout.close()
329  if (visualize):
330  # plt.waitforbuttonpress() # hold until buttom pressed
331  plt.show() # Keep windows open until the program is terminated
332  return []
333 
334 if __name__ == "__main__":
335  rospack = rospkg.RosPack()
336  outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
337  genmprim_unicycle(outfilename, visualize=True)
genmprim_unicycle_highcost_5cm.matrix_size
def matrix_size(mat, elem=None)
Definition: genmprim_unicycle_highcost_5cm.py:45
genmprim_unicycle_highcost_5cm.genmprim_unicycle
def genmprim_unicycle(outfilename, visualize=False, separate_plots=False)
Definition: genmprim_unicycle_highcost_5cm.py:52

mir_navigation
Author(s): Martin Günther
autogenerated on Sun Feb 14 2021 03:40:24