Skip to content

Commit 6575d48

Browse files
jhavljhavl
committed
small fixes
1 parent 5c071d0 commit 6575d48

File tree

3 files changed

+95
-88
lines changed

3 files changed

+95
-88
lines changed

‎roboticstoolbox/examples/swift_benchmark.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
import numpy as np
1010
import cProfile
1111

12-
env = swift.Swift(_dev=False)
12+
env = swift.Swift(_dev=True)
1313
env.launch()
1414

1515

‎roboticstoolbox/robot/Robot.py

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -141,7 +141,7 @@ def __getitem__(self, i):
141141
>>> print(robot[1]) # print the 2nd link
142142
>>> print([link.a for link in robot]) # print all the a_j values
143143
144-
.. note:: ``ERobot`` supports link lookup by name,
144+
.. note:: ``ERobot`` supports link lookup by name,
145145
eg. ``robot['link1']``
146146
"""
147147
if isinstance(i, str):
@@ -237,7 +237,8 @@ def qrandom(self):
237237
qlim = self.qlim
238238
if np.any(np.isnan(qlim)):
239239
raise ValueError('some joint limits not defined')
240-
return np.random.uniform(low=qlim[0, :], high=qlim[1, :], size=(self.n,))
240+
return np.random.uniform(
241+
low=qlim[0, :], high=qlim[1, :], size=(self.n,))
241242

242243
def addconfiguration(self, name, q, unit='rad'):
243244
"""
@@ -848,6 +849,11 @@ def base(self):
848849
self._base = SE2()
849850
else:
850851
self._base = SE3()
852+
elif isinstance(self._base, np.ndarray):
853+
# if isinstance(self, ERobot2):
854+
# self._base = SE2(self._base, check=False)
855+
# else:
856+
self._base = SE3(self._base, check=False)
851857

852858
return self._base
853859

@@ -1034,7 +1040,8 @@ def control_type(self, cn):
10341040
# TODO probably should be a static method
10351041
def _get_graphical_backend(self, backend):
10361042
#
1037-
# find the right backend, modules are imported here on an as needs basis
1043+
# find the right backend, modules are imported here on an as needs
1044+
# basis
10381045
if backend.lower() == 'swift': # pragma nocover
10391046
if isinstance(self, rtb.DHRobot):
10401047
raise NotImplementedError(

‎tests/test_ERobot.py

Lines changed: 84 additions & 84 deletions
Original file line numberDiff line numberDiff line change
@@ -13,12 +13,13 @@
1313
import spatialgeometry as gm
1414
from math import pi, sin, cos
1515

16+
1617
class TestERobot(unittest.TestCase):
1718

1819
def test_init(self):
1920
ets = tb.ETS.rz()
2021
robot = ERobot(ets, name='myname', manufacturer='I made it',
21-
comment='other stuff')
22+
comment='other stuff')
2223
self.assertEqual(robot.name, 'myname')
2324
self.assertEqual(robot.manufacturer, 'I made it')
2425
self.assertEqual(robot.comment, 'other stuff')
@@ -44,7 +45,8 @@ def test_init_ets(self):
4445

4546
def test_init_elink(self):
4647
link1 = ELink(ETS.rx(), name='link1')
47-
link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2', parent=link1)
48+
link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(),
49+
name='link2', parent=link1)
4850
link3 = ELink(ETS.tx(1), name='ee_1', parent=link2)
4951
robot = ERobot([link1, link2, link3])
5052
self.assertEqual(robot.n, 2)
@@ -66,7 +68,8 @@ def test_init_elink(self):
6668
self.assertEqual(robot[2].children, [])
6769

6870
link1 = ELink(ETS.rx(), name='link1')
69-
link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2', parent='link1')
71+
link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(),
72+
name='link2', parent='link1')
7073
link3 = ELink(ETS.tx(1), name='ee_1', parent='link2')
7174
robot = ERobot([link1, link2, link3])
7275
self.assertEqual(robot.n, 2)
@@ -84,13 +87,12 @@ def test_init_elink(self):
8487
self.assertEqual(robot[1].children, [robot[2]])
8588
self.assertEqual(robot[2].children, [])
8689

87-
8890
def test_init_elink_autoparent(self):
8991
links = [
9092
ELink(ETS.rx(), name='link1'),
9193
ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2'),
9294
ELink(ETS.tx(1), name='ee_1')
93-
]
95+
]
9496
robot = ERobot(links)
9597
self.assertEqual(robot.n, 2)
9698
self.assertIsInstance(robot[0], ELink)
@@ -106,7 +108,6 @@ def test_init_elink_autoparent(self):
106108
self.assertEqual(robot[1].children, [robot[2]])
107109
self.assertEqual(robot[2].children, [])
108110

109-
110111
def test_init_elink_branched(self):
111112
robot = ERobot([
112113
ELink(ETS.rz(), name='link1'),
@@ -247,10 +248,10 @@ def test_fkine(self):
247248
q3 = np.expand_dims(q1, 0)
248249

249250
ans = np.array([
250-
[-0.50827907, -0.57904589, 0.63746234, 0.44682295],
251-
[0.83014553, -0.52639462, 0.18375824, 0.16168396],
252-
[0.22915229, 0.62258699, 0.74824773, 0.96798113],
253-
[0., 0., 0., 1.]
251+
[-0.50827907, -0.57904589, 0.63746234, 0.44682295],
252+
[0.83014553, -0.52639462, 0.18375824, 0.16168396],
253+
[0.22915229, 0.62258699, 0.74824773, 0.96798113],
254+
[0., 0., 0., 1.]
254255
])
255256

256257
panda.q = q1
@@ -266,10 +267,10 @@ def test_fkine_traj(self):
266267
qq = np.r_[q, q, q, q]
267268

268269
ans = np.array([
269-
[-0.50827907, -0.57904589, 0.63746234, 0.44682295],
270-
[0.83014553, -0.52639462, 0.18375824, 0.16168396],
271-
[0.22915229, 0.62258699, 0.74824773, 0.96798113],
272-
[0., 0., 0., 1.]
270+
[-0.50827907, -0.57904589, 0.63746234, 0.44682295],
271+
[0.83014553, -0.52639462, 0.18375824, 0.16168396],
272+
[0.22915229, 0.62258699, 0.74824773, 0.96798113],
273+
[0., 0., 0., 1.]
273274
])
274275

275276
TT = panda.fkine(qq)
@@ -303,23 +304,23 @@ def test_jacob0(self):
303304
q4 = np.expand_dims(q1, 1)
304305

305306
ans = np.array([
306-
[-1.61683957e-01, 1.07925929e-01, -3.41453006e-02,
307-
3.35029257e-01, -1.07195463e-02, 1.03187865e-01,
307+
[-1.61683957e-01, 1.07925929e-01, -3.41453006e-02,
308+
3.35029257e-01, -1.07195463e-02, 1.03187865e-01,
308309
0.00000000e+00],
309-
[4.46822947e-01, 6.25741987e-01, 4.16474664e-01,
310-
-8.04745724e-02, 7.78257566e-02, -1.17720983e-02,
310+
[4.46822947e-01, 6.25741987e-01, 4.16474664e-01,
311+
-8.04745724e-02, 7.78257566e-02, -1.17720983e-02,
311312
0.00000000e+00],
312313
[0.00000000e+00, -2.35276631e-01, -8.20187641e-02,
313314
-5.14076923e-01, -9.98040745e-03, -2.02626953e-01,
314315
0.00000000e+00],
315-
[1.29458954e-16, -9.85449730e-01, 3.37672585e-02,
316-
-6.16735653e-02, 6.68449878e-01, -1.35361558e-01,
316+
[1.29458954e-16, -9.85449730e-01, 3.37672585e-02,
317+
-6.16735653e-02, 6.68449878e-01, -1.35361558e-01,
317318
6.37462344e-01],
318-
[9.07021273e-18, 1.69967143e-01, 1.95778638e-01,
319-
9.79165111e-01, 1.84470262e-01, 9.82748279e-01,
319+
[9.07021273e-18, 1.69967143e-01, 1.95778638e-01,
320+
9.79165111e-01, 1.84470262e-01, 9.82748279e-01,
320321
1.83758244e-01],
321-
[1.00000000e+00, -2.26036604e-17, 9.80066578e-01,
322-
-1.93473657e-01, 7.20517510e-01, -1.26028049e-01,
322+
[1.00000000e+00, -2.26036604e-17, 9.80066578e-01,
323+
-1.93473657e-01, 7.20517510e-01, -1.26028049e-01,
323324
7.48247732e-01]
324325
])
325326

@@ -340,7 +341,7 @@ def test_hessian0(self):
340341
ans = np.array([
341342
[
342343
[-4.46822947e-01, -6.25741987e-01, -4.16474664e-01,
343-
8.04745724e-02, -7.78257566e-02, 1.17720983e-02,
344+
8.04745724e-02, -7.78257566e-02, 1.17720983e-02,
344345
0.00000000e+00],
345346
[-6.25741987e-01, -3.99892968e-02, -1.39404950e-02,
346347
-8.73761859e-02, -1.69634134e-03, -3.44399243e-02,
@@ -349,131 +350,131 @@ def test_hessian0(self):
349350
-2.17748413e-02, -7.82283735e-02, -2.81325889e-02,
350351
0.00000000e+00],
351352
[8.04745724e-02, -8.73761859e-02, -2.17748413e-02,
352-
-5.18935898e-01, 5.28476698e-03, -2.00682834e-01,
353+
-5.18935898e-01, 5.28476698e-03, -2.00682834e-01,
353354
0.00000000e+00],
354355
[-7.78257566e-02, -1.69634134e-03, -7.82283735e-02,
355356
5.28476698e-03, -5.79159088e-02, -2.88966443e-02,
356357
0.00000000e+00],
357358
[1.17720983e-02, -3.44399243e-02, -2.81325889e-02,
358359
-2.00682834e-01, -2.88966443e-02, -2.00614904e-01,
359360
0.00000000e+00],
360-
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
361-
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
361+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
362+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
362363
0.00000000e+00]
363364
],
364365
[
365-
[-1.61683957e-01, 1.07925929e-01, -3.41453006e-02,
366-
3.35029257e-01, -1.07195463e-02, 1.03187865e-01,
366+
[-1.61683957e-01, 1.07925929e-01, -3.41453006e-02,
367+
3.35029257e-01, -1.07195463e-02, 1.03187865e-01,
367368
0.00000000e+00],
368369
[1.07925929e-01, -2.31853293e-01, -8.08253690e-02,
369370
-5.06596965e-01, -9.83518983e-03, -1.99678676e-01,
370371
0.00000000e+00],
371372
[-3.41453006e-02, -8.08253690e-02, -3.06951191e-02,
372-
3.45709946e-01, -1.01688580e-02, 1.07973135e-01,
373+
3.45709946e-01, -1.01688580e-02, 1.07973135e-01,
373374
0.00000000e+00],
374-
[3.35029257e-01, -5.06596965e-01, 3.45709946e-01,
375-
-9.65242924e-02, 1.45842251e-03, -3.24608603e-02,
375+
[3.35029257e-01, -5.06596965e-01, 3.45709946e-01,
376+
-9.65242924e-02, 1.45842251e-03, -3.24608603e-02,
376377
0.00000000e+00],
377378
[-1.07195463e-02, -9.83518983e-03, -1.01688580e-02,
378-
1.45842251e-03, -1.05221866e-03, 2.09794626e-01,
379+
1.45842251e-03, -1.05221866e-03, 2.09794626e-01,
379380
0.00000000e+00],
380-
[1.03187865e-01, -1.99678676e-01, 1.07973135e-01,
381-
-3.24608603e-02, 2.09794626e-01, -4.04324654e-02,
381+
[1.03187865e-01, -1.99678676e-01, 1.07973135e-01,
382+
-3.24608603e-02, 2.09794626e-01, -4.04324654e-02,
382383
0.00000000e+00],
383-
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
384-
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
384+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
385+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
385386
0.00000000e+00]
386387
],
387388
[
388-
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
389-
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
389+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
390+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
390391
0.00000000e+00],
391392
[0.00000000e+00, -6.34981134e-01, -4.04611266e-01,
392393
2.23596800e-02, -7.48714002e-02, -5.93773551e-03,
393394
0.00000000e+00],
394-
[0.00000000e+00, -4.04611266e-01, 2.07481281e-02,
395-
-6.83089775e-02, 4.72662062e-03, -2.05994912e-02,
395+
[0.00000000e+00, -4.04611266e-01, 2.07481281e-02,
396+
-6.83089775e-02, 4.72662062e-03, -2.05994912e-02,
396397
0.00000000e+00],
397-
[0.00000000e+00, 2.23596800e-02, -6.83089775e-02,
398-
-3.23085806e-01, 5.69641385e-03, -1.00311930e-01,
398+
[0.00000000e+00, 2.23596800e-02, -6.83089775e-02,
399+
-3.23085806e-01, 5.69641385e-03, -1.00311930e-01,
399400
0.00000000e+00],
400-
[0.00000000e+00, -7.48714002e-02, 4.72662062e-03,
401-
5.69641385e-03, 5.40000550e-02, -2.69041502e-02,
401+
[0.00000000e+00, -7.48714002e-02, 4.72662062e-03,
402+
5.69641385e-03, 5.40000550e-02, -2.69041502e-02,
402403
0.00000000e+00],
403404
[0.00000000e+00, -5.93773551e-03, -2.05994912e-02,
404405
-1.00311930e-01, -2.69041502e-02, -9.98142073e-02,
405406
0.00000000e+00],
406-
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
407-
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
407+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
408+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
408409
0.00000000e+00]
409410
],
410411
[
411412
[-9.07021273e-18, -2.77555756e-17, -2.77555756e-17,
412-
-1.11022302e-16, -2.77555756e-17, 0.00000000e+00,
413+
-1.11022302e-16, -2.77555756e-17, 0.00000000e+00,
413414
-2.77555756e-17],
414-
[-1.69967143e-01, -1.97756387e-17, 4.11786040e-17,
415+
[-1.69967143e-01, -1.97756387e-17, 4.11786040e-17,
415416
-1.48932398e-16, -5.07612940e-17, -8.38219650e-17,
416417
-4.90138154e-17],
417-
[-1.95778638e-01, 1.66579116e-01, -1.38777878e-17,
418-
1.04083409e-17, -1.38777878e-17, 3.46944695e-18,
418+
[-1.95778638e-01, 1.66579116e-01, -1.38777878e-17,
419+
1.04083409e-17, -1.38777878e-17, 3.46944695e-18,
419420
0.00000000e+00],
420421
[-9.79165111e-01, -3.28841647e-02, -9.97525009e-01,
421-
-4.16333634e-17, -1.14491749e-16, 1.38777878e-17,
422+
-4.16333634e-17, -1.14491749e-16, 1.38777878e-17,
422423
-6.24500451e-17],
423-
[-1.84470262e-01, 1.22464303e-01, -3.97312016e-02,
424-
7.41195745e-01, -2.77555756e-17, 1.12757026e-16,
424+
[-1.84470262e-01, 1.22464303e-01, -3.97312016e-02,
425+
7.41195745e-01, -2.77555756e-17, 1.12757026e-16,
425426
2.77555756e-17],
426427
[-9.82748279e-01, -2.14206274e-02, -9.87832342e-01,
427-
6.67336352e-02, -7.31335770e-01, 2.08166817e-17,
428+
6.67336352e-02, -7.31335770e-01, 2.08166817e-17,
428429
-6.07153217e-17],
429-
[-1.83758244e-01, 1.27177529e-01, -3.36043908e-02,
430-
7.68210453e-01, 5.62842325e-03, 7.58497864e-01,
430+
[-1.83758244e-01, 1.27177529e-01, -3.36043908e-02,
431+
7.68210453e-01, 5.62842325e-03, 7.58497864e-01,
431432
0.00000000e+00]
432433
],
433434
[
434-
[1.29458954e-16, -1.11022302e-16, 8.67361738e-17,
435-
-4.16333634e-17, 5.55111512e-17, 2.77555756e-17,
435+
[1.29458954e-16, -1.11022302e-16, 8.67361738e-17,
436+
-4.16333634e-17, 5.55111512e-17, 2.77555756e-17,
436437
5.55111512e-17],
437438
[-9.85449730e-01, -6.36381327e-17, -1.02735399e-16,
438-
-1.83043043e-17, -5.63484308e-17, 8.08886307e-18,
439+
-1.83043043e-17, -5.63484308e-17, 8.08886307e-18,
439440
1.07112702e-18],
440-
[3.37672585e-02, 9.65806345e-01, 8.32667268e-17,
441-
-2.55871713e-17, 1.07552856e-16, 2.08166817e-17,
441+
[3.37672585e-02, 9.65806345e-01, 8.32667268e-17,
442+
-2.55871713e-17, 1.07552856e-16, 2.08166817e-17,
442443
-5.20417043e-18],
443444
[-6.16735653e-02, -1.90658563e-01, -5.39111251e-02,
444-
-6.59194921e-17, -2.77555756e-17, 2.38524478e-17,
445+
-6.59194921e-17, -2.77555756e-17, 2.38524478e-17,
445446
-4.16333634e-17],
446-
[6.68449878e-01, 7.10033786e-01, 6.30795483e-01,
447-
-8.48905588e-02, 0.00000000e+00, 3.46944695e-17,
447+
[6.68449878e-01, 7.10033786e-01, 6.30795483e-01,
448+
-8.48905588e-02, 0.00000000e+00, 3.46944695e-17,
448449
2.77555756e-17],
449450
[-1.35361558e-01, -1.24194307e-01, -1.28407717e-01,
450-
1.84162966e-02, -1.32869389e-02, 2.77555756e-17,
451+
1.84162966e-02, -1.32869389e-02, 2.77555756e-17,
451452
-2.08166817e-17],
452-
[6.37462344e-01, 7.37360525e-01, 5.99489263e-01,
453-
-7.71850655e-02, -4.08633244e-02, 2.09458434e-02,
453+
[6.37462344e-01, 7.37360525e-01, 5.99489263e-01,
454+
-7.71850655e-02, -4.08633244e-02, 2.09458434e-02,
454455
0.00000000e+00]
455456
],
456457
[
457458
[0.00000000e+00, -6.59521910e-17, -1.31033786e-16,
458-
-1.92457571e-16, 1.54134782e-17, -7.69804929e-17,
459+
-1.92457571e-16, 1.54134782e-17, -7.69804929e-17,
459460
1.11140361e-17],
460-
[0.00000000e+00, -2.77555756e-17, 7.15573434e-17,
461-
1.65666092e-16, 1.38777878e-17, -8.67361738e-18,
461+
[0.00000000e+00, -2.77555756e-17, 7.15573434e-17,
462+
1.65666092e-16, 1.38777878e-17, -8.67361738e-18,
462463
3.46944695e-17],
463-
[0.00000000e+00, -1.98669331e-01, 8.67361738e-18,
464-
-1.46584134e-16, 6.02816408e-17, -3.12250226e-17,
464+
[0.00000000e+00, -1.98669331e-01, 8.67361738e-18,
465+
-1.46584134e-16, 6.02816408e-17, -3.12250226e-17,
465466
6.11490025e-17],
466-
[0.00000000e+00, -9.54435515e-01, 4.51380881e-02,
467-
1.38777878e-17, 1.08420217e-16, 3.46944695e-18,
467+
[0.00000000e+00, -9.54435515e-01, 4.51380881e-02,
468+
1.38777878e-17, 1.08420217e-16, 3.46944695e-18,
468469
6.24500451e-17],
469470
[0.00000000e+00, -2.95400686e-01, -1.24639152e-01,
470471
-6.65899738e-01, -4.85722573e-17, -5.20417043e-18,
471472
-5.55111512e-17],
472-
[0.00000000e+00, -9.45442009e-01, 5.96856167e-02,
473-
7.19317248e-02, 6.81888149e-01, -2.77555756e-17,
473+
[0.00000000e+00, -9.45442009e-01, 5.96856167e-02,
474+
7.19317248e-02, 6.81888149e-01, -2.77555756e-17,
474475
1.04083409e-17],
475476
[0.00000000e+00, -2.89432165e-01, -1.18596498e-01,
476-
-6.35513913e-01, 5.24032975e-03, -6.51338823e-01,
477+
-6.35513913e-01, 5.24032975e-03, -6.51338823e-01,
477478
0.00000000e+00]
478479
]
479480
])
@@ -794,18 +795,18 @@ def test_invdyn(self):
794795
tau = robot.rne(z, z, z) / 9.81
795796
nt.assert_array_almost_equal(tau, np.r_[-2, -0.5])
796797

797-
tau = robot.rne([0, -pi/2], z, z) / 9.81
798+
tau = robot.rne([0, -pi / 2], z, z) / 9.81
798799
nt.assert_array_almost_equal(tau, np.r_[-1.5, 0])
799800

800-
tau = robot.rne([-pi/2, pi/2], z, z) / 9.81
801+
tau = robot.rne([-pi / 2, pi / 2], z, z) / 9.81
801802
nt.assert_array_almost_equal(tau, np.r_[-0.5, -0.5])
802803

803-
tau = robot.rne([-pi/2, 0], z, z) / 9.81
804+
tau = robot.rne([-pi / 2, 0], z, z) / 9.81
804805
nt.assert_array_almost_equal(tau, np.r_[0, 0])
805806

806807
# check velocity terms
807808
robot.gravity = [0, 0, 0]
808-
q = [0, -pi/2]
809+
q = [0, -pi / 2]
809810
h = -0.5 * sin(q[1])
810811

811812
tau = robot.rne(q, [0, 0], z)
@@ -840,7 +841,6 @@ def test_invdyn(self):
840841
nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22])
841842

842843

843-
844844
if __name__ == '__main__': # pragma nocover
845845

846846
unittest.main()

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy