Skip to content

Commit ac8f458

Browse files
petercorkepetercorke
committed
Merge branch 'future' of github.com:petercorke/robotics-toolbox-python into future
2 parents 6b566d2 + 9efe462 commit ac8f458

File tree

5 files changed

+91
-74
lines changed

5 files changed

+91
-74
lines changed

‎roboticstoolbox/__init__.py

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
from roboticstoolbox.tools import *
2+
23
from roboticstoolbox.robot import *
4+
35
from roboticstoolbox.mobile import *
46
import roboticstoolbox.models
57
import roboticstoolbox.backends

‎roboticstoolbox/blocks/Icons/250x250/ctraj.png

6.48 KB
Loading

‎roboticstoolbox/blocks/Icons/ctraj.png

6.48 KB
Loading

‎roboticstoolbox/core/fknm.c

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -669,9 +669,9 @@ void _jacob0(PyObject *links, int m, int n, npy_float64 *q, npy_float64 *etool,
669669
J[0 * n + j] = U[0 * 4 + 2] * temp[1 * 4 + 3] - U[0 * 4 + 1] * temp[2 * 4 + 3];
670670
J[1 * n + j] = U[1 * 4 + 2] * temp[1 * 4 + 3] - U[1 * 4 + 1] * temp[2 * 4 + 3];
671671
J[2 * n + j] = U[2 * 4 + 2] * temp[1 * 4 + 3] - U[2 * 4 + 1] * temp[2 * 4 + 3];
672-
J[3 * n + j] = U[0 * 4 + 2];
673-
J[4 * n + j] = U[1 * 4 + 2];
674-
J[5 * n + j] = U[2 * 4 + 2];
672+
J[3 * n + j] = U[0 * 4 + 0];
673+
J[4 * n + j] = U[1 * 4 + 0];
674+
J[5 * n + j] = U[2 * 4 + 0];
675675
}
676676
else if (link->axis == 1)
677677
{

‎roboticstoolbox/mobile/Animations.py

Lines changed: 86 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@
5252
5353
"""
5454

55+
5556
class VehicleAnimationBase(ABC):
5657
"""
5758
Class to support animation of a vehicle on Matplotlib plot
@@ -66,7 +67,7 @@ class VehicleAnimationBase(ABC):
6667
6768
a.add()
6869
a.update(q)
69-
70+
7071
adds an instance of the animation shape to the plot and subsequent calls
7172
to ``update`` will animate it.
7273
@@ -86,7 +87,7 @@ class VehicleAnimationBase(ABC):
8687
veh.run(animation=a)
8788
8889
"""
89-
90+
9091
def __init__(self):
9192
self._object = None
9293
self._ax = None
@@ -142,10 +143,11 @@ def __del__(self):
142143
if self._object is not None:
143144
self._object.remove()
144145

146+
145147
# ========================================================================= #
146148

147-
class VehicleMarker(VehicleAnimationBase):
148149

150+
class VehicleMarker(VehicleAnimationBase):
149151
def __init__(self, **kwargs):
150152
"""
151153
Create graphical animation of vehicle as a Matplotlib marker
@@ -170,13 +172,12 @@ def __init__(self, **kwargs):
170172
super().__init__()
171173
if len(kwargs) == 0:
172174
kwargs = {
173-
'marker': 'o',
174-
'markerfacecolor': 'r',
175-
'markeredgecolor': 'w',
176-
'markersize': 12
175+
"marker": "o",
176+
"markerfacecolor": "r",
177+
"markeredgecolor": "w",
178+
"markersize": 12,
177179
}
178180
self._args = kwargs
179-
180181

181182
def _update(self, x):
182183
self._object.set_xdata(x[0])
@@ -187,11 +188,12 @@ def _add(self, x=None, **kwargs):
187188
x = (0, 0)
188189
self._object = plt.plot(x[0], x[1], **self._args)[0]
189190

191+
190192
# ========================================================================= #
191193

192-
class VehiclePolygon(VehicleAnimationBase):
193194

194-
def __init__(self, shape='car', scale=1, **kwargs):
195+
class VehiclePolygon(VehicleAnimationBase):
196+
def __init__(self, shape="car", scale=1, **kwargs):
195197
"""
196198
Create graphical animation of vehicle as a polygon
197199
@@ -207,7 +209,7 @@ def __init__(self, shape='car', scale=1, **kwargs):
207209
:return: animation object
208210
:rtype: VehicleAnimation
209211
210-
Creates an object that can be passed to a ``Vehicle`` subclass to
212+
Creates an object that can be passed to a ``Vehicle`` subclass to
211213
depict the moving robot as a polygon during simulation.
212214
213215
For example, a red filled car-shaped polygon is::
@@ -233,30 +235,34 @@ def __init__(self, shape='car', scale=1, **kwargs):
233235
h = 0.3
234236
t = 0.8 # start of head taper
235237
c = 0.5 # centre x coordinate
236-
w = 1 # width in x direction
238+
w = 1 # width in x direction
237239

238240
if isinstance(shape, str):
239-
if shape == 'car':
240-
self._coords = np.array([
241-
[-c, h],
242-
[t - c, h],
243-
[w - c, 0],
244-
[t - c, -h],
245-
[-c, -h],
246-
]).T
247-
elif shape == 'triangle':
248-
self._coords = np.array([
249-
[-c, h],
250-
[ w, 0],
251-
[-c, -h],
252-
]).T
241+
if shape == "car":
242+
self._coords = np.array(
243+
[
244+
[-c, h],
245+
[t - c, h],
246+
[w - c, 0],
247+
[t - c, -h],
248+
[-c, -h],
249+
]
250+
).T
251+
elif shape == "triangle":
252+
self._coords = np.array(
253+
[
254+
[-c, h],
255+
[w, 0],
256+
[-c, -h],
257+
]
258+
).T
253259
else:
254-
raise ValueError('unknown vehicle shape name')
255-
260+
raise ValueError("unknown vehicle shape name")
261+
256262
elif isinstance(shape, np.ndarray) and shape.shape[1] == 2:
257263
self._coords = shape
258264
else:
259-
raise TypeError('unknown shape argument')
265+
raise TypeError("unknown shape argument")
260266
self._coords *= scale
261267
self._args = kwargs
262268

@@ -268,16 +274,17 @@ def _add(self, **kwargs):
268274
self._ax.add_patch(self._object)
269275

270276
def _update(self, x):
271-
277+
272278
if self._object is not None:
273279
# if animation is initialized
274280
xy = SE2(x) * self._coords
275281
self._object.set_xy(xy.T)
276282

283+
277284
# ========================================================================= #
278285

279-
class VehicleIcon(VehicleAnimationBase):
280286

287+
class VehicleIcon(VehicleAnimationBase):
281288
def __init__(self, filename, origin=None, scale=1, rotation=0):
282289
"""
283290
Create graphical animation of vehicle as an icon
@@ -294,7 +301,7 @@ def __init__(self, filename, origin=None, scale=1, rotation=0):
294301
:return: animation object
295302
:rtype: VehicleAnimation
296303
297-
Creates an object that can be passed to a ``Vehicle`` subclass to
304+
Creates an object that can be passed to a ``Vehicle`` subclass to
298305
depict the moving robot as a polygon during simulation.
299306
300307
For example, the image of a red car is::
@@ -331,8 +338,8 @@ def __init__(self, filename, origin=None, scale=1, rotation=0):
331338
middle of the icon image, (0.2, 0.5) moves it toward the back of the
332339
vehicle, (0.8, 0.5) moves it toward the front of the vehicle.
333340
334-
.. note::
335-
341+
.. note::
342+
336343
- The standard icons are kept in ``roboticstoolbox/data``
337344
- By default the image is assumed to contain a car parallel to
338345
the x-axis and facing right. If the vehicle is facing upward
@@ -341,18 +348,20 @@ def __init__(self, filename, origin=None, scale=1, rotation=0):
341348
:seealso: :func:`~Vehicle`
342349
"""
343350
super().__init__()
344-
if '.' not in filename:
351+
if "." not in filename:
345352
try:
346353
# try the default folder first
347-
image = rtb_load_data(Path("data") / Path(filename + ".png"), plt.imread)
354+
image = rtb_load_data(
355+
Path("data") / Path(filename + ".png"), plt.imread
356+
)
348357
except FileNotFoundError:
349358
raise ValueError(f"{filename} is not a provided icon")
350359
else:
351360
try:
352361
image = plt.imread(filename)
353362
except FileNotFoundError:
354363
raise ValueError(f"icon file {filename} not found")
355-
364+
356365
self._rotation = rotation
357366
self._image = image
358367

@@ -364,49 +373,53 @@ def __init__(self, filename, origin=None, scale=1, rotation=0):
364373
if image.shape[0] >= image.shape[1]:
365374
# width >= height
366375
self._width = scale
367-
self._height =scale * image.shape[1] / image.shape[0]
376+
self._height = scale * image.shape[1] / image.shape[0]
368377
else:
369378
# width < height
370379
self._height = scale
371380
self._width = scale * image.shape[0] / image.shape[1]
372381

373-
374382
def _add(self, ax=None, **kwargs):
375-
376383
def imshow_affine(ax, z, *args, **kwargs):
377384
im = ax.imshow(z, *args, **kwargs)
378385
x1, x2, y1, y2 = im.get_extent()
379386
# im._image_skew_coordinate = (x2, y1)
380387
return im
381388

382389
self._ax = plt.gca()
383-
extent = [ -self._origin[0] * self._height,
384-
(1 - self._origin[0]) * self._height,
385-
-self._origin[1] * self._width,
386-
(1 - self._origin[1]) * self._width
387-
]
390+
extent = [
391+
-self._origin[0] * self._height,
392+
(1 - self._origin[0]) * self._height,
393+
-self._origin[1] * self._width,
394+
(1 - self._origin[1]) * self._width,
395+
]
388396
self._ax = plt.gca()
389397

390398
args = {}
391-
if 'color' in kwargs and self._image.ndim == 2:
392-
color = kwargs['color']
393-
del kwargs['color']
399+
if "color" in kwargs and self._image.ndim == 2:
400+
color = kwargs["color"]
401+
del kwargs["color"]
394402
rgb = colors.to_rgb(color)
395-
cmapdata = {'red': [(0.0, 0.0, 0.0), (1.0, rgb[0], 0.0)],
396-
'green': [(0.0, 0.0, 0.0), (1.0, rgb[1], 0.0)],
397-
'blue': [(0.0, 0.0, 0.0), (1.0, rgb[2], 0.0)]
398-
}
399-
cmap = colors.LinearSegmentedColormap('linear', segmentdata=cmapdata, N=256)
400-
args = {'cmap': cmap}
403+
cmapdata = {
404+
"red": [(0.0, 0.0, 0.0), (1.0, rgb[0], 0.0)],
405+
"green": [(0.0, 0.0, 0.0), (1.0, rgb[1], 0.0)],
406+
"blue": [(0.0, 0.0, 0.0), (1.0, rgb[2], 0.0)],
407+
}
408+
cmap = colors.LinearSegmentedColormap("linear", segmentdata=cmapdata, N=256)
409+
args = {"cmap": cmap}
401410
elif self._image.ndim == 2:
402-
args = {'cmap': 'gray'}
403-
if 'zorder' not in kwargs:
404-
args['zorder'] = 3
405-
406-
self._object = imshow_affine(self._ax, self._image,
407-
interpolation='none',
408-
extent=extent, clip_on=True,
409-
**{**kwargs, **args})
411+
args = {"cmap": "gray"}
412+
if "zorder" not in kwargs:
413+
args["zorder"] = 3
414+
415+
self._object = imshow_affine(
416+
self._ax,
417+
self._image,
418+
interpolation="none",
419+
extent=extent,
420+
clip_on=True,
421+
**{**kwargs, **args},
422+
)
410423

411424
def _update(self, x):
412425

@@ -415,12 +428,15 @@ def _update(self, x):
415428
center_x = 0
416429
center_y = 0
417430

418-
T = (mtransforms.Affine2D()
419-
.rotate_deg_around(center_x, center_y, np.degrees(x[2]) - self._rotation)
420-
.translate(x[0], x[1])
421-
+ self._ax.transData)
431+
T = (
432+
mtransforms.Affine2D()
433+
.rotate_deg_around(center_x, center_y, np.degrees(x[2]) - self._rotation)
434+
.translate(x[0], x[1])
435+
+ self._ax.transData
436+
)
422437
self._object.set_transform(T)
423438

439+
424440
if __name__ == "__main__":
425441
from math import pi
426442

@@ -442,14 +458,14 @@ def _update(self, x):
442458
print(veh.f([0, 0, 0], odo))
443459

444460
def control(v, t, x):
445-
goal = (6,6)
446-
goal_heading = atan2(goal[1]-x[1], goal[0]-x[0])
461+
goal = (6, 6)
462+
goal_heading = atan2(goal[1] - x[1], goal[0] - x[0])
447463
d_heading = base.angdiff(goal_heading, x[2])
448464
v.stopif(base.norm(x[0:2] - goal) < 0.1)
449465

450466
return (1, d_heading)
451467

452-
veh.control=RandomPath(10)
468+
veh.control = RandomPath(10)
453469
p = veh.run(1000, plot=True)
454470
# plt.show()
455471
print(p)
@@ -465,7 +481,6 @@ def control(v, t, x):
465481
# ax.set_xlim(-5, 5)
466482
# ax.set_ylim(-5, 5)
467483

468-
469484
# v = VehicleAnimation.Polygon(shape='triangle', maxdim=0.1, color='r')
470485
# v = VehicleAnimation.Icon('car3.png', maxdim=2, centre=[0.3, 0.5])
471486
# v = VehicleAnimation.Icon('/Users/corkep/Dropbox/code/robotics-toolbox-python/roboticstoolbox/data/car1.png', maxdim=2, centre=[0.3, 0.5])
@@ -477,4 +492,4 @@ def control(v, t, x):
477492

478493
# for theta in np.linspace(0, 2 * np.pi, 100):
479494
# v.update([0, 0, theta])
480-
# plt.pause(0.1)
495+
# plt.pause(0.1)

0 commit comments

Comments
 (0)