From 83d6419d70f09349cf8a5c58c45204eb2c49e360 Mon Sep 17 00:00:00 2001 From: yutang Date: Thu, 5 Jun 2025 21:56:52 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=8B=E6=9F=84=E6=8E=A7=E5=88=B6=E7=AC=AC?= =?UTF-8?q?=E4=B8=80=E6=AC=A1=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- realman.md | 2 +- realman_src/dual_arm_connect_test.py | 41 ++- realman_src/realman_demo.py | 387 ++++++++++--------------- realman_src/single_arm_control_test.py | 17 ++ 4 files changed, 201 insertions(+), 246 deletions(-) create mode 100644 realman_src/single_arm_control_test.py diff --git a/realman.md b/realman.md index be482c5f..a26076db 100644 --- a/realman.md +++ b/realman.md @@ -23,7 +23,7 @@ conda install ffmpeg=7.1.1 -c conda-forge Install Realman SDK: ```bash -pip install Robotic_Arm +pip install Robotic_Arm==1.0.4.1 pip install pygame ``` diff --git a/realman_src/dual_arm_connect_test.py b/realman_src/dual_arm_connect_test.py index 2f969773..d2bebab7 100644 --- a/realman_src/dual_arm_connect_test.py +++ b/realman_src/dual_arm_connect_test.py @@ -1,18 +1,31 @@ from Robotic_Arm.rm_robot_interface import * -robot = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) -handle = robot.rm_create_robot_arm("169.254.128.19", 8080) -print("机械臂ID：", handle.id) +armleft = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) +armright = RoboticArm() -software_info = robot.rm_get_arm_software_info() -if software_info[0] == 0: - print("\n================== Arm Software Information ==================") - print("Arm Model: ", software_info[1]['product_version']) - print("Algorithm Library Version: ", software_info[1]['algorithm_info']['version']) - print("Control Layer Software Version: ", software_info[1]['ctrl_info']['version']) - print("Dynamics Version: ", software_info[1]['dynamic_info']['model_version']) - print("Planning Layer Software Version: ", software_info[1]['plan_info']['version']) - print("==============================================================\n") -else: - print("\nFailed to get arm software information, Error code: ", software_info[0], "\n") +lefthandle = armleft.rm_create_robot_arm("169.254.128.18", 8080) +print("机械臂ID：", lefthandle.id) +righthandle = armright.rm_create_robot_arm("169.254.128.19", 8080) +print("机械臂ID：", righthandle.id) + +# software_info = armleft.rm_get_arm_software_info() +# if software_info[0] == 0: +# print("\n================== Arm Software Information ==================") +# print("Arm Model: ", software_info[1]['product_version']) +# print("Algorithm Library Version: ", software_info[1]['algorithm_info']['version']) +# print("Control Layer Software Version: ", software_info[1]['ctrl_info']['version']) +# print("Dynamics Version: ", software_info[1]['dynamic_info']['model_version']) +# print("Planning Layer Software Version: ", software_info[1]['plan_info']['version']) +# print("==============================================================\n") +# else: +# print("\nFailed to get arm software information, Error code: ", software_info[0], "\n") + +print("Left: ", armleft.rm_get_current_arm_state()) +print("Left: ", armleft.rm_get_arm_all_state()) +armleft.rm_movej_p() +# print("Right: ", armright.rm_get_current_arm_state()) + + +# 断开所有连接，销毁线程 +RoboticArm.rm_destory() \ No newline at end of file diff --git a/realman_src/realman_demo.py b/realman_src/realman_demo.py index 36116441..4afba56b 100644 --- a/realman_src/realman_demo.py +++ b/realman_src/realman_demo.py @@ -2,13 +2,13 @@ # -*-coding:utf8-*- from typing import Optional import time -from piper_sdk import * +from Robotic_Arm.rm_robot_interface import * import pygame import threading from typing import Dict -def enable_fun(piper:C_PiperInterface_V2): +def enable_fun(arm: RoboticArm): ''' 使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序 ''' @@ -18,18 +18,18 @@ def enable_fun(piper:C_PiperInterface_V2): # 记录进入循环前的时间 start_time = time.time() elapsed_time_flag = False - while not (enable_flag): + + while not enable_flag: elapsed_time = time.time() - start_time print("--------------------") - enable_flag = piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status and \ - piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status and \ - piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status and \ - piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status and \ - piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status and \ - piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status - print("使能状态:",enable_flag) - piper.EnableArm(7) - piper.GripperCtrl(0,1000,0x01, 0) + + # 获取机械臂状态 + ret = arm.rm_get_current_arm_state() + if ret[0] == 0: # 成功获取状态 + arm_state = ret[1] + enable_flag = True + + print("使能状态:", enable_flag) print("--------------------") # 检查是否超过超时时间 if elapsed_time > timeout: @@ -38,13 +38,13 @@ def enable_fun(piper:C_PiperInterface_V2): enable_flag = True break time.sleep(1) - pass - if(elapsed_time_flag): + + if elapsed_time_flag: print("程序自动使能超时,退出程序") exit(0) -class UnifiedArmController: +class EndPoseController: def __init__(self): # 初始化pygame和手柄 pygame.init() @@ -58,47 +58,32 @@ class UnifiedArmController: self.joystick = pygame.joystick.Joystick(0) self.joystick.init() - # 控制模式标志 - True为末端位姿控制，False为关节控制 - self.end_pose_mode = False - # 摇杆死区 self.deadzone = 0.15 # 精细控制模式 self.fine_control_mode = False - # 初始化关节状态 - self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节 - self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节的速度 - - # 关节弧度限制 - self.joint_limits = [ - (-92000 / 57324.840764, 92000 / 57324.840764), # joint1 - ( 0 / 57324.840764, 120000 / 57324.840764), # joint2 - (-80000 / 57324.840764, 0 / 57324.840764), # joint3 - (-90000 / 57324.840764, 90000 / 57324.840764), # joint4 - (-65000 / 57324.840764, 65000 / 57324.840764), # joint5 - (-90000 / 57324.840764, 90000 / 57324.840764) # joint6 - ] - - # 初始化末端姿态 [X, Y, Z, RX, RY, RZ] - self.pose = [0.056127, 0, 0.213266, 0, 84.999, 0] + # 初始化末端姿态 [X, Y, Z, RX, RY, RZ] XYZ meter RX RY RZ rad + self.init_joint = [10, -125, 100, 100, 0, 0, -10] + self.init_pose = [-0.185, 0.315, 0.080, -1.500, -0.800, -0.000] + self.joint = self.init_joint + self.pose = self.init_pose self.pose_speeds = [0.0] * 6 # 末端位姿限制 self.pose_limits = [ - (-0.6, 0.6), # X (m) - (-0.6, 0.6), # Y (m) - (0.05, 0.6), # Z (m) - 设置最小高度防止碰撞 - (-180, 180), # RX (deg) - (-180, 180), # RY (deg) - (-180, 180) # RZ (deg) + (-0.850, 0.850), # X (m) + (-0.850, 0.850), # Y (m) + (0.850, 0.850), # Z (m) - 设置最小高度防止碰撞 + (-3.14, 3.14), # RX (rad) + (-3.14, 3.14), # RY (rad) + (-3.14, 3.14) # RZ (rad) ] # 控制参数 self.linear_step = 0.0015 # 线性移动步长(m) - self.angular_step = 0.05 # 角度步长(deg) - self.joint_step = 0.015 # 关节步长(rad) + self.angular_step = 0.001 # 角度步长(rad) - 从度转换为弧度 # 夹爪状态和速度 self.gripper = 0.0 @@ -109,9 +94,7 @@ class UnifiedArmController: self.thread = threading.Thread(target=self.update_controller) self.thread.start() - print("机械臂统一控制器已启动") - print("按下OPTIONS(Start)切换控制模式") - print("当前模式:", "末端位姿控制" if self.end_pose_mode else "关节控制") + print("机械臂末端位姿控制器已启动") def _apply_nonlinear_mapping(self, value): """应用非线性映射以提高控制精度""" @@ -120,11 +103,12 @@ class UnifiedArmController: return sign * (abs(value) ** 2) def _normalize_angle(self, angle): - """将角度归一化到[-180, 180]范围内""" - while angle > 180: - angle -= 360 - while angle < -180: - angle += 360 + """将角度归一化到[-π, π]范围内""" + import math + while angle > math.pi: + angle -= 2 * math.pi + while angle < -math.pi: + angle += 2 * math.pi return angle def update_controller(self): @@ -136,12 +120,6 @@ class UnifiedArmController: self.stop() continue - # 检查控制模式切换 (使用左摇杆按钮) - if self.joystick.get_button(9): # 左摇杆按钮 - self.end_pose_mode = not self.end_pose_mode - print(f"切换到{'末端位姿控制' if self.end_pose_mode else '关节控制'}模式") - time.sleep(0.3) # 防止多次触发 - # 检查精细控制模式切换 (使用L3按钮) if self.joystick.get_button(10): # L3按钮 self.fine_control_mode = not self.fine_control_mode @@ -150,24 +128,20 @@ class UnifiedArmController: # 检查重置按钮 (7号按钮，通常是Start按钮) if self.joystick.get_button(7): # Start按钮 - print("重置机械臂到0位...") - # 重置关节和位姿 - self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + print("重置机械臂到初始位置...") + # 重置位姿 + self.joint = self.init_joint + self.pose = self.init_pose + self.pose_speeds = [0.0] * 6 self.gripper = 0.0 self.gripper_speed = 0.0 - # 临时切换到关节控制模式 - self.end_pose_mode = False - print("机械臂已重置到0位") + print("机械臂已重置到初始位置") time.sleep(0.3) # 防止多次触发 - # 根据控制模式获取输入并更新状态 - if self.end_pose_mode: - self.update_end_pose() - else: - self.update_joints() + # 更新末端位姿 + self.update_end_pose() - # 夹爪控制（圈/叉）- 两种模式下都保持一致 + # 夹爪控制（圈/叉） circle = self.joystick.get_button(1) # 夹爪开 cross = self.joystick.get_button(0) # 夹爪关 self.gripper_speed = 0.01 if circle else (-0.01 if cross else 0.0) @@ -179,11 +153,15 @@ class UnifiedArmController: time.sleep(0.02) def update_end_pose(self): + print("1", self.pose) """更新末端位姿控制""" # 根据控制模式调整步长 current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0) current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0) + # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}") + print(f"精细控制模式: {self.fine_control_mode}") + # 方向键控制XY hat = self.joystick.get_hat(0) hat_up = hat[1] == 1 # Y+ @@ -191,21 +169,32 @@ class UnifiedArmController: hat_left = hat[0] == -1 # X- hat_right = hat[0] == 1 # X+ + # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}") + # 右摇杆控制Z - right_y = -self.joystick.get_axis(4) # Z控制，取反使向上为正 + right_y_raw = -self.joystick.get_axis(4) + # print(f"右摇杆原始值(axis 4): {self.joystick.get_axis(4)}") + # print(f"右摇杆处理值: {right_y_raw}") # 左摇杆控制RZ - left_y = -self.joystick.get_axis(1) # RZ控制，取反使向上为正 + left_y_raw = -self.joystick.get_axis(1) + # print(f"左摇杆原始值(axis 1): {self.joystick.get_axis(1)}") + # print(f"左摇杆处理值: {left_y_raw}") # 应用死区 - right_y = 0.0 if abs(right_y) < self.deadzone else right_y - left_y = 0.0 if abs(left_y) < self.deadzone else left_y + right_y = 0.0 if abs(right_y_raw) < self.deadzone else right_y_raw + left_y = 0.0 if abs(left_y_raw) < self.deadzone else left_y_raw + + # print(f"死区处理后 - 右摇杆: {right_y}, 左摇杆: {left_y}") + # 计算各轴速度 self.pose_speeds[0] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # X self.pose_speeds[1] = current_linear_step if hat_left else (-current_linear_step if hat_right else 0.0) # Y # 设置Z速度（右摇杆Y轴控制） - self.pose_speeds[2] = self._apply_nonlinear_mapping(right_y) * current_linear_step # Z + z_mapping = self._apply_nonlinear_mapping(right_y) + # print(f"Z轴非线性映射: {right_y} -> {z_mapping}") + self.pose_speeds[2] = z_mapping * current_linear_step # Z # L1/R1控制RX旋转 LB = self.joystick.get_button(4) # RX- @@ -218,117 +207,59 @@ class UnifiedArmController: self.pose_speeds[4] = (current_angular_step if triangle else (-current_angular_step if square else 0.0)) # 左摇杆Y轴控制RZ旋转 - self.pose_speeds[5] = self._apply_nonlinear_mapping(left_y) * current_angular_step * 2 # RZ，增加系数使旋转更明显 + rz_mapping = self._apply_nonlinear_mapping(left_y) + # print(f"RZ轴非线性映射: {left_y} -> {rz_mapping}") + self.pose_speeds[5] = rz_mapping * current_angular_step * 2 # RZ + + # print(f"计算出的速度: {self.pose_speeds}") # 更新末端位姿 + old_pose = self.pose.copy() for i in range(6): self.pose[i] += self.pose_speeds[i] + # print(f"位姿更新: {old_pose} -> {self.pose}") + # 位置限制 - for i in range(3): - min_val, max_val = self.pose_limits[i] - self.pose[i] = max(min_val, min(max_val, self.pose[i])) + # pose_before_limit = self.pose.copy() + # for i in range(3): + # min_val, max_val = self.pose_limits[i] + # self.pose[i] = max(min_val, min(max_val, self.pose[i])) + + # if pose_before_limit != self.pose: + # print(f"位置限制生效: {pose_before_limit} -> {self.pose}") # 角度归一化处理 + pose_before_normalize = self.pose.copy() for i in range(3, 6): self.pose[i] = self._normalize_angle(self.pose[i]) - - def update_joints(self): - """更新关节控制""" - # 根据控制模式调整步长 - current_joint_step = self.joint_step * (0.1 if self.fine_control_mode else 1.0) - - # 使用类似于末端位姿控制的映射，但直接控制关节 - - # 左摇杆控制关节1和2 (类似于末端位姿控制中的X和Y) - left_x = -self.joystick.get_axis(0) # 左摇杆x轴 - left_y = -self.joystick.get_axis(1) # 左摇杆y轴 - - # 应用死区 - left_x = 0.0 if abs(left_x) < self.deadzone else left_x - left_y = 0.0 if abs(left_y) < self.deadzone else left_y - - # 右摇杆控制关节3和4 - right_x = self.joystick.get_axis(3) # 右摇杆x轴 - right_y = self.joystick.get_axis(4) # 右摇杆y轴 - - # 应用死区 - right_x = 0.0 if abs(right_x) < self.deadzone else right_x - right_y = 0.0 if abs(right_y) < self.deadzone else right_y - - # 方向键控制关节5和6 - hat = self.joystick.get_hat(0) - up = hat[1] == 1 - down = hat[1] == -1 - left = hat[0] == -1 - right = hat[0] == 1 - - # 映射输入到关节速度 - self.joint_speeds[0] = left_x * current_joint_step # joint1速度 - self.joint_speeds[1] = left_y * current_joint_step # joint2速度 - self.joint_speeds[2] = right_y * current_joint_step # joint3速度 - self.joint_speeds[3] = right_x * current_joint_step # joint4速度 - self.joint_speeds[4] = -current_joint_step if up else (current_joint_step if down else 0.0) # joint5速度 - self.joint_speeds[5] = current_joint_step if right else (-current_joint_step if left else 0.0) # joint6速度 - - # 积分速度到关节位置 - for i in range(6): - self.joints[i] += self.joint_speeds[i] - - # 关节范围保护 - for i in range(6): - min_val, max_val = self.joint_limits[i] - self.joints[i] = max(min_val, min(max_val, self.joints[i])) - def update_state(self, ctrl_mode, end_pose, joint_state): - if ctrl_mode == 'end_pose': - _joint_state = [0] * 6 - _joint_state[0] = joint_state.joint_1 / 57324.840764 - _joint_state[1] = joint_state.joint_2 / 57324.840764 - _joint_state[2] = joint_state.joint_3 / 57324.840764 - _joint_state[3] = joint_state.joint_4 / 57324.840764 - _joint_state[4] = joint_state.joint_5 / 57324.840764 - _joint_state[5] = joint_state.joint_6 / 57324.840764 - self.joints = _joint_state - else: - _end_pose = [0] * 6 - _end_pose[0] = end_pose.X_axis / 1000 / 1000 - _end_pose[1] = end_pose.Y_axis / 1000 / 1000 - _end_pose[2] = end_pose.Z_axis / 1000 / 1000 - _end_pose[3] = end_pose.RX_axis / 1000 - _end_pose[4] = end_pose.RY_axis / 1000 - _end_pose[5] = end_pose.RZ_axis / 1000 - self.pose = _end_pose + # if pose_before_normalize != self.pose: + # print(f"角度归一化生效: {pose_before_normalize} -> {self.pose}") + + # print("2", self.pose) + # print("=" * 50) + + + + def update_state(self, end_pose, joint_state): + """更新状态信息（从机械臂获取当前状态）""" + # 这里可以选择是否要同步机械臂的实际位置到控制器 + # 如果需要严格同步，可以取消下面的注释 + # self.pose = end_pose.copy() + pass - def get_action(self) -> Dict: """获取当前控制命令""" - if self.end_pose_mode: - # 返回末端位姿 - return { - 'X': self.pose[0], - 'Y': self.pose[1], - 'Z': self.pose[2], - 'RX': self.pose[3], - 'RY': self.pose[4], - 'RZ': self.pose[5], - 'gripper': self.gripper - } - else: - # 返回关节角度 - return { - 'joint0': self.joints[0], - 'joint1': self.joints[1], - 'joint2': self.joints[2], - 'joint3': self.joints[3], - 'joint4': self.joints[4], - 'joint5': self.joints[5], - 'gripper': self.gripper - } - - def get_control_mode(self): - """返回当前控制模式""" - return "end_pose" if self.end_pose_mode else "joints" + return { + 'X': self.pose[0], + 'Y': self.pose[1], + 'Z': self.pose[2], + 'RX': self.pose[3], + 'RY': self.pose[4], + 'RZ': self.pose[5], + 'gripper': self.gripper + } def stop(self): """停止控制器""" @@ -340,9 +271,8 @@ class UnifiedArmController: def reset(self): """重置到初始状态""" - self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self.pose = [0.056127, 0, 0.213266, 0, 84.999, 0] + self.joint = self.init_joint + self.pose = self.init_pose self.pose_speeds = [0.0] * 6 self.gripper = 0.0 self.gripper_speed = 0.0 @@ -350,66 +280,61 @@ class UnifiedArmController: if __name__ == "__main__": - piper = C_PiperInterface_V2("can0") - piper.ConnectPort() - piper.EnableArm(7) - enable_fun(piper=piper) - piper.GripperCtrl(0,1000,0x01, 0) + # 初始化睿尔曼机械臂 + arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) - teleop = UnifiedArmController() - factor = 1000 + # 创建机械臂连接 + handle = arm.rm_create_robot_arm("169.254.128.18", 8080) + print(f"机械臂连接ID: {handle.id}") - while True: - # 获取当前控制命令 - action = teleop.get_action() - control_mode = teleop.get_control_mode() - - if control_mode == "end_pose": - # 末端位姿控制 - position = list(action.values()) - X = round(position[0]*factor*factor) - Y = round(position[1]*factor*factor) - Z = round(position[2]*factor*factor) - RX = round(position[3]*factor) - RY = round(position[4]*factor) - RZ = round(position[5]*factor) - joint_6 = round(position[6]*factor*factor) + # 使能机械臂 + enable_fun(arm=arm) + + teleop = EndPoseController() + + try: + while True: + # 获取当前控制命令 + action = teleop.get_action() - piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) - piper.EndPoseCtrl(X, Y, Z, RX, RY, RZ) - piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0) - - new_end_pose = piper.GetArmEndPoseMsgs().end_pose - new_joint_state = piper.GetArmJointMsgs().joint_state - teleop.update_state(control_mode, new_end_pose, new_joint_state) - print("控制模式: 末端控制") - print("末端位置", new_end_pose) - print("关节位置:", new_joint_state) - - - else: - # 关节控制 - joints = list(action.values()) - # 将关节角度转换为适合发送的格式 - joint0 = round(joints[0] * 57324.840764) # 转换为机械臂期望的单位 - joint1 = round(joints[1] * 57324.840764) - joint2 = round(joints[2] * 57324.840764) - joint3 = round(joints[3] * 57324.840764) - joint4 = round(joints[4] * 57324.840764) - joint5 = round(joints[5] * 57324.840764) - gripper = round(joints[6] * 1000 * 1000) + # 构建目标位姿列表 [X, Y, Z, RX, RY, RZ] + target_pose = [ + action['X'], # X (m) + action['Y'], # Y (m) + action['Z'], # Z (m) + action['RX'], # RX (rad) + action['RY'], # RY (rad) + action['RZ'] # RZ (rad) + ] - # 发送关节控制命令 - piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) - piper.JointCtrl(joint0, joint1, joint2, joint3, joint4, joint5) - piper.GripperCtrl(abs(gripper), 1000, 0x01, 0) - - new_end_pose = piper.GetArmEndPoseMsgs().end_pose - new_joint_state = piper.GetArmJointMsgs().joint_state - teleop.update_state(control_mode, new_end_pose, new_joint_state) + # 使用笛卡尔空间直线运动控制末端位姿 + # 参数: 目标位姿, 速度比例(20%), 交融半径(0), 连接标志(0), 阻塞模式(0-非阻塞) + result = arm.rm_movej_p(target_pose, 50, 0, 0, 1) - print("控制模式: 关节控制") - print("末端位置", new_end_pose) - print("关节位置:", new_joint_state) + if result != 0: + print(f"运动控制错误，错误码: {result}") - time.sleep(0.1) + # 获取当前机械臂状态 + ret = arm.rm_get_current_arm_state() + if ret[0] == 0: # 成功获取状态 + current_pose = ret[1].get('pose', target_pose) + current_joint = ret[1].get('joint', [0]*7) + + teleop.update_state(current_pose, current_joint) + + print("控制模式: 末端控制") + print(f"目标位姿: {target_pose}") + print(f"当前位姿: {current_pose}") + print(f"关节位置: {current_joint}") + else: + print(f"获取机械臂状态失败，错误码: {ret[0]}") + + time.sleep(0.1) + + except KeyboardInterrupt: + print("程序被用户中断") + finally: + # 清理资源 + teleop.stop() + arm.rm_delete_robot_arm() + print("程序退出完成") diff --git a/realman_src/single_arm_control_test.py b/realman_src/single_arm_control_test.py new file mode 100644 index 00000000..395c9889 --- /dev/null +++ b/realman_src/single_arm_control_test.py @@ -0,0 +1,17 @@ +from Robotic_Arm.rm_robot_interface import * + +armleft = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) + +lefthandle = armleft.rm_create_robot_arm("169.254.128.18", 8080) +print("机械臂ID：", lefthandle.id) + + +print("Left: ", armleft.rm_get_current_arm_state()) +print("Left: ", armleft.rm_get_arm_all_state()) +armleft.rm_movej([10, -125, 100, 100, 0, 0, -10], 50, 0, 0, 1) +# armleft.rm_movej_p([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1) +# armleft.rm_movel([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1) + + +# 断开所有连接，销毁线程 +RoboticArm.rm_destory() \ No newline at end of file