tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Record初步实现

Browse Source
This commit is contained in:
1002142102@qq.com
2025-12-08 17:43:12 +08:00
parent 2b523acc52
commit 1d09062f79
6 changed files with 140 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
robot_client/robots/realman/config.py
View File

@@ -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)

97
robot_client/robots/realman/motors_bus.py
View File

@@ -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":

33
robot_client/robots/realman/robot.py
View File

@@ -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:

13
robot_client/teleoperators/flight_stick/flight_stick.py
View File

@@ -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

22
robot_client/teleoperators/xbox/xbox.py
View File

@@ -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