from roboimi.assets.robots.arm_base import *
import os



class BiDianaMedVS(ArmBase):
    def __init__(self):
        super().__init__(
            name="bidiana_med",
            urdf_path= "./assets/models/manipulators/DianaMed/DianaMed_vs.urdf",
            xml_path = "./assets/models/manipulators/DianaMed/BiDianaMed_vs.xml",
            gripper=None
        )
        self.left_arm = self.Arm(self, 'single', self.urdf_path)
        self.left_arm.set_Arm_base_link('base_link')
        self.left_arm.set_Arm_ee_link('link7')
        self.left_arm.InitKDL
        self.left_arm.joint_index = ['0_j1', '0_j2', '0_j3', '0_j4', '0_j5', '0_j6', '0_j7']
        self.left_arm.actuator_index = ['0_m1', '0_m2', '0_m3', '0_m4', '0_m5', '0_m6', '0_m7']
        self.left_arm.setArmInitPose(self.init_qpos)
        self.arms.append(self.left_arm)

        self.aruco_arm = self.Arm(self, 'single',self.urdf_path)
        self.aruco_arm.set_Arm_base_link('base_link_aruco')
        self.aruco_arm.set_Arm_ee_link('link7_aruco')
        self.aruco_arm.InitKDL
        self.aruco_arm.joint_index = ['0_aruco_j1', '0_aruco_j2', '0_aruco_j3', '0_aruco_j4', '0_aruco_j5', '0_aruco_j6', '0_aruco_j7']
        self.aruco_arm.actuator_index = ['0_aruco_m1', '0_aruco_m2', '0_aruco_m3', '0_aruco_m4', '0_aruco_m5', '0_aruco_m6', '0_aruco_m7']
        self.aruco_arm.setArmInitPose(self.init_aruco_qpos)
        self.arms.append(self.aruco_arm)
        self.jnt_num = self.left_arm.jnt_num + self.aruco_arm.jnt_num

        # joint PD
        self.kp = 500 * np.ones(self.jnt_num)
        self.kd = 44.57 * np.ones(self.jnt_num)
        print("BiDianaMed init !")

    @property
    def init_qpos(self):
        """ Robot's init joint position. """
        return np.array([0.0, 0.0, 0.0, 1.5708, 0.0, 0.0, 0.0])
    
    @property
    def init_aruco_qpos(self):
        """ Robot's init joint position. """
        return np.array([0.0, 0.0, 0.0, 1.5708, 0.0, 0.0, 0.0])
