33# @author Jesse Haviland
44# """
55
6+ from math import pi
67import roboticstoolbox as rtb
7- import spatialmath as sm
8+ from spatialmath import SO3 , SE3
89import numpy as np
910import pathlib
1011import os
1718
1819path = pathlib .Path (path ) / 'roboticstoolbox' / 'data'
1920
21+
2022g1 = rtb .Mesh (
2123 filename = str (path / 'gimbal-ring1.stl' ),
22- base = sm . SE3 (0 , 0 , 1.3 ),
24+ base = SE3 (0 , 0 , 1.3 ),
2325 color = [34 , 143 , 201 ]
2426)
2527# g1.v = [0, 0, 0, 0.4, 0, 0]
2628
2729g2 = rtb .Mesh (
2830 filename = str (path / 'gimbal-ring2.stl' ),
29- base = sm . SE3 (0 , 0 , 1.3 ),
31+ base = SE3 (0 , 0 , 1.3 ),
3032 color = [31 , 184 , 72 ]
3133)
3234# g2.v = [0, 0, 0, 0.4, 0.0, 0]
3335
3436g3 = rtb .Mesh (
3537 filename = str (path / 'gimbal-ring3.stl' ),
36- base = sm . SE3 (0 , 0 , 1.3 ),
38+ base = SE3 (0 , 0 , 1.3 ),
3739 color = [240 , 103 , 103 ]
3840)
3941# g3.v = [0, 0, 0, 0.4, 0, 0]
4042
43+ plane = rtb .Mesh (
44+ filename = str (path / 'spitfire_assy-gear_up.stl' ),
45+ base = SE3 (0 , 0 , 1.3 ),
46+ scale = (1. / 180 ,) * 3 ,
47+ color = [240 , 103 , 103 ]
48+ )
49+
4150env .add (g1 )
4251env .add (g2 )
4352env .add (g3 )
53+ env .add (plane )
54+
55+ print ('Supermarine Spitfire Mk VIII by Ed Morley @GRABCAD' )
56+ print ('Gimbal models by Peter Corke using OpenSCAD' )
57+
58+ # compute the three rotation matrices
59+
60+ R1 = SO3 ()
61+ R2 = SO3 ()
62+ R3 = SO3 ()
63+
64+ # rotation angle sequence
65+ sequence = "zyx"
4466
67+ def update_gimbals (theta , ring ):
68+ global R1 , R2 , R3
4569
70+ # update the relevant transform, depending on which ring's slider moved
71+
72+ def Rxyz (theta , which ):
73+ theta = np .radians (theta )
74+ if which == 'x' :
75+ return SO3 .Rx (theta )
76+ elif which == 'y' :
77+ return SO3 .Ry (theta )
78+ elif which == 'z' :
79+ return SO3 .Rz (theta )
80+
81+ if ring == 1 :
82+ R1 = Rxyz (theta , sequence [ring - 1 ])
83+ elif ring == 2 :
84+ R2 = Rxyz (theta , sequence [ring - 1 ])
85+ elif ring == 3 :
86+ R3 = Rxyz (theta , sequence [ring - 1 ])
87+
88+ # figure the transforms for each gimbal and the plane, and update their
89+ # pose
90+
91+ def convert (R ):
92+ return SE3 .SO3 (R )
93+
94+ g3 .base = convert (R1 * SO3 .Ry (pi / 2 ))
95+ g2 .base = convert (R1 * R2 * SO3 .Rz (pi / 2 ))
96+ g1 .base = convert (R1 * R2 * R3 * SO3 .Rx (pi / 2 ))
97+ plane .base = convert (R1 * R2 * R3 * SO3 .Ry (pi / 2 ) * SO3 .Rz (pi / 2 ))
98+
99+ # slider call backs, invoke the central handler
46100def set_one (x ):
47- g1 . base = sm . SE3 ( 0 , 0 , 1.3 ) * sm . SE3 . Rz ( np . deg2rad ( float (x )) )
101+ update_gimbals ( float (x ), 1 )
48102
49103def set_two (x ):
50- g2 . base = sm . SE3 ( 0 , 0 , 1.3 ) * sm . SE3 . Ry ( np . deg2rad ( float (x )) )
104+ update_gimbals ( float (x ), 2 )
51105
52106def set_three (x ):
53- g3 .base = sm .SE3 (0 , 0 , 1.3 ) * sm .SE3 .Rx (np .deg2rad (float (x )))
54- # g2.base = sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.pi/2) * sm.SE3.Ry(-np.deg2rad(float(x)))
55- # g2.base = g3.base
107+ update_gimbals (float (x ), 3 )
56108
57109
58110env .add_slider (
@@ -73,6 +125,10 @@ def set_three(x):
73125 step = 1 , value = 0 ,
74126 desc = 'Ring Three' , unit = '°' )
75127
128+ update_gimbals (0 , 1 )
129+ update_gimbals (0 , 2 )
130+ update_gimbals (0 , 3 )
131+
76132while (True ):
77133 env .process_events ()
78134 env .step (0 )
0 commit comments