Add RPY motion to the gimbals

petercorke · petercorke · commit 62799c2ffc25 · 2021-02-21T15:49:12.000+10:00
diff --git a/examples/gimbal.py b/examples/gimbal.py
@@ -3,8 +3,9 @@
 # @author Jesse Haviland
 # """
 
+from math import pi
 import roboticstoolbox as rtb
-import spatialmath as sm
+from spatialmath import SO3, SE3
 import numpy as np
 import pathlib
 import os
@@ -17,42 +18,93 @@
 
 path = pathlib.Path(path) / 'roboticstoolbox' / 'data'
 
+
 g1 = rtb.Mesh(
     filename=str(path / 'gimbal-ring1.stl'),
-    base=sm.SE3(0, 0, 1.3),
+    base=SE3(0, 0, 1.3),
     color=[34, 143, 201]
 )
 # g1.v = [0, 0, 0, 0.4, 0, 0]
 
 g2 = rtb.Mesh(
     filename=str(path / 'gimbal-ring2.stl'),
-    base=sm.SE3(0, 0, 1.3),
+    base=SE3(0, 0, 1.3),
     color=[31, 184, 72]
 )
 # g2.v = [0, 0, 0, 0.4, 0.0, 0]
 
 g3 = rtb.Mesh(
     filename=str(path / 'gimbal-ring3.stl'),
-    base=sm.SE3(0, 0, 1.3),
+    base=SE3(0, 0, 1.3),
     color=[240, 103, 103]
 )
 # g3.v = [0, 0, 0, 0.4, 0, 0]
 
+plane = rtb.Mesh(
+    filename=str(path / 'spitfire_assy-gear_up.stl'),
+    base=SE3(0, 0, 1.3),
+    scale=(1./180,) * 3,
+    color=[240, 103, 103]
+)
+
 env.add(g1)
 env.add(g2)
 env.add(g3)
+env.add(plane)
+
+print('Supermarine Spitfire Mk VIII by Ed Morley @GRABCAD')
+print('Gimbal models by Peter Corke using OpenSCAD')
+
+# compute the three rotation matrices
+
+R1 = SO3()
+R2 = SO3()
+R3 = SO3()
+
+# rotation angle sequence
+sequence = "zyx"
 
+def update_gimbals(theta, ring):
+    global R1, R2, R3
 
+    # update the relevant transform, depending on which ring's slider moved
+
+    def Rxyz(theta, which):
+        theta = np.radians(theta)
+        if which == 'x':
+            return SO3.Rx(theta)
+        elif which == 'y':
+            return SO3.Ry(theta)
+        elif which == 'z':
+            return SO3.Rz(theta)
+
+    if ring == 1:
+        R1 = Rxyz(theta, sequence[ring - 1])
+    elif ring == 2:
+        R2 = Rxyz(theta, sequence[ring - 1])
+    elif ring == 3:
+        R3 = Rxyz(theta, sequence[ring - 1])
+
+    # figure the transforms for each gimbal and the plane, and update their
+    # pose
+
+    def convert(R):
+        return SE3.SO3(R)
+
+    g3.base = convert(R1 * SO3.Ry(pi/2))
+    g2.base = convert(R1 * R2 * SO3.Rz(pi/2))
+    g1.base = convert(R1 * R2 * R3 * SO3.Rx(pi/2))
+    plane.base = convert(R1 * R2 * R3 * SO3.Ry(pi/2) * SO3.Rz(pi/2))
+
+# slider call backs, invoke the central handler
 def set_one(x):
-    g1.base = sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.deg2rad(float(x)))
+    update_gimbals(float(x), 1)
 
 def set_two(x):
-    g2.base = sm.SE3(0, 0, 1.3) * sm.SE3.Ry(np.deg2rad(float(x)))
+    update_gimbals(float(x), 2)
 
 def set_three(x):
-    g3.base = sm.SE3(0, 0, 1.3) * sm.SE3.Rx(np.deg2rad(float(x)))
-    # g2.base = sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.pi/2) * sm.SE3.Ry(-np.deg2rad(float(x)))
-    # g2.base = g3.base 
+    update_gimbals(float(x), 3)
 
 
 env.add_slider(
@@ -73,6 +125,10 @@ def set_three(x):
     step=1, value=0,
     desc='Ring Three', unit='&#176;')
 
+update_gimbals(0, 1)
+update_gimbals(0, 2)
+update_gimbals(0, 3)
+
 while(True):
     env.process_events()
     env.step(0)
