Record初步实现

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:
+# cameras:
-  left:
+#   left:
-    serial_number_or_name: 153122077516
+#     serial_number_or_name: 153122077516
-    fps: 30
+#     fps: 30
-    width: 640
+#     width: 640
-    height: 480
+#     height: 480
-    use_depth: False
+#     use_depth: False
-  front:
+#   front:
-    serial_number_or_name: 145422072751
+#     serial_number_or_name: 145422072751
-    fps: 30
+#     fps: 30
-    width: 640
+#     width: 640
-    height: 480
+#     height: 480
-    use_depth: False
+#     use_depth: False
-  high:
+#   high:
-    serial_number_or_name: 145422072193
+#     serial_number_or_name: 145422072193
-    fps: 30
+#     fps: 30
-    width: 640
+#     width: 640
-    height: 480
+#     height: 480
-    use_depth: False
+#     use_depth: False
 joint:
   - -90
   - 90

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)

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)
+        self.handle = None
-        time.sleep(3)
+        self.init_pose = None
-        ret = self.rmarm.rm_get_current_arm_state()
-        self.init_pose = ret[1]['pose']
 
     @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":

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.
+            if len(goal_pos)>0:
-        # /!\ Slower fps expected due to reading from the follower.
+                # Cap goal position when too far away from present position.
-        if self.config.max_relative_target is not None:
+                # /!\ Slower fps expected due to reading from the follower.
-            present_pos = self.bus.sync_read("Present_Position")
+                if self.config.max_relative_target is not None:
-            goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()}
+                    present_pos = self.bus.sync_read("Present_Position")
-            goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target)
+                    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
+                # Send goal position to the arm
-        self.bus.sync_write("Goal_Position", goal_pos)
+                self.bus.sync_write("Goal_Position", goal_pos)
-        return {f"{motor}.pos": val for motor, val in goal_pos.items()}
+            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:

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['x.vel'] = -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['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

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()),),
+            "shape": (7,),
-            "names": self.pose.keys(),
+            "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['x.vel'] = -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['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