66import numpy as np
77import roboticstoolbox as rp
88from 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 ]
0 commit comments