6
6
import numpy as np
7
7
import roboticstoolbox as rp
8
8
from spatialmath import SE3
9
- from spatialmath import base
10
9
11
10
12
- class RobotPlot ():
13
-
11
+ class RobotPlot :
14
12
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
+ ):
18
25
19
26
super (RobotPlot , self ).__init__ ()
20
27
@@ -55,15 +62,15 @@ def __init__(
55
62
self .showname = name
56
63
57
64
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 ,
67
74
}
68
75
69
76
if options is not None :
@@ -83,7 +90,7 @@ def draw(self):
83
90
84
91
# compute all link frames
85
92
T = self .robot .fkine_all (self .robot .q )
86
-
93
+
87
94
# draw all the line segments for the noodle plot
88
95
for i , segment in enumerate (self .segments ):
89
96
linkframes = []
@@ -95,8 +102,8 @@ def draw(self):
95
102
points = np .array ([linkframe .t for linkframe in linkframes ])
96
103
97
104
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 ])
100
107
101
108
# Update the shadow of the robot links
102
109
if self .shadow :
@@ -115,7 +122,7 @@ def draw(self):
115
122
116
123
if self .eeframe :
117
124
# Axes arrow transforms
118
- len = self .options [' eelength' ]
125
+ len = self .options [" eelength" ]
119
126
Tjx = SE3 ([len , 0 , 0 ])
120
127
Tjy = SE3 ([0 , len , 0 ])
121
128
Tjz = SE3 ([0 , 0 , len ])
@@ -129,9 +136,9 @@ def draw(self):
129
136
Tey = Te * Tjy
130
137
Tez = Te * Tjz
131
138
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" ])
135
142
136
143
self .eeframes .extend ([xaxis , yaxis , zaxis ])
137
144
@@ -159,18 +166,17 @@ def draw(self):
159
166
elif link .isjoint :
160
167
Tj = T [link .number ]
161
168
R = Tj .R
162
- if link .v .axis [1 ] == 'z' :
169
+ if link .v .axis [1 ] == "z" :
163
170
direction = R [:, 2 ] # z direction
164
- elif link .v .axis [1 ] == 'y' :
171
+ elif link .v .axis [1 ] == "y" :
165
172
direction = R [:, 1 ] # y direction
166
- elif link .v .axis [1 ] == 'x' :
173
+ elif link .v .axis [1 ] == "x" :
167
174
direction = R [:, 0 ] # direction
168
175
169
176
if direction is not None :
170
177
arrow = self ._plot_quiver2 (Tj .t , direction , link .jindex )
171
178
self .joints .extend (arrow )
172
179
173
-
174
180
def init (self ):
175
181
176
182
self .drawn = True
@@ -189,54 +195,50 @@ def init(self):
189
195
190
196
# Plot robot name
191
197
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 )
194
199
195
200
# Initialize the robot links
196
201
self .links = []
197
202
self .sh_links = []
198
203
for i in range (len (self .segments )):
199
204
200
-
201
205
# Plot the shadow of the robot links, draw first so robot is always
202
206
# in front
203
207
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" ])
208
209
self .sh_links .append (shadow )
209
210
210
- line , = self .ax .plot (
211
- [0 ], [0 ], [0 ], ** self .options ['robot' ])
211
+ (line ,) = self .ax .plot ([0 ], [0 ], [0 ], ** self .options ["robot" ])
212
212
self .links .append (line )
213
213
214
214
self .eeframes = []
215
215
self .joints = []
216
216
217
217
def _plot_quiver (self , p0 , p1 , options ):
218
218
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
224
220
)
225
221
return qv
226
222
227
223
def _plot_quiver2 (self , p0 , dir , j ):
228
- vec = dir * self .options [' jointaxislength' ]
224
+ vec = dir * self .options [" jointaxislength" ]
229
225
start = p0 - vec / 2
230
226
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 ],
233
233
zorder = 5 ,
234
- ** self .options [' jointaxes' ]
234
+ ** self .options [" jointaxes" ],
235
235
)
236
236
237
237
if self .jointlabels :
238
238
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
+ )
240
242
return [qv , label ]
241
243
else :
242
244
return [qv ]
0 commit comments