Skip to content

Commit fd4a29d

Browse files
jhavljhavl
committed
tests fixed
1 parent c5457df commit fd4a29d

File tree

3 files changed

+79
-64
lines changed

3 files changed

+79
-64
lines changed

‎examples/swift2.py

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
@author Jesse Haviland
44
"""
55

6+
67
# import fknm
78
# import numpy as np
89
# import spatialmath as sm
@@ -80,6 +81,26 @@
8081

8182
from spatialmath.base import r2q
8283

84+
85+
pm = rtb.models.DH.Panda()
86+
p = rtb.models.ETS.Panda()
87+
p2 = rtb.models.Panda()
88+
q = np.array([1, 2, 3, 4, 5, 6, 7])
89+
p.q = q
90+
pm.q = q
91+
92+
p.fkine_all(q)
93+
p2.fkine_all(q)
94+
r2 = pm.fkine_all(q)
95+
96+
for i in range(7):
97+
print(np.round(p.links[i]._fk, 2))
98+
# print(np.round(p2.links[i]._fk, 2))
99+
print(np.round(r2[i].A, 2))
100+
101+
print()
102+
print()
103+
83104
# import swift
84105

85106

‎roboticstoolbox/models/DH/Panda.py

Lines changed: 48 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -42,54 +42,54 @@ def __init__(self):
4242
# This Panda model is defined using modified
4343
# Denavit-Hartenberg parameters
4444
L = [
45-
RevoluteMDH(
46-
a=0.0,
47-
d=0.333,
48-
alpha=0.0,
49-
qlim=np.array([-2.8973, 2.8973])
50-
),
51-
52-
RevoluteMDH(
53-
a=0.0,
54-
d=0.0,
55-
alpha=-np.pi/2,
56-
qlim=np.array([-1.7628, 1.7628])
57-
),
58-
59-
RevoluteMDH(
60-
a=0.0,
61-
d=0.316,
62-
alpha=np.pi/2,
63-
qlim=np.array([-2.8973, 2.8973])
64-
),
65-
66-
RevoluteMDH(
67-
a=0.0825,
68-
d=0.0,
69-
alpha=np.pi/2,
70-
qlim=np.array([-3.0718, -0.0698])
71-
),
72-
73-
RevoluteMDH(
74-
a=-0.0825,
75-
d=0.384,
76-
alpha=-np.pi/2,
77-
qlim=np.array([-2.8973, 2.8973])
78-
),
79-
80-
RevoluteMDH(
81-
a=0.0,
82-
d=0.0,
83-
alpha=np.pi/2,
84-
qlim=np.array([-0.0175, 3.7525])
85-
),
86-
87-
RevoluteMDH(
88-
a=0.088,
89-
d=flange,
90-
alpha=np.pi/2,
91-
qlim=np.array([-2.8973, 2.8973])
92-
)
45+
RevoluteMDH(
46+
a=0.0,
47+
d=0.333,
48+
alpha=0.0,
49+
qlim=np.array([-2.8973, 2.8973])
50+
),
51+
52+
RevoluteMDH(
53+
a=0.0,
54+
d=0.0,
55+
alpha=-np.pi/2,
56+
qlim=np.array([-1.7628, 1.7628])
57+
),
58+
59+
RevoluteMDH(
60+
a=0.0,
61+
d=0.316,
62+
alpha=np.pi/2,
63+
qlim=np.array([-2.8973, 2.8973])
64+
),
65+
66+
RevoluteMDH(
67+
a=0.0825,
68+
d=0.0,
69+
alpha=np.pi/2,
70+
qlim=np.array([-3.0718, -0.0698])
71+
),
72+
73+
RevoluteMDH(
74+
a=-0.0825,
75+
d=0.384,
76+
alpha=-np.pi/2,
77+
qlim=np.array([-2.8973, 2.8973])
78+
),
79+
80+
RevoluteMDH(
81+
a=0.0,
82+
d=0.0,
83+
alpha=np.pi/2,
84+
qlim=np.array([-0.0175, 3.7525])
85+
),
86+
87+
RevoluteMDH(
88+
a=0.088,
89+
d=flange,
90+
alpha=np.pi/2,
91+
qlim=np.array([-2.8973, 2.8973])
92+
)
9393
]
9494

9595
tool = transl(0, 0, tool_offset) @ trotz(-np.pi/4)
@@ -109,4 +109,3 @@ def __init__(self):
109109

110110
panda = Panda()
111111
print(panda)
112-

‎roboticstoolbox/robot/ERobot.py

Lines changed: 10 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -353,61 +353,56 @@ def vis_children(link):
353353
# path.reverse()
354354
# return path
355355

356-
def to_dict(self):
357-
# ob = {
358-
# 'links': [],
359-
# 'name': self.name,
360-
# 'n': self.n
361-
# }
356+
def to_dict(self, show_robot=True, show_collision=False):
362357

363358
self.fkine_all(self.q)
364359

365360
ob = []
366361

367362
for link in self.links:
368363

369-
if self._show_robot:
364+
if show_robot:
370365
for gi in link.geometry:
371366
ob.append(gi.to_dict())
372-
if self._show_collision:
367+
if show_collision:
373368
for gi in link.collision:
374369
ob.append(gi.to_dict())
375370

376371
# Do the grippers now
377372
for gripper in self.grippers:
378373
for link in gripper.links:
379374

380-
if self._show_robot:
375+
if show_robot:
381376
for gi in link.geometry:
382377
ob.append(gi.to_dict())
383-
if self._show_collision:
378+
if show_collision:
384379
for gi in link.collision:
385380
ob.append(gi.to_dict())
386381

387382
return ob
388383

389-
def fk_dict(self):
384+
def fk_dict(self, show_robot=True, show_collision=False):
390385
ob = []
391386

392387
self.fkine_all(self.q)
393388

394389
# Do the robot
395390
for link in self.links:
396391

397-
if self._show_robot:
392+
if show_robot:
398393
for gi in link.geometry:
399394
ob.append(gi.fk_dict())
400-
if self._show_collision:
395+
if show_collision:
401396
for gi in link.collision:
402397
ob.append(gi.fk_dict())
403398

404399
# Do the grippers now
405400
for gripper in self.grippers:
406401
for link in gripper.links:
407-
if self._show_robot:
402+
if show_robot:
408403
for gi in link.geometry:
409404
ob.append(gi.fk_dict())
410-
if self._show_collision:
405+
if show_collision:
411406
for gi in link.collision:
412407
ob.append(gi.fk_dict())
413408

0 commit comments

Comments
 (0)