import numpy as np
from roboticstoolbox import DHRobot ,RevoluteMDH
from spatialmath import SE3
import matplotlib.pyplot as plt 

class Arm(DHRobot):
    def __init__(self):
        self.dhparam = [
            RevoluteMDH( a = 0, alpha = 0, d = 0.2405, offset = 0 ),
            RevoluteMDH( a = 0, alpha = np.deg2rad(90) , d = 0, offset = np.deg2rad(90) ),
            RevoluteMDH( a = 0.256, alpha = 0 , d = 0, offset = np.deg2rad(90) ),
            RevoluteMDH( a = 0, alpha = np.deg2rad( 90 ), d = 0.210, offset = 0 ),
            RevoluteMDH( a = 0, alpha = np.deg2rad( -90 ), d = 0, offset = 0 ),
            RevoluteMDH( a = 0, alpha = np.deg2rad( 90 ), d = 0.144, offset = 0 ),
        ]
        name = "RM65-B"
        super().__init__(self.dhparam, name = name)

def test0():
    arm = Arm()

    # 创建两个独立的 figure
    fig1 = plt.figure(1)  # 或者 plt.figure(figsize=(8,6))
    fig2 = plt.figure(2)

    # 正确传入 Figure 对象
    arm.plot(np.deg2rad([100, 46, 70, 20, 85, 65]), block=False, fig=fig1)
    arm.plot(np.deg2rad([-80, -46, -70, 20, -85, -115]), block=False, fig=fig2)

    # 可选：设置窗口标题
    fig1.canvas.manager.set_window_title("RM65-B Pose 1")
    fig2.canvas.manager.set_window_title("RM65-B Pose 2")

    plt.show(block = True)