Skip to content

Commit b4ddf79

Browse files
jhavljhavl
committed
fix triple angles script
1 parent 175fe6c commit b4ddf79

File tree

2 files changed

+130
-125
lines changed

2 files changed

+130
-125
lines changed

‎roboticstoolbox/examples/tripleangledemo.py

Lines changed: 129 additions & 124 deletions
Original file line numberDiff line numberDiff line change
@@ -11,54 +11,40 @@
1111
from spatialmath import SO3, SE3
1212
import numpy as np
1313

14-
15-
# TODO
16-
# rotate the rings according to the rotation axis, so that the axles
17-
# point the right way
18-
19-
# Launch the simulator Swift
20-
env = swift.Swift()
21-
env.launch()
22-
2314
path = rtb.rtb_path_to_datafile("data")
2415

16+
# rotation angle sequence
17+
sequence = "ZYX"
18+
19+
# compute the three rotation matrices
20+
BASE = SE3(0, 0, 0.5)
21+
R1 = SO3()
22+
R2 = SO3()
23+
R3 = SO3()
2524

2625
g1 = Mesh(
27-
filename=str(path / "gimbal-ring1.stl"), color=[34, 143, 201], scale=(1.0 / 3,) * 3
26+
filename=str(path / "gimbal-ring1.stl"),
27+
color=[34, 143, 201],
28+
scale=(1.0 / 3,) * 3,
2829
)
2930

3031
g2 = Mesh(
31-
filename=str(path / "gimbal-ring2.stl"), color=[31, 184, 72], scale=(1.1 / 3,) * 3
32+
filename=str(path / "gimbal-ring2.stl"),
33+
color=[31, 184, 72],
34+
scale=(1.1 / 3,) * 3,
3235
)
3336

3437
g3 = Mesh(
3538
filename=str(path / "gimbal-ring3.stl"),
3639
color=[240, 103, 103],
37-
scale=(1.1**2 / 3,) * 3,
40+
scale=(1.1 ** 2 / 3,) * 3,
3841
)
3942

4043
plane = Mesh(
4144
filename=str(path / "spitfire_assy-gear_up.stl"),
4245
scale=(1.0 / (180 * 3),) * 3,
4346
color=[240, 103, 103],
4447
)
45-
print(path / "spitfire_assy-gear_up.stl")
46-
env.add(g1)
47-
env.add(g2)
48-
env.add(g3)
49-
env.add(plane)
50-
51-
print("Supermarine Spitfire Mk VIII by Ed Morley @GRABCAD")
52-
print("Gimbal models by Peter Corke using OpenSCAD")
53-
54-
# compute the three rotation matrices
55-
BASE = SE3(0, 0, 0.5)
56-
R1 = SO3()
57-
R2 = SO3()
58-
R3 = SO3()
59-
60-
# rotation angle sequence
61-
sequence = "ZYX"
6248

6349

6450
def update_gimbals(theta, ring):
@@ -92,140 +78,159 @@ def convert(R):
9278
plane.T = convert(R1 * R2 * R3 * SO3.Ry(pi / 2) * SO3.Rz(pi / 2))
9379

9480

95-
# slider call backs, invoke the central handler
96-
def set_one(x):
97-
update_gimbals(float(x), 1)
98-
99-
100-
def set_two(x):
101-
update_gimbals(float(x), 2)
102-
103-
104-
def set_three(x):
105-
update_gimbals(float(x), 3)
81+
def demo():
82+
# TODO
83+
# rotate the rings according to the rotation axis, so that the axles
84+
# point the right way
10685

86+
# Launch the simulator Swift
87+
env = swift.Swift()
88+
env.launch()
10789

108-
r_one = swift.Slider(
109-
set_one, min=-180, max=180, step=1, value=0, desc="Outer gimbal", unit="°"
110-
)
90+
print(path / "spitfire_assy-gear_up.stl")
91+
env.add(g1)
92+
env.add(g2)
93+
env.add(g3)
94+
env.add(plane)
11195

96+
print("Supermarine Spitfire Mk VIII by Ed Morley @GRABCAD")
97+
print("Gimbal models by Peter Corke using OpenSCAD")
11298

113-
r_two = swift.Slider(
114-
set_two, min=-180, max=180, step=1, value=0, desc="Middle gimbal", unit="°"
115-
)
99+
# slider call backs, invoke the central handler
100+
def set_one(x):
101+
update_gimbals(float(x), 1)
116102

103+
def set_two(x):
104+
update_gimbals(float(x), 2)
117105

118-
r_three = swift.Slider(
119-
set_three, min=-180, max=180, step=1, value=0, desc="Inner gimbal", unit="°"
120-
)
106+
def set_three(x):
107+
update_gimbals(float(x), 3)
121108

109+
r_one = swift.Slider(
110+
set_one, min=-180, max=180, step=1, value=0, desc="Outer gimbal", unit="°"
111+
)
122112

123-
# buttons to set a 3-angle sequence
124-
ZYX_button = swift.Button(
125-
lambda x: change_sequence("ZYX"), desc="ZYX (roll-pitch-yaw angles)"
126-
)
113+
r_two = swift.Slider(
114+
set_two, min=-180, max=180, step=1, value=0, desc="Middle gimbal", unit="°"
115+
)
127116

128-
XYZ_button = swift.Button(
129-
lambda x: change_sequence("XYZ"), desc="XYZ (roll-pitch-yaw angles)"
130-
)
117+
r_three = swift.Slider(
118+
set_three,
119+
min=-180,
120+
max=180,
121+
step=1,
122+
value=0,
123+
desc="Inner gimbal",
124+
unit="°",
125+
)
131126

132-
ZYZ_button = swift.Button(lambda x: change_sequence("ZYZ"), desc="ZYZ (Euler angles)")
127+
# buttons to set a 3-angle sequence
128+
ZYX_button = swift.Button(
129+
lambda x: change_sequence("ZYX"), desc="ZYX (roll-pitch-yaw angles)"
130+
)
133131

134-
button = swift.Button(lambda x: set("ZYX"), desc="Set to Zero")
132+
XYZ_button = swift.Button(
133+
lambda x: change_sequence("XYZ"), desc="XYZ (roll-pitch-yaw angles)"
134+
)
135135

136+
ZYZ_button = swift.Button(
137+
lambda x: change_sequence("ZYZ"), desc="ZYZ (Euler angles)"
138+
)
136139

137-
# button to reset joint angles
138-
def reset(e):
139-
r_one.value = 0
140-
r_two.value = 0
141-
r_three.value = 0
142-
# env.step(0)
140+
button = swift.Button(lambda x: set("ZYX"), desc="Set to Zero")
143141

142+
# button to reset joint angles
143+
def reset(e):
144+
r_one.value = 0
145+
r_two.value = 0
146+
r_three.value = 0
147+
# env.step(0)
144148

145-
zero_button = swift.Button(reset, desc="Set to Zero")
146-
147-
148-
def update_all_sliders():
149-
update_gimbals(float(r_one.value), 1)
150-
update_gimbals(float(r_two.value), 2)
151-
update_gimbals(float(r_three.value), 3)
152-
149+
zero_button = swift.Button(reset, desc="Set to Zero")
153150

154-
def change_sequence(new):
155-
global sequence
151+
def update_all_sliders():
152+
update_gimbals(float(r_one.value), 1)
153+
update_gimbals(float(r_two.value), 2)
154+
update_gimbals(float(r_three.value), 3)
156155

157-
xyz = "XYZ"
156+
def change_sequence(new):
157+
global sequence
158158

159-
# update the state of the ring_axis dropdowns
160-
ring1_axis.checked = xyz.find(new[0])
161-
ring2_axis.checked = xyz.find(new[1])
162-
ring3_axis.checked = xyz.find(new[2])
159+
xyz = "XYZ"
163160

164-
sequence = new
165-
update_all_sliders()
161+
# update the state of the ring_axis dropdowns
162+
ring1_axis.checked = xyz.find(new[0])
163+
ring2_axis.checked = xyz.find(new[1])
164+
ring3_axis.checked = xyz.find(new[2])
166165

166+
sequence = new
167+
update_all_sliders()
167168

168-
# handle radio button on angle slider
169-
def angle(index, ring):
170-
global sequence
169+
# handle radio button on angle slider
170+
def angle(index, ring):
171+
global sequence
171172

172-
# print('angle', index, ring)
173-
xyz = "XYZ"
174-
s = list(sequence)
175-
s[ring] = xyz[int(index)]
176-
sequence = "".join(s)
177-
update_all_sliders()
173+
# print('angle', index, ring)
174+
xyz = "XYZ"
175+
s = list(sequence)
176+
s[ring] = xyz[int(index)]
177+
sequence = "".join(s)
178+
update_all_sliders()
178179

180+
ring1_axis = swift.Radio(lambda x: angle(x, 0), options=["X", "Y", "Z"], checked=2)
179181

180-
ring1_axis = swift.Radio(lambda x: angle(x, 0), options=["X", "Y", "Z"], checked=2)
182+
ring2_axis = swift.Radio(lambda x: angle(x, 1), options=["X", "Y", "Z"], checked=1)
181183

182-
ring2_axis = swift.Radio(lambda x: angle(x, 1), options=["X", "Y", "Z"], checked=1)
184+
ring3_axis = swift.Radio(lambda x: angle(x, 2), options=["X", "Y", "Z"], checked=0)
183185

184-
ring3_axis = swift.Radio(lambda x: angle(x, 2), options=["X", "Y", "Z"], checked=0)
186+
label = swift.Label(desc="Triple angle")
185187

188+
def chekked(e, el):
189+
nlabel = "s: "
186190

187-
label = swift.Label(desc="Triple angle")
191+
if e[0]:
192+
nlabel += "a"
193+
r_one.value = 0
188194

195+
if e[1]:
196+
nlabel += "b"
197+
r_two.value = 0
189198

190-
def chekked(e, el):
191-
nlabel = "s: "
199+
if e[2]:
200+
nlabel += "c"
201+
r_three.value = 0
192202

193-
if e[0]:
194-
nlabel += "a"
195-
r_one.value = 0
203+
if e[3]:
204+
el.value = 1
196205

197-
if e[1]:
198-
nlabel += "b"
199-
r_two.value = 0
206+
label.desc = nlabel
200207

201-
if e[2]:
202-
nlabel += "c"
203-
r_three.value = 0
208+
env.add(label)
209+
env.add(r_one)
210+
env.add(ring1_axis)
204211

205-
if e[3]:
206-
el.value = 1
212+
env.add(r_two)
213+
env.add(ring2_axis)
207214

208-
label.desc = nlabel
215+
env.add(r_three)
216+
env.add(ring3_axis)
209217

218+
env.add(ZYX_button)
219+
env.add(XYZ_button)
220+
env.add(ZYZ_button)
221+
env.add(zero_button)
210222

211-
env.add(label)
212-
env.add(r_one)
213-
env.add(ring1_axis)
223+
update_gimbals(0, 1)
224+
update_gimbals(0, 2)
225+
update_gimbals(0, 3)
214226

215-
env.add(r_two)
216-
env.add(ring2_axis)
227+
while True:
228+
env.step(0)
217229

218-
env.add(r_three)
219-
env.add(ring3_axis)
220230

221-
env.add(ZYX_button)
222-
env.add(XYZ_button)
223-
env.add(ZYZ_button)
224-
env.add(zero_button)
231+
def main():
232+
demo()
225233

226-
update_gimbals(0, 1)
227-
update_gimbals(0, 2)
228-
update_gimbals(0, 3)
229234

230-
while True:
231-
env.step(0)
235+
if __name__ == "__main__":
236+
main()

‎setup.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,7 @@ def package_files(directory):
136136
entry_points={
137137
"console_scripts": [
138138
"eigdemo=roboticstoolbox.examples.eigdemo:main",
139-
"tripleangledemo=roboticstoolbox.examples.tripleangledemo",
139+
"tripleangledemo=roboticstoolbox.examples.tripleangledemo:main",
140140
"twistdemo=roboticstoolbox.examples.twistdemo:main",
141141
]
142142
},

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