From 1d09062f79e1613675fc4844c466b9add9bef62a Mon Sep 17 00:00:00 2001 From: "1002142102@qq.com" <1002142102@qq.com> Date: Mon, 8 Dec 2025 17:43:12 +0800 Subject: [PATCH] =?UTF-8?q?Record=E5=88=9D=E6=AD=A5=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- realman.yml | 40 ++++---- robot_client/robots/realman/config.py | 4 + robot_client/robots/realman/motors_bus.py | 97 ++++++++++++++----- robot_client/robots/realman/robot.py | 33 ++++--- .../flight_stick/flight_stick.py | 13 +-- robot_client/teleoperators/xbox/xbox.py | 22 +++-- 6 files changed, 140 insertions(+), 69 deletions(-) diff --git a/realman.yml b/realman.yml index 4ab738f5..0a99cfee 100644 --- a/realman.yml +++ b/realman.yml @@ -1,5 +1,7 @@ type: realman port: 192.168.3.18:8080 +mock: True +mode: 1 gripper_range: - 10 - 990 @@ -32,25 +34,25 @@ motors: id: 7 model: realman norm_mode: DEGREES -cameras: - left: - serial_number_or_name: 153122077516 - fps: 30 - width: 640 - height: 480 - use_depth: False - front: - serial_number_or_name: 145422072751 - fps: 30 - width: 640 - height: 480 - use_depth: False - high: - serial_number_or_name: 145422072193 - fps: 30 - width: 640 - height: 480 - use_depth: False +# cameras: +# left: +# serial_number_or_name: 153122077516 +# fps: 30 +# width: 640 +# height: 480 +# use_depth: False +# front: +# serial_number_or_name: 145422072751 +# fps: 30 +# width: 640 +# height: 480 +# use_depth: False +# high: +# serial_number_or_name: 145422072193 +# fps: 30 +# width: 640 +# height: 480 +# use_depth: False joint: - -90 - 90 diff --git a/robot_client/robots/realman/config.py b/robot_client/robots/realman/config.py index 016ad635..bef97d78 100644 --- a/robot_client/robots/realman/config.py +++ b/robot_client/robots/realman/config.py @@ -9,8 +9,12 @@ from lerobot.robots.config import RobotConfig class RealmanRobotConfig(RobotConfig): # Port to connect to the arm port: str + mock: bool = field(default_factory=bool)\ + #0：关节角度透传模式 1:笛卡尔速度透传 + mode: int = 0 gripper_range: list[int] = field(default_factory=list) disable_torque_on_disconnect: bool = True + max_relative_target = None # cameras cameras: dict[str, RealSenseCameraConfig] = field(default_factory=dict) joint: list=field(default_factory=list) diff --git a/robot_client/robots/realman/motors_bus.py b/robot_client/robots/realman/motors_bus.py index 987dc8a4..ce82a49c 100644 --- a/robot_client/robots/realman/motors_bus.py +++ b/robot_client/robots/realman/motors_bus.py @@ -5,11 +5,60 @@ from lerobot.motors import Motor, MotorCalibration, MotorsBus #动作执行成功 ACTION_SUCCESS = 0 +class RoboticArmMock: + def __init__(self,*args): + pass + + def rm_create_robot_arm(self, ip, port): + print(f"connnect {ip}:{port}") + return {"id":1} + + def rm_movej(self,*args): + print(f"rm_movej:{args}") + + def rm_movej_follow(self,*args): + print(f"rm_movej_follow:{args}") + + def rm_get_current_arm_state(self): + print("get_current_arm_state") + return ACTION_SUCCESS, {"pose":[0.0,0.0,0.0,0.0,0.0,0.0],"joint":[0.0,0.0,0.0,0.0,0.0,0.0]} + + def rm_movej_p(self,*args): + print(f"rm_movej_p:{args}") + + + def rm_set_joint_en_state(self,*args): + print(f"rm_set_joint_en_state:{args}") + + def rm_set_gripper_route(self,*args): + print(f"rm_set_gripper_route:{args}") + + def rm_set_gripper_position(self,*args): + print(f"rm_set_gripper_position:{args}") + + def rm_get_gripper_state(self): + print("get_gripper_state") + return ACTION_SUCCESS, {"actpos":0.0} + + def rm_set_gripper_release(self,*args): + print(f"rm_set_gripper_release:{args}") + def rm_set_gripper_pick_on(self,*args): + print(f"rm_set_gripper_pick_on:{args}") + + def rm_movev_canfd(self,*args): + print(f"rm_movev_canfd:{args}") + + def rm_destory(self): + print("destory") + + class RealmanMotorsBus(MotorsBus): model_number_table={} model_ctrl_table={ "realman":{} } + ip: str + port: int """ 对Realman SDK的二次封装 """ @@ -19,20 +68,23 @@ class RealmanMotorsBus(MotorsBus): motors:dict[str, Motor], gripper_range:list[int], joint: list, + mock: bool = False, calibration: dict[str, MotorCalibration] | None = None, ): super().__init__(port,motors, calibration) - self.rmarm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) + if mock: + self.rmarm = RoboticArmMock() + else: + self.rmarm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) address = port.split(':') - self.handle = self.rmarm.rm_create_robot_arm(address[0], int(address[1])) + self.ip = address[0] + self.port = int(address[1]) self.motors = motors self.gripper_range = gripper_range self.init_joint_position = joint self.safe_disable_position = joint - self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1) - time.sleep(3) - ret = self.rmarm.rm_get_current_arm_state() - self.init_pose = ret[1]['pose'] + self.handle = None + self.init_pose = None @property def motor_names(self) -> list[str]: @@ -87,7 +139,12 @@ class RealmanMotorsBus(MotorsBus): enable_flag = True break time.sleep(1) - + + self.handle = self.rmarm.rm_create_robot_arm(self.ip, self.port) + self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1) + time.sleep(3) + ret = self.rmarm.rm_get_current_arm_state() + self.init_pose = ret[1]['pose'] resp = enable_flag print(f"Returning response: {resp}") return resp @@ -120,22 +177,6 @@ class RealmanMotorsBus(MotorsBus): ret = self.rmarm.rm_set_gripper_position(target_joint[-1], block=False, timeout=2) return ret - - def write_endpose(self, target_endpose: list, gripper: int): - self.rmarm.rm_movej_p(target_endpose, 50, 0, 0, 1) - self.rmarm.rm_set_gripper_position(gripper, block=False, timeout=2) - - def write_joint_slow(self, target_joint: list): - self.rmarm.rm_movej(target_joint, 5, 0, 0, 0) - - def write_joint_canfd(self, target_joint: list): - self.rmarm.rm_movej_canfd(target_joint, False) - - def write_endpose_canfd(self, target_pose: list): - self.rmarm.rm_movep_canfd(target_pose, False) - - def write_gripper(self, gripper: int): - self.rmarm.rm_set_gripper_position(gripper, False, 2) def read(self) -> Dict: """ @@ -163,6 +204,9 @@ class RealmanMotorsBus(MotorsBus): def read_current_arm_endpose_state(self): return self.rmarm.rm_get_current_arm_state()[1]['pose'] + + def disconnect(self, disable_torque = True): + self.safe_disconnect() def safe_disconnect(self): """ @@ -178,6 +222,13 @@ class RealmanMotorsBus(MotorsBus): def sync_write(self,name,actionData: dict[str, Any]): if name =="Goal_Position": self.write_arm(target_joint=actionData) + elif name == "Goal_Velocity":#速度透传模式 + self.rmarm.rm_movev_canfd(list(actionData.values())[:6]) + gripper = actionData['gripper'] + if gripper > 0 : + self.rmarm.rm_set_gripper_release(gripper*1000, False) + else: + self.rmarm.rm_set_gripper_pick_on(gripper*1000, gripper*1000, False) def sync_read(self, data_name, motors = None, *, normalize = True, num_retry = 0): if data_name == "Present_Position": diff --git a/robot_client/robots/realman/robot.py b/robot_client/robots/realman/robot.py index 505b2d55..3ca379a2 100644 --- a/robot_client/robots/realman/robot.py +++ b/robot_client/robots/realman/robot.py @@ -20,6 +20,7 @@ class RealmanRobot(Robot): port=self.config.port, motors=config.motors, joint=config.joint, + mock=self.config.mock, gripper_range=config.gripper_range, calibration=self.calibration, ) @@ -40,7 +41,10 @@ class RealmanRobot(Robot): @cached_property def action_features(self) -> dict[str, type]: - return self._motors_ft + if self.config.mode==0: + return self._motors_ft + else: + return {"x.vel":float,"y.vel":float,"z.vel":float,"rx.vel":float,"ry.vel":float,"rz.vel":float,"gripper.vel":float} @property def is_connected(self) -> bool: @@ -120,19 +124,26 @@ class RealmanRobot(Robot): """ if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") + if self.config.mode==0: - goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} + goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} - # Cap goal position when too far away from present position. - # /!\ Slower fps expected due to reading from the follower. - if self.config.max_relative_target is not None: - present_pos = self.bus.sync_read("Present_Position") - goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()} - goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) + if len(goal_pos)>0: + # Cap goal position when too far away from present position. + # /!\ Slower fps expected due to reading from the follower. + if self.config.max_relative_target is not None: + present_pos = self.bus.sync_read("Present_Position") + goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()} + goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) - # Send goal position to the arm - self.bus.sync_write("Goal_Position", goal_pos) - return {f"{motor}.pos": val for motor, val in goal_pos.items()} + # Send goal position to the arm + self.bus.sync_write("Goal_Position", goal_pos) + return {f"{motor}.pos": val for motor, val in goal_pos.items()} + else: + valAction = {key.removesuffix(".vel"): val for key, val in action.items() if key.endswith(".vel")} + #笛卡尔速度透传 + self.bus.sync_write("Goal_Velocity", valAction) + return action def disconnect(self): if not self.is_connected: diff --git a/robot_client/teleoperators/flight_stick/flight_stick.py b/robot_client/teleoperators/flight_stick/flight_stick.py index 4010c224..8159490b 100644 --- a/robot_client/teleoperators/flight_stick/flight_stick.py +++ b/robot_client/teleoperators/flight_stick/flight_stick.py @@ -26,8 +26,8 @@ class FlightStick(Xbox): hat_left = hat[0] == -1 # X- hat_right = hat[0] == 1 # X+ # 计算各轴速度 - pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X - pose_speeds[1] = 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轴 @@ -36,25 +36,26 @@ class FlightStick(Xbox): # 设置Z速度 z_mapping = self.apply_nonlinear_mapping(gas_axis) # print(f"Z轴非线性映射: {gas_axis} -> {z_mapping}") - pose_speeds[2] = z_mapping * current_linear_step # Z + pose_speeds['z.vel'] = z_mapping * current_linear_step # Z # 主摇杆X轴控制RX旋转 # 设置RX旋转速度 rx_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1)) # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}") - pose_speeds[3] = rx_mapping * current_angular_step * 2 # RX + pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2 # RX # 主摇杆Y轴控制RY旋转 # 设置RY旋转速度 ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(0)) # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}") - pose_speeds[4] = ry_mapping * current_angular_step * 2 # RY + pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY # 主摇杆左右旋转轴 控制RZ旋转 # 设置RZ旋转速度 rz_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3)) # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}") - pose_speeds[5] = rz_mapping * current_angular_step * 2 # RZ + pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ + pose_speeds['gripper.vel'] = 0 return pose_speeds diff --git a/robot_client/teleoperators/xbox/xbox.py b/robot_client/teleoperators/xbox/xbox.py index 0987cee8..2ecbcf6f 100644 --- a/robot_client/teleoperators/xbox/xbox.py +++ b/robot_client/teleoperators/xbox/xbox.py @@ -8,7 +8,7 @@ Xbox类用于检测和监控飞行手柄。 """ class Xbox(Teleoperator): config_class = XboxConfig - name = "flight_stick" + name = "xbox" def __init__(self, config: XboxConfig): self.config = config # 控制参数 @@ -26,8 +26,8 @@ class Xbox(Teleoperator): def action_features(self): return { "dtype": "float32", - "shape": (len(self.pose.keys()),), - "names": self.pose.keys(), + "shape": (7,), + "names": {"x.vel":0, "y.vel":1, "z.vel":2, "rx.vel.vel":3, "ry.vel":4, "rz.vel":5, "gripper.vel":6}, } @property @@ -69,7 +69,7 @@ class Xbox(Teleoperator): def get_action(self): pygame.event.pump() - pose_speeds = [0.0] * 6 + pose_speeds = {} """更新末端位姿控制""" # 根据控制模式调整步长 current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0) @@ -90,25 +90,25 @@ 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[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X - pose_speeds[1] = 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[2] = z_mapping * current_angular_step * 2 # RY + pose_speeds['z.vel'] = z_mapping * current_angular_step * 2 # RY #左摇杆X轴控制RX旋转 # 设置RX旋转速度 rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(2)) # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}") - pose_speeds[3] = rx_mapping * current_angular_step * 2 # RX + pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2 # RX # 左摇杆Y轴控制RY旋转 # 设置RY旋转速度 ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(3)) # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}") - pose_speeds[4] = ry_mapping * current_angular_step * 2 # RY + pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY # 左右扳机控制RZ旋转 # 设置RZ旋转速度 @@ -117,7 +117,9 @@ class Xbox(Teleoperator): print(f"左右扳机: {left_tiger} {right_tiger}") rz_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger) # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}") - pose_speeds[5] = rz_mapping * current_angular_step * 2 # RZ + pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ + + pose_speeds['gripper.vel'] = 0 return pose_speeds