diff --git a/robot_client/robots/realman/motors_bus.py b/robot_client/robots/realman/motors_bus.py index f63f4031..af31ecfb 100644 --- a/robot_client/robots/realman/motors_bus.py +++ b/robot_client/robots/realman/motors_bus.py @@ -26,11 +26,6 @@ class RealmanMotorsBus(MotorsBus): calibration: dict[str, MotorCalibration] | None = None, ): super().__init__(port,motors, calibration) - self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E) - if mock: - self.rmarm.rm_set_arm_run_mode(0)#仿真模式 - else: - self.rmarm.rm_set_arm_run_mode(1) address = port.split(':') self.ip = address[0] self.port = int(address[1]) @@ -38,6 +33,8 @@ class RealmanMotorsBus(MotorsBus): self.gripper_range = gripper_range self.init_joint_position = joint self.safe_disable_position = joint + self.mock = mock + self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E) self.handle = None self.init_pose = None @@ -58,8 +55,11 @@ class RealmanMotorsBus(MotorsBus): ''' 使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序 ''' - self.handle = self.rmarm.rm_create_robot_arm(self.ip, self.port) + if self.mock: + self.rmarm.rm_set_arm_run_mode(0)#仿真模式 + else: + self.rmarm.rm_set_arm_run_mode(1) enable_flag = False loop_flag = False # 设置超时时间(秒) @@ -178,8 +178,8 @@ 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) + r = self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0) + print(f"r:{r},pose:{self.init_pose}") # self.init_pose[6]+=actionData['gripper'] # self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0) diff --git a/robot_client/teleoperators/flight_stick/flight_stick.py b/robot_client/teleoperators/flight_stick/flight_stick.py index 9263ac11..d413ba67 100644 --- a/robot_client/teleoperators/flight_stick/flight_stick.py +++ b/robot_client/teleoperators/flight_stick/flight_stick.py @@ -15,6 +15,10 @@ class FlightStick(Xbox): # 根据控制模式调整步长 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) + if self.joystick.get_button(2): # A按钮 + self.fine_control_mode = True + if self.joystick.get_button(3): # B按钮 + self.fine_control_mode = False # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}") print(f"精细控制模式: {self.fine_control_mode}") @@ -26,17 +30,16 @@ class FlightStick(Xbox): hat_left = hat[0] == -1 # X- hat_right = hat[0] == 1 # X+ # 计算各轴速度 - 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 # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}") - # 油门控制Z轴 - gas_axis = -self.joystick.get_axis(2) - - # 设置Z速度 - z_mapping = self.apply_nonlinear_mapping(gas_axis) - # print(f"Z轴非线性映射: {gas_axis} -> {z_mapping}") - pose_speeds['z.vel'] = z_mapping * current_linear_step # Z + if self.joystick.get_button(8): # X按钮 + pose_speeds['z.vel'] = current_linear_step + elif self.joystick.get_button(9): # Y按钮 + pose_speeds['z.vel'] = -current_linear_step + else: + pose_speeds['z.vel'] = 0 # 主摇杆X轴控制RX旋转 @@ -56,6 +59,11 @@ class FlightStick(Xbox): rz_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3)) # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}") pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ - - pose_speeds['gripper.vel'] = 0 + if self.joystick.get_button(0): # X按钮 + pose_speeds['gripper.vel'] = current_linear_step + elif self.joystick.get_button(1): # Y按钮 + pose_speeds['gripper.vel'] = -current_linear_step + else: + pose_speeds['gripper.vel'] = 0 + print(f"pose_speeds: {pose_speeds}") return pose_speeds diff --git a/robot_client/teleoperators/xbox/xbox.py b/robot_client/teleoperators/xbox/xbox.py index 044a1454..39d8aa4c 100644 --- a/robot_client/teleoperators/xbox/xbox.py +++ b/robot_client/teleoperators/xbox/xbox.py @@ -122,6 +122,7 @@ class Xbox(Teleoperator): 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 + print(f"pose_speeds: {pose_speeds}") return pose_speeds