fix triple angles script

Author: jhavl

roboticstoolbox/examples/tripleangledemo.py

@@ -11,54 +11,40 @@
 from spatialmath import SO3, SE3
 import numpy as np
 
-
-# TODO
-#  rotate the rings according to the rotation axis, so that the axles
-#  point the right way
-
-# Launch the simulator Swift
-env = swift.Swift()
-env.launch()
-
 path = rtb.rtb_path_to_datafile("data")
 
+# rotation angle sequence
+sequence = "ZYX"
+
+# compute the three rotation matrices
+BASE = SE3(0, 0, 0.5)
+R1 = SO3()
+R2 = SO3()
+R3 = SO3()
 
 g1 = Mesh(
-    filename=str(path / "gimbal-ring1.stl"), color=[34, 143, 201], scale=(1.0 / 3,) * 3
+    filename=str(path / "gimbal-ring1.stl"),
+    color=[34, 143, 201],
+    scale=(1.0 / 3,) * 3,
 )
 
 g2 = Mesh(
-    filename=str(path / "gimbal-ring2.stl"), color=[31, 184, 72], scale=(1.1 / 3,) * 3
+    filename=str(path / "gimbal-ring2.stl"),
+    color=[31, 184, 72],
+    scale=(1.1 / 3,) * 3,
 )
 
 g3 = Mesh(
     filename=str(path / "gimbal-ring3.stl"),
     color=[240, 103, 103],
-    scale=(1.1**2 / 3,) * 3,
+    scale=(1.1 ** 2 / 3,) * 3,
 )
 
 plane = Mesh(
     filename=str(path / "spitfire_assy-gear_up.stl"),
     scale=(1.0 / (180 * 3),) * 3,
     color=[240, 103, 103],
 )
-print(path / "spitfire_assy-gear_up.stl")
-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
-BASE = SE3(0, 0, 0.5)
-R1 = SO3()
-R2 = SO3()
-R3 = SO3()
-
-# rotation angle sequence
-sequence = "ZYX"
 
 
 def update_gimbals(theta, ring):
@@ -92,140 +78,159 @@ def convert(R):
     plane.T = convert(R1 * R2 * R3 * SO3.Ry(pi / 2) * SO3.Rz(pi / 2))
 
 
-# slider call backs, invoke the central handler
-def set_one(x):
-    update_gimbals(float(x), 1)
-
-
-def set_two(x):
-    update_gimbals(float(x), 2)
-
-
-def set_three(x):
-    update_gimbals(float(x), 3)
+def demo():
+    # TODO
+    #  rotate the rings according to the rotation axis, so that the axles
+    #  point the right way
 
+    # Launch the simulator Swift
+    env = swift.Swift()
+    env.launch()
 
-r_one = swift.Slider(
-    set_one, min=-180, max=180, step=1, value=0, desc="Outer gimbal", unit="°"
-)
+    print(path / "spitfire_assy-gear_up.stl")
+    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")
 
-r_two = swift.Slider(
-    set_two, min=-180, max=180, step=1, value=0, desc="Middle gimbal", unit="°"
-)
+    # slider call backs, invoke the central handler
+    def set_one(x):
+        update_gimbals(float(x), 1)
 
+    def set_two(x):
+        update_gimbals(float(x), 2)
 
-r_three = swift.Slider(
-    set_three, min=-180, max=180, step=1, value=0, desc="Inner gimbal", unit="°"
-)
+    def set_three(x):
+        update_gimbals(float(x), 3)
 
+    r_one = swift.Slider(
+        set_one, min=-180, max=180, step=1, value=0, desc="Outer gimbal", unit="°"
+    )
 
-# buttons to set a 3-angle sequence
-ZYX_button = swift.Button(
-    lambda x: change_sequence("ZYX"), desc="ZYX (roll-pitch-yaw angles)"
-)
+    r_two = swift.Slider(
+        set_two, min=-180, max=180, step=1, value=0, desc="Middle gimbal", unit="°"
+    )
 
-XYZ_button = swift.Button(
-    lambda x: change_sequence("XYZ"), desc="XYZ (roll-pitch-yaw angles)"
-)
+    r_three = swift.Slider(
+        set_three,
+        min=-180,
+        max=180,
+        step=1,
+        value=0,
+        desc="Inner gimbal",
+        unit="°",
+    )
 
-ZYZ_button = swift.Button(lambda x: change_sequence("ZYZ"), desc="ZYZ (Euler angles)")
+    # buttons to set a 3-angle sequence
+    ZYX_button = swift.Button(
+        lambda x: change_sequence("ZYX"), desc="ZYX (roll-pitch-yaw angles)"
+    )
 
-button = swift.Button(lambda x: set("ZYX"), desc="Set to Zero")
+    XYZ_button = swift.Button(
+        lambda x: change_sequence("XYZ"), desc="XYZ (roll-pitch-yaw angles)"
+    )
 
+    ZYZ_button = swift.Button(
+        lambda x: change_sequence("ZYZ"), desc="ZYZ (Euler angles)"
+    )
 
-# button to reset joint angles
-def reset(e):
-    r_one.value = 0
-    r_two.value = 0
-    r_three.value = 0
-    # env.step(0)
+    button = swift.Button(lambda x: set("ZYX"), desc="Set to Zero")
 
+    # button to reset joint angles
+    def reset(e):
+        r_one.value = 0
+        r_two.value = 0
+        r_three.value = 0
+        # env.step(0)
 
-zero_button = swift.Button(reset, desc="Set to Zero")
-
-
-def update_all_sliders():
-    update_gimbals(float(r_one.value), 1)
-    update_gimbals(float(r_two.value), 2)
-    update_gimbals(float(r_three.value), 3)
-
+    zero_button = swift.Button(reset, desc="Set to Zero")
 
-def change_sequence(new):
-    global sequence
+    def update_all_sliders():
+        update_gimbals(float(r_one.value), 1)
+        update_gimbals(float(r_two.value), 2)
+        update_gimbals(float(r_three.value), 3)
 
-    xyz = "XYZ"
+    def change_sequence(new):
+        global sequence
 
-    # update the state of the ring_axis dropdowns
-    ring1_axis.checked = xyz.find(new[0])
-    ring2_axis.checked = xyz.find(new[1])
-    ring3_axis.checked = xyz.find(new[2])
+        xyz = "XYZ"
 
-    sequence = new
-    update_all_sliders()
+        # update the state of the ring_axis dropdowns
+        ring1_axis.checked = xyz.find(new[0])
+        ring2_axis.checked = xyz.find(new[1])
+        ring3_axis.checked = xyz.find(new[2])
 
+        sequence = new
+        update_all_sliders()
 
-# handle radio button on angle slider
-def angle(index, ring):
-    global sequence
+    # handle radio button on angle slider
+    def angle(index, ring):
+        global sequence
 
-    # print('angle', index, ring)
-    xyz = "XYZ"
-    s = list(sequence)
-    s[ring] = xyz[int(index)]
-    sequence = "".join(s)
-    update_all_sliders()
+        # print('angle', index, ring)
+        xyz = "XYZ"
+        s = list(sequence)
+        s[ring] = xyz[int(index)]
+        sequence = "".join(s)
+        update_all_sliders()
 
+    ring1_axis = swift.Radio(lambda x: angle(x, 0), options=["X", "Y", "Z"], checked=2)
 
-ring1_axis = swift.Radio(lambda x: angle(x, 0), options=["X", "Y", "Z"], checked=2)
+    ring2_axis = swift.Radio(lambda x: angle(x, 1), options=["X", "Y", "Z"], checked=1)
 
-ring2_axis = swift.Radio(lambda x: angle(x, 1), options=["X", "Y", "Z"], checked=1)
+    ring3_axis = swift.Radio(lambda x: angle(x, 2), options=["X", "Y", "Z"], checked=0)
 
-ring3_axis = swift.Radio(lambda x: angle(x, 2), options=["X", "Y", "Z"], checked=0)
+    label = swift.Label(desc="Triple angle")
 
+    def chekked(e, el):
+        nlabel = "s: "
 
-label = swift.Label(desc="Triple angle")
+        if e[0]:
+            nlabel += "a"
+            r_one.value = 0
 
+        if e[1]:
+            nlabel += "b"
+            r_two.value = 0
 
-def chekked(e, el):
-    nlabel = "s: "
+        if e[2]:
+            nlabel += "c"
+            r_three.value = 0
 
-    if e[0]:
-        nlabel += "a"
-        r_one.value = 0
+        if e[3]:
+            el.value = 1
 
-    if e[1]:
-        nlabel += "b"
-        r_two.value = 0
+        label.desc = nlabel
 
-    if e[2]:
-        nlabel += "c"
-        r_three.value = 0
+    env.add(label)
+    env.add(r_one)
+    env.add(ring1_axis)
 
-    if e[3]:
-        el.value = 1
+    env.add(r_two)
+    env.add(ring2_axis)
 
-    label.desc = nlabel
+    env.add(r_three)
+    env.add(ring3_axis)
 
+    env.add(ZYX_button)
+    env.add(XYZ_button)
+    env.add(ZYZ_button)
+    env.add(zero_button)
 
-env.add(label)
-env.add(r_one)
-env.add(ring1_axis)
+    update_gimbals(0, 1)
+    update_gimbals(0, 2)
+    update_gimbals(0, 3)
 
-env.add(r_two)
-env.add(ring2_axis)
+    while True:
+        env.step(0)
 
-env.add(r_three)
-env.add(ring3_axis)
 
-env.add(ZYX_button)
-env.add(XYZ_button)
-env.add(ZYZ_button)
-env.add(zero_button)
+def main():
+    demo()
 
-update_gimbals(0, 1)
-update_gimbals(0, 2)
-update_gimbals(0, 3)
 
-while True:
-    env.step(0)
+if __name__ == "__main__":
+    main()

setup.py

@@ -136,7 +136,7 @@ def package_files(directory):
     entry_points={
         "console_scripts": [
             "eigdemo=roboticstoolbox.examples.eigdemo:main",
-            "tripleangledemo=roboticstoolbox.examples.tripleangledemo",
+            "tripleangledemo=roboticstoolbox.examples.tripleangledemo:main",
             "twistdemo=roboticstoolbox.examples.twistdemo:main",
         ]
     },