roboticstoolbox-python 1.4.0__py3-none-any.whl

roboticstoolbox/demo/eigdemo.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+"""
+Eigenvalue demonstration
+
+The revolving red hand is the input vector and the blue hand is the linearly
+transformed vector.
+
+Four times every revolution the two hands are parallel (or anti-parallel),
+twice to each eigenvector of the matrix A.  The ratio of lengths, blue hand
+over red hand, is the corresponding eigenvalue.  The eigenvalue will be
+negative if the hands are anti-parallel.
+
+"""
+
+import sys
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+from math import pi, sin, cos
+
+
+def eigdemo(A):
+
+    print("matrix A =")
+    print(A)
+    e, x = np.linalg.eig(A)
+
+    print(f"λ1 = {e[0]:.3f}, x1 = {np.real(x[:, 0].flatten())}")
+    print(f"λ2 = {e[1]:.3f}, x2 = {np.real(x[:, 1].flatten())}")
+
+    s = np.max(np.abs(e))
+
+    fig, ax = plt.subplots()
+    plt.axis([-s, s, -s, s])
+    plt.grid(True)
+    plt.title("Eigenvector demonstration")
+    plt.xlabel("x")
+    plt.ylabel("y")
+    plt.axis("equal")
+    plt.xlim(-s, s)
+    plt.ylim(-s, s)
+    ax.set_aspect("equal")
+
+    (l1,) = plt.plot([0, 0], [0, 0], color="r", linewidth=3)  # input vector
+    (l2,) = plt.plot([0, 0], [0, 0], color="b", linewidth=3)  # transformed vector
+
+    plt.plot([0, x[0, 0]], [0, x[1, 0]], color="k", linewidth=1)
+    plt.plot([0, x[0, 1]], [0, x[1, 1]], color="k", linewidth=1)
+
+    plt.legend(["$x$", r"${\bf A} x$", r"$x_1$", r"$x_2$"])
+
+    print("\nto exit: type q, or close the window")
+
+    def animate(theta):
+
+        x = np.r_[cos(-theta), sin(-theta)]
+        y = A @ x
+
+        l1.set_xdata([0, x[0]])
+        l1.set_ydata([0, x[1]])
+
+        l2.set_xdata([0, y[0]])
+        l2.set_ydata([0, y[1]])
+
+        return l1, l2
+
+    myAnimation = animation.FuncAnimation(
+        fig,
+        animate,
+        frames=np.linspace(0, 2 * pi, 400),
+        blit=True,
+        interval=20,
+        repeat=True,
+    )
+
+    plt.show(block=True)
+
+
+def main():
+    # eigdemo() prints λ1/λ2 -- on Windows, stdout defaults to the legacy
+    # cp1252 console codepage (which can't encode λ) whenever it isn't a
+    # real console, e.g. piped/redirected output. reconfigure() (3.7+)
+    # forces UTF-8 regardless.
+    if hasattr(sys.stdout, "reconfigure"):
+        sys.stdout.reconfigure(encoding="utf-8")
+
+    def help():
+        print("eigdemo          # uses default matrix [1 2; 3 4]")
+        print("eigdemo a b c d  # uses matrix [a b; c d]")
+        sys.exit(0)
+
+    if any(arg in ("-h", "--help", "help") for arg in sys.argv[1:]):
+        help()
+
+    if len(sys.argv) == 5:
+        try:
+            vec = [float(a) for a in sys.argv[1:5]]
+            A = np.reshape(vec, (2, 2))
+        except:
+            help()
+
+    elif len(sys.argv) == 1:
+        A = np.array([[1, 2], [3, 3]])
+
+    else:
+        help()
+
+    eigdemo(A)
+
+
+if __name__ == "__main__":
+    main()

roboticstoolbox/demo/tripleangledemo.py

@@ -0,0 +1,239 @@
+#!/usr/bin/env python
+"""
+@author Peter Corke
+@author Jesse Haviland
+"""
+
+import sys
+import swift
+from math import pi
+import roboticstoolbox as rtb
+from spatialgeometry import Mesh
+from spatialmath import SO3, SE3
+import numpy as np
+
+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,
+)
+
+g2 = Mesh(
+    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,
+)
+
+plane = Mesh(
+    filename=str(path / "spitfire_assy-gear_up.stl"),
+    scale=(1.0 / (180 * 3),) * 3,
+    color=[240, 103, 103],
+)
+
+
+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 BASE * SE3.Rt(R, t=[0, 0, 0])
+
+    g3.T = convert(R1 * SO3.Ry(pi / 2))
+    g2.T = convert(R1 * R2 * SO3.Rz(pi / 2))
+    g1.T = convert(R1 * R2 * R3 * SO3.Rx(pi / 2))
+    plane.T = convert(R1 * R2 * R3 * SO3.Ry(pi / 2) * SO3.Rz(pi / 2))
+
+
+def demo(test: bool = False):
+    # 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()
+
+    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")
+
+    # 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)
+
+    r_one = swift.Slider(
+        set_one, min=-180, max=180, step=1, value=0, label="Outer gimbal", unit="°"
+    )
+
+    r_two = swift.Slider(
+        set_two, min=-180, max=180, step=1, value=0, label="Middle gimbal", unit="°"
+    )
+
+    r_three = swift.Slider(
+        set_three,
+        min=-180,
+        max=180,
+        step=1,
+        value=0,
+        label="Inner gimbal",
+        unit="°",
+    )
+
+    # buttons to set a 3-angle sequence
+    ZYX_button = swift.Button(
+        lambda x: change_sequence("ZYX"), label="ZYX (roll-pitch-yaw angles)"
+    )
+
+    XYZ_button = swift.Button(
+        lambda x: change_sequence("XYZ"), label="XYZ (roll-pitch-yaw angles)"
+    )
+
+    ZYZ_button = swift.Button(
+        lambda x: change_sequence("ZYZ"), label="ZYZ (Euler angles)"
+    )
+
+    swift.Button(lambda x: set("ZYX"), label="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, label="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)
+
+    def change_sequence(new):
+        global sequence
+
+        xyz = "XYZ"
+
+        # 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
+
+        # 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)
+
+    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)
+
+    label = swift.Label(label="Triple angle")
+
+    # def chekked(e, el):
+    #     nlabel = "s: "
+
+    #     if e[0]:
+    #         nlabel += "a"
+    #         r_one.value = 0
+
+    #     if e[1]:
+    #         nlabel += "b"
+    #         r_two.value = 0
+
+    #     if e[2]:
+    #         nlabel += "c"
+    #         r_three.value = 0
+
+    #     if e[3]:
+    #         el.value = 1
+
+    #     label.label = nlabel
+
+    env.add(label)
+    env.add(r_one)
+    env.add(ring1_axis)
+
+    env.add(r_two)
+    env.add(ring2_axis)
+
+    env.add(r_three)
+    env.add(ring3_axis)
+
+    env.add(ZYX_button)
+    env.add(XYZ_button)
+    env.add(ZYZ_button)
+    env.add(zero_button)
+
+    update_gimbals(0, 1)
+    update_gimbals(0, 2)
+    update_gimbals(0, 3)
+
+    # duration=None runs until the browser disconnects (or forever, ^C to
+    # stop); a bounded duration lets --test smoke-test this demo without
+    # needing a browser at all (see SWIFT_HEADLESS in swift's launch()).
+    env.run(duration=0.5 if test else None)
+
+
+def main():
+    demo(test="--test" in sys.argv[1:])
+
+
+if __name__ == "__main__":
+    main()

roboticstoolbox/demo/twistdemo.py

@@ -0,0 +1,175 @@
+#!/usr/bin/env python
+"""
+@author Peter Corke
+@author Jesse Haviland
+"""
+
+import sys
+import swift
+from math import pi
+import roboticstoolbox as rtb
+from spatialgeometry import Mesh, Cylinder
+from spatialmath import SO3, SE3, Twist3
+import numpy as np
+
+
+def demo(test: bool = False):
+    # 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")
+
+    plane = Mesh(
+        filename=str(path / "spitfire_assy-gear_up.stl"),
+        scale=(1.0 / (180 * 3),) * 3,
+        color=[240, 103, 103],
+    )
+    print("Supermarine Spitfire Mk VIII by Ed Morley @GRABCAD")
+
+    screw = Cylinder(radius=0.015, length=4, color="blue", collision=False)
+
+    BASE = SE3(0.15, 0, 0.1) * SE3.Rz(pi / 2) * SE3.Rx(np.pi / 2)
+
+    env.add(plane)
+    env.add(screw)
+
+    twist_params = {"x": 0, "y": 0, "R": 0, "P": 0, "p": 0.1}
+    twist = Twist3()
+
+    def update_twist_param(value, key):
+        nonlocal twist_params
+
+        twist_params[key] = value
+
+        update_twist()
+
+    def update_twist():
+        nonlocal twist
+
+        axis = SO3.Rx(twist_params["R"]) @ SO3.Ry(twist_params["P"])
+        point = [twist_params["x"], twist_params["y"], 0]
+        twist = Twist3.UnitRevolute(axis.A[:, 2], point, twist_params["p"])
+
+        update_screw_axis(axis, point)
+        update_plane()
+
+    # button to reset twist
+    def reset_twist(e):
+        nonlocal twist_params
+
+        twist_params["x"] = 0
+        twist_params["y"] = 0
+        twist_params["R"] = 0
+        twist_params["P"] = 0
+        twist_params["p"] = 0
+        update_all_sliders()
+
+        update_twist()
+        update_plane()
+        # env.step(0)
+
+    def update_plane(*args):
+
+        T = twist.SE3(twist_theta.value) * BASE
+        plane.T = T
+
+    def update_screw_axis(axis, point):
+        screw.T = SE3.Rt(axis, point)
+
+    def update_all_sliders():
+        twist_x.value = twist_params["x"]
+        twist_y.value = twist_params["y"]
+        twist_rollangle.value = twist_params["R"]
+        twist_pitchangle.value = twist_params["P"]
+        twist_pitch.value = twist_params["p"]
+
+    twist_x = swift.Slider(
+        lambda x: update_twist_param(x, "x"),
+        min=-1,
+        max=1,
+        step=0.01,
+        value=0,
+        label="x position",
+    )
+
+    twist_y = swift.Slider(
+        lambda y: update_twist_param(y, "y"),
+        min=-1,
+        max=1,
+        step=0.01,
+        value=0,
+        label="y position",
+    )
+
+    twist_rollangle = swift.Slider(
+        lambda r: update_twist_param(np.deg2rad(r), "R"),
+        min=-180,
+        max=180,
+        step=1,
+        value=0,
+        label="roll angle (rx)",
+        unit="°",
+    )
+
+    twist_pitchangle = swift.Slider(
+        lambda p: update_twist_param(np.deg2rad(p), "P"),
+        min=-180,
+        max=180,
+        step=1,
+        value=0,
+        label="pitch angle (ry)",
+        unit="°",
+    )
+
+    twist_pitch = swift.Slider(
+        lambda p: update_twist_param(p, "p"),
+        min=0,
+        max=1,
+        step=0.02,
+        value=0,
+        label="screw pitch",
+    )
+
+    twist_theta = swift.Slider(
+        update_plane, min=0, max=10, step=0.02, value=0, label="Twist rotation"
+    )
+
+    button = swift.Button(reset_twist, label="Set to Zero")
+    quit = swift.Button(lambda x: sys.exit(0), label="Quit")
+
+    label1 = swift.Label(label="Twist parameters")
+    label2 = swift.Label(label="Twist application")
+
+    env.add(label1)
+    env.add(twist_x)
+    env.add(twist_y)
+    env.add(twist_rollangle)
+    env.add(twist_pitchangle)
+    env.add(twist_pitch)
+    env.add(button)
+
+    env.add(label2)
+
+    env.add(twist_theta)
+
+    env.add(quit)
+
+    update_all_sliders()
+
+    # duration=None runs until the browser disconnects (or forever, ^C to
+    # stop); a bounded duration lets --test smoke-test this demo without
+    # needing a browser at all (see SWIFT_HEADLESS in swift's launch()).
+    env.run(duration=0.5 if test else None)
+
+
+def main():
+    demo(test="--test" in sys.argv[1:])
+
+
+if __name__ == "__main__":
+    main()