diff --git a/realman_src/realman_demo.py b/realman_src/realman_xbox.py similarity index 89% rename from realman_src/realman_demo.py rename to realman_src/realman_xbox.py index 4afba56bb..58f64a58b 100644 --- a/realman_src/realman_demo.py +++ b/realman_src/realman_xbox.py @@ -45,7 +45,7 @@ def enable_fun(arm: RoboticArm): class EndPoseController: - def __init__(self): + def __init__(self, init_joint, init_pose): # 初始化pygame和手柄 pygame.init() pygame.joystick.init() @@ -65,8 +65,8 @@ class EndPoseController: self.fine_control_mode = False # 初始化末端姿态 [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.init_joint = init_joint + self.init_pose = init_pose self.joint = self.init_joint self.pose = self.init_pose self.pose_speeds = [0.0] * 6 @@ -86,8 +86,8 @@ class EndPoseController: self.angular_step = 0.001 # 角度步长(rad) - 从度转换为弧度 # 夹爪状态和速度 - self.gripper = 0.0 - self.gripper_speed = 0.0 + self.gripper_open = False + self.gripper_speed = 10 # 启动更新线程 self.running = True @@ -133,8 +133,8 @@ class EndPoseController: 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.gripper_open = False + self.gripper_speed = 10 print("机械臂已重置到初始位置") time.sleep(0.3) # 防止多次触发 @@ -144,16 +144,17 @@ class EndPoseController: # 夹爪控制（圈/叉） 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) - + self.gripper_speed = 10 if circle else (10 if cross else 0) + self.gripper_open = True if circle else False + # 更新夹爪 - self.gripper += self.gripper_speed - self.gripper = max(0.0, min(0.08, self.gripper)) + # self.gripper += self.gripper_speed + # self.gripper = max(0.0, min(0.1, self.gripper)) time.sleep(0.02) def update_end_pose(self): - print("1", self.pose) + print("1", self.pose, "griper", self.gripper_open) """更新末端位姿控制""" # 根据控制模式调整步长 current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0) @@ -258,7 +259,8 @@ class EndPoseController: 'RX': self.pose[3], 'RY': self.pose[4], 'RZ': self.pose[5], - 'gripper': self.gripper + 'gripper_speed': self.gripper_speed, + 'gripper_open': self.gripper_open } def stop(self): @@ -274,8 +276,8 @@ class EndPoseController: 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.gripper_open = False + self.gripper_speed = 10 print("已重置到初始状态") @@ -283,14 +285,17 @@ if __name__ == "__main__": # 初始化睿尔曼机械臂 arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) + init_joint = [-90, 90, 90, 90, -90, -90, 90] + init_pose = [-0.030, 0.255, 0.161, 3.142, 0, -1.57] + # 创建机械臂连接 - handle = arm.rm_create_robot_arm("169.254.128.18", 8080) + handle = arm.rm_create_robot_arm("192.168.3.18", 8080) print(f"机械臂连接ID: {handle.id}") # 使能机械臂 enable_fun(arm=arm) - teleop = EndPoseController() + teleop = EndPoseController(init_joint, init_pose) try: while True: @@ -314,6 +319,13 @@ if __name__ == "__main__": if result != 0: print(f"运动控制错误，错误码: {result}") + if action['gripper_open']: + # arm.rm_set_gripper_release(action['gripper_speed'], block=True) + arm.rm_set_gripper_position(1000, True, 1) + else: + # arm.rm_set_gripper_pick(action['gripper_speed'], force=50, block=True) + arm.rm_set_gripper_position(1, True, 1) + # 获取当前机械臂状态 ret = arm.rm_get_current_arm_state() if ret[0] == 0: # 成功获取状态 diff --git a/realman_src/single_arm_connect_test.py b/realman_src/single_arm_connect_test.py index 2f9697734..456a30f28 100644 --- a/realman_src/single_arm_connect_test.py +++ b/realman_src/single_arm_connect_test.py @@ -1,7 +1,7 @@ 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) +handle = robot.rm_create_robot_arm("192.168.3.18", 8080) print("机械臂ID：", handle.id) software_info = robot.rm_get_arm_software_info() @@ -16,3 +16,8 @@ if software_info[0] == 0: else: print("\nFailed to get arm software information, Error code: ", software_info[0], "\n") +print("Left: ", robot.rm_get_current_arm_state()) +print("Left: ", robot.rm_get_arm_all_state()) + +# 断开所有连接，销毁线程 +RoboticArm.rm_destory() \ No newline at end of file diff --git a/realman_src/single_arm_control_test.py b/realman_src/single_arm_control_test.py index 395c98892..1f2c3eae1 100644 --- a/realman_src/single_arm_control_test.py +++ b/realman_src/single_arm_control_test.py @@ -2,16 +2,20 @@ 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) +lefthandle = armleft.rm_create_robot_arm("192.168.3.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_movej([-90, 90, 90, -90, -90, 90], 50, 0, 0, 1) +# armleft.rm_movej([-90, 90, 90, -90, -90, 90], 50, 0, 0, 1) +# armleft.rm_movej_p([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1)s # armleft.rm_movel([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1) - +armleft.rm_set_gripper_position(1000, True, 2) +import time +time.sleep(3) +armleft.rm_set_gripper_position(1, True, 2) # 断开所有连接，销毁线程 RoboticArm.rm_destory() \ No newline at end of file