from Robotic_Arm.rm_robot_interface import *
import numpy

def getPose(handle)->tuple[int,rm_current_arm_state_t]:
        state = rm_current_arm_state_t()
        ret = rm_get_current_arm_state(handle, byref(state))
        return ret,state
# 运行简化版
if __name__ == "__main__":
    rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
    handle = rmarm.rm_create_robot_arm("127.0.0.1", 8800)
    rmarm.rm_set_arm_run_mode(0)
    rmarm.rm_movej([15,90,10,80,110,0], 5, 0, 0, 1)
    ret,pose = getPose(handle)
    init_pose = numpy.array([
            pose.pose.position.x, 
            pose.pose.position.y, 
            pose.pose.position.z,
            pose.qu
        ])
    init_pose[0]+=0.02
    print(init_pose)
    ret = rmarm.rm_movel(init_pose.tolist(),5,50,0,1)
    print(ret)