diff --git a/robot_client/robots/realman/motors_bus.py b/robot_client/robots/realman/motors_bus.py index f02cf634..e6ae04f2 100644 --- a/robot_client/robots/realman/motors_bus.py +++ b/robot_client/robots/realman/motors_bus.py @@ -1,6 +1,6 @@ import time from typing import Any, Dict -from Robotic_Arm.rm_robot_interface import * +from Robotic_Arm.rm_robot_interface import RoboticArm,rm_thread_mode_e from lerobot.motors import Motor, MotorCalibration, MotorsBus #动作执行成功 ACTION_SUCCESS = 0 @@ -224,6 +224,7 @@ class RealmanMotorsBus(MotorsBus): values = list(actionData.values()) for k,v in enumerate(self.init_pose): self.init_pose[k]+=values[k] + print(f"init_pose:{self.init_pose[:-1]}") self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0) # self.init_pose[6]+=actionData['gripper'] # self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0) @@ -310,15 +311,15 @@ class RealmanMotorsBus(MotorsBus): return 0,self.motors[motor].id def _encode_sign(self, data_name, ids_values): - raise NotImplementedError + pass def _decode_sign(self, data_name, ids_values): - raise NotImplementedError + pass def _split_into_byte_chunks(self, value, length): - raise NotImplementedError + pass def broadcast_ping(self, num_retry = 0, raise_on_error = False): - raise NotImplementedError + pass def _handshake(self) -> None: pass diff --git a/robot_client/teleoperators/xbox/xbox.py b/robot_client/teleoperators/xbox/xbox.py index 7fd06610..044a1454 100644 --- a/robot_client/teleoperators/xbox/xbox.py +++ b/robot_client/teleoperators/xbox/xbox.py @@ -90,38 +90,36 @@ class Xbox(Teleoperator): hat_right = hat[0] == 1 # X+ # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}") # 计算各轴速度 - pose_speeds['x.vel'] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X - pose_speeds['y.vel'] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # Y + pose_speeds['x.vel'] = current_linear_step if hat_left else (-current_linear_step if hat_right else 0.0) # X + pose_speeds['y.vel'] = -current_linear_step if hat_up else (current_linear_step if hat_down else 0.0) # Y - # 左摇杆X轴 - z_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(1)) - print(f"Z轴非线性映射: {self.joystick.get_axis(1)} -> {z_mapping}") - pose_speeds['z.vel'] = z_mapping * current_angular_step * 2 # RY + # 左右扳机控制z轴 + right_tiger = ((self.joystick.get_axis(5)+1)/2) + left_tiger = ((self.joystick.get_axis(4)+1)/2) + z_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger) + pose_speeds['z.vel'] = z_mapping * current_angular_step * 2 - #左摇杆X轴控制RX旋转 + #右摇杆X轴控制RX旋转 # 设置RX旋转速度 rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(2)) # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}") pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2 # RX - # 左摇杆Y轴控制RY旋转 + # 右摇杆Y轴控制RY旋转 # 设置RY旋转速度 - ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(3)) + ry_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(3)) # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}") pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY - # 左右扳机控制RZ旋转 + # 左摇杆X轴RZ旋转 # 设置RZ旋转速度 - right_tiger = ((self.joystick.get_axis(5)+1)/2) - left_tiger = ((self.joystick.get_axis(4)+1)/2) - print(f"左右扳机: {left_tiger} {right_tiger}") - rz_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger) + rz_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(0)) # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}") pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ - # 右摇杆Y轴控制夹爪 + # 左摇杆Y轴控制夹爪 # 设置夹爪开合速度 - gripper_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(0)) + gripper_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(1)) # print(f"gripper轴非线性映射: {self.joystick.get_axis(0)} -> {gripper_mapping}") pose_speeds['gripper.vel'] = gripper_mapping * current_angular_step * 2 # RY diff --git a/test.py b/test.py index 9f1f3dca..47df3ba5 100644 --- a/test.py +++ b/test.py @@ -1,10 +1,18 @@ import time -from Robotic_Arm.rm_robot_interface import * +import pygame if __name__=="__main__": - rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E) - rmarm.rm_create_robot_arm("192.168.3.18", 8080) - rmarm.rm_set_arm_run_mode(0) + pygame.init() + pygame.joystick.init() + joystick = pygame.joystick.Joystick(0) + print(joystick.get_name()) while True: - ret = rmarm.rm_get_arm_software_info() - print(ret) - time.sleep(1) \ No newline at end of file + pygame.event.wait() + a={ + "leftx":joystick.get_axis(0), + "lefty":joystick.get_axis(1), + "rightx":joystick.get_axis(2), + "righty":joystick.get_axis(3), + "left_bj":joystick.get_axis(4), + "right_bj":joystick.get_axis(5), + } + print(a) \ No newline at end of file