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 
41  matplotlib_found = True
42 except ImportError:
43  pass
44 
45 
46 def matrix_size(mat, elem=None):
47  if not elem:
48  return mat.shape
49  else:
50  return mat.shape[int(elem) - 1]
51 
52 
53 def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
54  visualize = matplotlib_found and visualize # Plot the primitives
55 
56  # 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
57  # 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
58  #%
59  #%generates motion primitives and saves them into file
60  #%
61  #%written by Maxim Likhachev
62  #%---------------------------------------------------
63  #%
64  #%defines
65  UNICYCLE_MPRIM_16DEGS = 1.0
66  if UNICYCLE_MPRIM_16DEGS == 1.0:
67  resolution = 0.05
68  numberofangles = 16
69  #%preferably a power of 2, definitely multiple of 8
70  numberofprimsperangle = 7
71  #%multipliers (multiplier is used as costmult*cost)
72  forwardcostmult = 1.0
73  backwardcostmult = 40.0
74  forwardandturncostmult = 2.0
75  sidestepcostmult = 10.0
76  turninplacecostmult = 20.0
77  #%note, what is shown x,y,theta changes (not absolute numbers)
78  #%0 degreees
79  basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
80  #%x,y,theta,costmult
81  #%x aligned with the heading of the robot, angles are positive
82  #%counterclockwise
83  #%0 theta change
84  basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
85  basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
86  basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
87  #%1/16 theta change
88  basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
89  basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
90  #%turn in place
91  basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
92  basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
93  #%45 degrees
94  basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
95  #%x,y,theta,costmult (multiplier is used as costmult*cost)
96  #%x aligned with the heading of the robot, angles are positive
97  #%counterclockwise
98  #%0 theta change
99  basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
100  basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
101  basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
102  #%1/16 theta change
103  basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
104  basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
105  #%turn in place
106  basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
107  basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
108  #%22.5 degrees
109  basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
110  #%x,y,theta,costmult (multiplier is used as costmult*cost)
111  #%x aligned with the heading of the robot, angles are positive
112  #%counterclockwise
113  #%0 theta change
114  basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
115  basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
116  basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
117  #%1/16 theta change
118  basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
119  basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
120  #%turn in place
121  basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
122  basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
123  else:
124  print ('ERROR: undefined mprims type\n')
125  return []
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.0, (numberofangles) + 1):
134  currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
135  currentangle_36000int = np.round((angleind - 1) * 36000.0 / 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.0))
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.0, (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.0 * np.pi / 180.0
167 
168  elif ((currentangle_36000int - 7875) % 9000) == 0:
169  basemprimendpts_c = (
170  1 * basemprimendpts33p75_c[primind, :]
171  ) # 1* to force deep copy to avoid reference update below
172  basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
173  #%reverse x and y
174  basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
175  basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
176  #%reverse the angle as well
177  angle = currentangle - (78.75 * np.pi) / 180.0
178  print ('78p75\n')
179 
180  elif ((currentangle_36000int - 6750) % 9000) == 0:
181  basemprimendpts_c = (
182  1 * basemprimendpts22p5_c[int(primind) - 1, :]
183  ) # 1* to force deep copy to avoid reference update below
184  basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
185  #%reverse x and y
186  basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
187  basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
188  #%reverse the angle as well
189  # 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]))
190  angle = currentangle - (67.5 * np.pi) / 180.0
191  print ('67p5\n')
192 
193  elif ((currentangle_36000int - 5625) % 9000) == 0:
194  basemprimendpts_c = (
195  1 * basemprimendpts11p25_c[primind, :]
196  ) # 1* to force deep copy to avoid reference update below
197  basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
198  #%reverse x and y
199  basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
200  basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
201  #%reverse the angle as well
202  angle = currentangle - (56.25 * np.pi) / 180.0
203  print ('56p25\n')
204 
205  elif ((currentangle_36000int - 3375) % 9000) == 0:
206  basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
207  angle = currentangle - (33.75 * np.pi) / 180.0
208  print ('33p75\n')
209 
210  elif ((currentangle_36000int - 2250) % 9000) == 0:
211  basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
212  angle = currentangle - (22.5 * np.pi) / 180.0
213  print ('22p5\n')
214 
215  elif ((currentangle_36000int - 1125) % 9000) == 0:
216  basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
217  angle = currentangle - (11.25 * np.pi) / 180.0
218  print ('11p25\n')
219 
220  else:
221  print ('ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int)
222  return []
223 
224  #%now figure out what action will be
225  baseendpose_c = basemprimendpts_c[0:3]
226  additionalactioncostmult = basemprimendpts_c[3]
227  endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
228  endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
229  endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
230  endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
231  print "endpose_c=", endpose_c
232  print ('rotation angle=%f\n' % (angle * 180.0 / np.pi))
233  # if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
234  #%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
235 
236  #%generate intermediate poses (remember they are w.r.t 0,0 (and not
237  #%centers of the cells)
238  numofsamples = 10
239  intermcells_m = np.zeros((numofsamples, 3))
240  if UNICYCLE_MPRIM_16DEGS == 1.0:
241  startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
242  endpt = np.array(
243  np.hstack(
244  (
245  (endpose_c[0] * resolution),
246  (endpose_c[1] * resolution),
247  (
248  ((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
249  / numberofangles
250  ),
251  )
252  )
253  )
254 
255  print "startpt =", startpt
256  print "endpt =", endpt
257  intermcells_m = np.zeros((numofsamples, 3))
258  if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
259  #%turn in place or move forward
260  for iind in np.arange(1.0, (numofsamples) + 1):
261  fraction = float(iind - 1) / (numofsamples - 1)
262  intermcells_m[int(iind) - 1, :] = np.array(
263  (
264  startpt[0] + (endpt[0] - startpt[0]) * fraction,
265  startpt[1] + (endpt[1] - startpt[1]) * fraction,
266  0,
267  )
268  )
269  rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
270  intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
271  # print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
272 
273  else:
274  #%unicycle-based move forward or backward (http://sbpl.net/node/53)
275  R = np.array(
276  np.vstack(
277  (
278  np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
279  np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
280  )
281  )
282  )
283 
284  S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
285  l = S[0]
286  tvoverrv = S[1]
287  rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
288  tv = tvoverrv * rv
289 
290  # print "R=\n",R
291  # print "Rpi=\n",np.linalg.pinv(R)
292  # print "S=\n",S
293  # print "l=",l
294  # print "tvoverrv=",tvoverrv
295  # print "rv=",rv
296  # print "tv=",tv
297 
298  if l < 0.0:
299  print ('WARNING: l = %f < 0 -> bad action start/end points\n' % (l))
300  l = 0.0
301 
302  #%compute rv
303  #%rv = baseendpose_c(3)*2*pi/numberofangles;
304  #%compute tv
305  #%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
306  #%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
307  #%tv = (tvx + tvy)/2.0;
308  #%generate samples
309  for iind in np.arange(1, numofsamples + 1):
310  dt = (iind - 1) / (numofsamples - 1)
311  #%dtheta = rv*dt + startpt(3);
312  #%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
313  #% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
314  #% dtheta];
315  if (dt * tv) < l:
316  intermcells_m[int(iind) - 1, :] = np.array(
317  np.hstack(
318  (
319  startpt[0] + dt * tv * np.cos(startpt[2]),
320  startpt[1] + dt * tv * np.sin(startpt[2]),
321  startpt[2],
322  )
323  )
324  )
325  else:
326  dtheta = rv * (dt - l / tv) + startpt[2]
327  intermcells_m[int(iind) - 1, :] = np.array(
328  np.hstack(
329  (
330  startpt[0]
331  + l * np.cos(startpt[2])
332  + tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
333  startpt[1]
334  + l * np.sin(startpt[2])
335  - tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
336  dtheta,
337  )
338  )
339  )
340 
341  #%correct
342  errorxy = np.array(
343  np.hstack(
344  (
345  endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
346  endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
347  )
348  )
349  )
350  # print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
351  interpfactor = np.array(
352  np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
353  )
354 
355  # print "intermcells_m=",intermcells_m
356  # print "interp'=",interpfactor.conj().T
357 
358  intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
359  intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
360 
361  #%write out
362  fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
363  fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
364  fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
365  for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
366  fout.write(
367  '%.4f %.4f %.4f\n'
368  % (
369  intermcells_m[int(interind) - 1, 0],
370  intermcells_m[int(interind) - 1, 1],
371  intermcells_m[int(interind) - 1, 2],
372  )
373  )
374 
375  if visualize:
376  plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
377  plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
378  plt.hold(True)
379  # if (visualize):
380  # plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time
381 
382  fout.close()
383  if visualize:
384  # plt.waitforbuttonpress() # hold until buttom pressed
385  plt.show() # Keep windows open until the program is terminated
386  return []
387 
388 
389 if __name__ == "__main__":
390  rospack = rospkg.RosPack()
391  outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
392  genmprim_unicycle(outfilename, visualize=True)
genmprim_unicycle_highcost_5cm.matrix_size
def matrix_size(mat, elem=None)
Definition: genmprim_unicycle_highcost_5cm.py:46
genmprim_unicycle_highcost_5cm.genmprim_unicycle
def genmprim_unicycle(outfilename, visualize=False, separate_plots=False)
Definition: genmprim_unicycle_highcost_5cm.py:53

mir_navigation
Author(s): Martin Günther
autogenerated on Wed Mar 22 2023 02:18:57