Skip to content

Commit a090a1e

Browse files
committed
Fix lgtm issues
1 parent 3139914 commit a090a1e

File tree

9 files changed

+333
-334
lines changed

9 files changed

+333
-334
lines changed

roboticstoolbox/backends/PyPlot/RobotPlot.py

+47-45
Original file line numberDiff line numberDiff line change
@@ -6,15 +6,22 @@
66
import numpy as np
77
import roboticstoolbox as rp
88
from spatialmath import SE3
9-
from spatialmath import base
109

1110

12-
class RobotPlot():
13-
11+
class RobotPlot:
1412
def __init__(
15-
self, robot, env, readonly, display=True,
16-
jointaxes=True, jointlabels=False, eeframe=True, shadow=True,
17-
name=True, options=None):
13+
self,
14+
robot,
15+
env,
16+
readonly,
17+
display=True,
18+
jointaxes=True,
19+
jointlabels=False,
20+
eeframe=True,
21+
shadow=True,
22+
name=True,
23+
options=None,
24+
):
1825

1926
super(RobotPlot, self).__init__()
2027

@@ -55,15 +62,15 @@ def __init__(
5562
self.showname = name
5663

5764
defaults = {
58-
'robot': {'color': '#E16F6D', 'linewidth': 5},
59-
'shadow': {'color': 'lightgrey', 'linewidth': 3},
60-
'jointaxes': {'color': '#8FC1E2', 'linewidth': 2},
61-
'jointlabels': {},
62-
'jointaxislength': 0.2,
63-
'eex': {'color': '#F84752', 'linewidth': 2}, # '#EE9494'
64-
'eey': {'color': '#BADA55', 'linewidth': 2}, # '#93E7B0'
65-
'eez': {'color': '#54AEFF', 'linewidth': 2},
66-
'eelength': 0.06,
65+
"robot": {"color": "#E16F6D", "linewidth": 5},
66+
"shadow": {"color": "lightgrey", "linewidth": 3},
67+
"jointaxes": {"color": "#8FC1E2", "linewidth": 2},
68+
"jointlabels": {},
69+
"jointaxislength": 0.2,
70+
"eex": {"color": "#F84752", "linewidth": 2}, # '#EE9494'
71+
"eey": {"color": "#BADA55", "linewidth": 2}, # '#93E7B0'
72+
"eez": {"color": "#54AEFF", "linewidth": 2},
73+
"eelength": 0.06,
6774
}
6875

6976
if options is not None:
@@ -83,7 +90,7 @@ def draw(self):
8390

8491
# compute all link frames
8592
T = self.robot.fkine_all(self.robot.q)
86-
93+
8794
# draw all the line segments for the noodle plot
8895
for i, segment in enumerate(self.segments):
8996
linkframes = []
@@ -95,8 +102,8 @@ def draw(self):
95102
points = np.array([linkframe.t for linkframe in linkframes])
96103

97104
self.links[i].set_xdata(points[:, 0])
98-
self.links[i].set_ydata(points[:,1 ])
99-
self.links[0].set_3d_properties(points[:,2 ])
105+
self.links[i].set_ydata(points[:, 1])
106+
self.links[0].set_3d_properties(points[:, 2])
100107

101108
# Update the shadow of the robot links
102109
if self.shadow:
@@ -115,7 +122,7 @@ def draw(self):
115122

116123
if self.eeframe:
117124
# Axes arrow transforms
118-
len = self.options['eelength']
125+
len = self.options["eelength"]
119126
Tjx = SE3([len, 0, 0])
120127
Tjy = SE3([0, len, 0])
121128
Tjz = SE3([0, 0, len])
@@ -129,9 +136,9 @@ def draw(self):
129136
Tey = Te * Tjy
130137
Tez = Te * Tjz
131138

132-
xaxis = self._plot_quiver(Te.t, Tex.t, self.options['eex'])
133-
yaxis = self._plot_quiver(Te.t, Tey.t, self.options['eey'])
134-
zaxis = self._plot_quiver(Te.t, Tez.t, self.options['eez'])
139+
xaxis = self._plot_quiver(Te.t, Tex.t, self.options["eex"])
140+
yaxis = self._plot_quiver(Te.t, Tey.t, self.options["eey"])
141+
zaxis = self._plot_quiver(Te.t, Tez.t, self.options["eez"])
135142

136143
self.eeframes.extend([xaxis, yaxis, zaxis])
137144

@@ -159,18 +166,17 @@ def draw(self):
159166
elif link.isjoint:
160167
Tj = T[link.number]
161168
R = Tj.R
162-
if link.v.axis[1] == 'z':
169+
if link.v.axis[1] == "z":
163170
direction = R[:, 2] # z direction
164-
elif link.v.axis[1] == 'y':
171+
elif link.v.axis[1] == "y":
165172
direction = R[:, 1] # y direction
166-
elif link.v.axis[1] == 'x':
173+
elif link.v.axis[1] == "x":
167174
direction = R[:, 0] # direction
168175

169176
if direction is not None:
170177
arrow = self._plot_quiver2(Tj.t, direction, link.jindex)
171178
self.joints.extend(arrow)
172179

173-
174180
def init(self):
175181

176182
self.drawn = True
@@ -189,54 +195,50 @@ def init(self):
189195

190196
# Plot robot name
191197
if self.showname:
192-
self.name = self.ax.text(
193-
Tb.t[0], Tb.t[1], 0.05, self.robot.name)
198+
self.name = self.ax.text(Tb.t[0], Tb.t[1], 0.05, self.robot.name)
194199

195200
# Initialize the robot links
196201
self.links = []
197202
self.sh_links = []
198203
for i in range(len(self.segments)):
199204

200-
201205
# Plot the shadow of the robot links, draw first so robot is always
202206
# in front
203207
if self.shadow:
204-
shadow, = self.ax.plot(
205-
[0], [0],
206-
zorder=1,
207-
**self.options['shadow'])
208+
(shadow,) = self.ax.plot([0], [0], zorder=1, **self.options["shadow"])
208209
self.sh_links.append(shadow)
209210

210-
line, = self.ax.plot(
211-
[0], [0], [0], **self.options['robot'])
211+
(line,) = self.ax.plot([0], [0], [0], **self.options["robot"])
212212
self.links.append(line)
213213

214214
self.eeframes = []
215215
self.joints = []
216216

217217
def _plot_quiver(self, p0, p1, options):
218218
qv = self.ax.quiver(
219-
p0[0], p0[1], p0[2],
220-
p1[0] - p0[0],
221-
p1[1] - p0[1],
222-
p1[2] - p0[2],
223-
**options
219+
p0[0], p0[1], p0[2], p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2], **options
224220
)
225221
return qv
226222

227223
def _plot_quiver2(self, p0, dir, j):
228-
vec = dir * self.options['jointaxislength']
224+
vec = dir * self.options["jointaxislength"]
229225
start = p0 - vec / 2
230226
qv = self.ax.quiver(
231-
start[0], start[1], start[2],
232-
vec[0], vec[1], vec[2],
227+
start[0],
228+
start[1],
229+
start[2],
230+
vec[0],
231+
vec[1],
232+
vec[2],
233233
zorder=5,
234-
**self.options['jointaxes']
234+
**self.options["jointaxes"],
235235
)
236236

237237
if self.jointlabels:
238238
pl = p0 - vec * 0.6
239-
label = self.ax.text(pl[0], pl[1], pl[2], f'$q_{j}$', **self.options['jointlabels'] )
239+
label = self.ax.text(
240+
pl[0], pl[1], pl[2], f"$q_{j}$", **self.options["jointlabels"]
241+
)
240242
return [qv, label]
241243
else:
242244
return [qv]

roboticstoolbox/examples/tripleangle.py

+1-4
Original file line numberDiff line numberDiff line change
@@ -26,19 +26,16 @@
2626
g1 = Mesh(
2727
filename=str(path / "gimbal-ring1.stl"), color=[34, 143, 201], scale=(1.0 / 3,) * 3
2828
)
29-
g1.v = [0, 0, 0, 0, 0.5, 0]
3029

3130
g2 = Mesh(
3231
filename=str(path / "gimbal-ring2.stl"), color=[31, 184, 72], scale=(1.1 / 3,) * 3
3332
)
34-
g2.v = [0, 0, 0, 0.5, 0, 0]
3533

3634
g3 = Mesh(
3735
filename=str(path / "gimbal-ring3.stl"),
3836
color=[240, 103, 103],
3937
scale=(1.1 ** 2 / 3,) * 3,
4038
)
41-
g3.v = [0, 0, 0, 0, 0, 0.5]
4239

4340
plane = Mesh(
4441
filename=str(path / "spitfire_assy-gear_up.stl"),
@@ -231,4 +228,4 @@ def chekked(e, el):
231228
update_gimbals(0, 3)
232229

233230
while True:
234-
env.step(0.05)
231+
env.step(0)

roboticstoolbox/models/URDF/YuMi.py

+5-11
Original file line numberDiff line numberDiff line change
@@ -32,23 +32,17 @@ class YuMi(ERobot):
3232

3333
def __init__(self):
3434

35-
links, name = self.URDF_read(
36-
"yumi_description/urdf/yumi.urdf")
35+
links, name = self.URDF_read("yumi_description/urdf/yumi.urdf")
3736

3837
super().__init__(
39-
links,
40-
name=name,
41-
manufacturer='ABB',
42-
gripper_links=[links[20]]
38+
links, name=name, manufacturer="ABB", gripper_links=[links[20]]
4339
)
4440

45-
# self.addconfiguration(
46-
# "qz", np.zeros((14,)))
47-
# self.addconfiguration(
48-
# "qr", np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]))
41+
self.addconfiguration("qz", np.zeros((14,)))
42+
self.addconfiguration("qr", np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]))
4943

5044

51-
if __name__ == '__main__': # pragma nocover
45+
if __name__ == "__main__": # pragma nocover
5246

5347
robot = YuMi()
5448
print(robot)

roboticstoolbox/models/list.py

+16-11
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
1+
from typing import Type
12
from roboticstoolbox.robot.Robot import Robot
23
from roboticstoolbox.robot.ERobot import ERobot2
34
from ansitable import ANSITable, Column
5+
46
# import importlib
57

68

@@ -32,6 +34,7 @@ def list(keywords=None, dof=None, mtype=None):
3234
"""
3335

3436
import roboticstoolbox.models as m
37+
3538
# module = importlib.import_module(
3639
# '.' + os.path.splitext(file)[0], package='bdsim.blocks')
3740

@@ -47,33 +50,33 @@ def make_table(border):
4750
Column("dynamics", colalign="<"),
4851
Column("geometry", colalign="<"),
4952
Column("keywords", headalign="^", colalign="<"),
50-
border=border
53+
border=border,
5154
)
5255

5356
if mtype is not None:
5457
categories = [mtype]
5558
else:
56-
categories = ['DH', 'URDF', 'ETS']
59+
categories = ["DH", "URDF", "ETS"]
5760
for category in categories:
5861
group = m.__dict__[category]
5962
for cls in group.__dict__.values():
6063
if isinstance(cls, type) and issubclass(cls, Robot):
6164
# we found a BaseRobot subclass, instantiate it
6265
try:
6366
robot = cls()
64-
except:
67+
except TypeError:
6568
print(f"failed to load {cls}")
6669
try:
6770
structure = robot.structure
68-
except Exception: # pragma nocover
71+
except Exception: # pragma nocover
6972
structure = ""
7073

7174
# apply filters
7275
if keywords is not None:
7376
if len(set(keywords) & set(robot.keywords)) == 0:
7477
continue
7578
if dof is not None and robot.n != dof:
76-
continue # pragma nocover
79+
continue # pragma nocover
7780

7881
dims = 0
7982

@@ -90,16 +93,18 @@ def make_table(border):
9093
robot.n,
9194
f"{dims}d",
9295
structure,
93-
'Y' if robot._hasdynamics else '',
94-
'Y' if robot._hasgeometry else '',
95-
', '.join(robot.keywords)
96+
"Y" if robot._hasdynamics else "",
97+
"Y" if robot._hasgeometry else "",
98+
", ".join(robot.keywords),
9699
)
97100

98101
table.print()
99102

103+
# make_table(border='')
104+
100105

101-
if __name__ == "__main__": # pragma nocover
106+
if __name__ == "__main__": # pragma nocover
102107
list()
103-
list(keywords=('dynamics',))
108+
list(keywords=("dynamics",))
104109
list(dof=6)
105-
list(keywords=('dynamics',), dof=6)
110+
list(keywords=("dynamics",), dof=6)

0 commit comments

Comments
 (0)