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