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

integrate agilex piper arm into lerobot

Browse Source
This commit is contained in:
brucecai-2001
2025-03-02 15:14:04 +08:00
parent 800c4a847f
commit 7dd20d6ed9
16 changed files with 1263 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
lerobot/common/datasets/video_utils.py
View File

@@ -131,7 +131,7 @@ def encode_video_frames(
imgs_dir: Path | str,
video_path: Path | str,
fps: int,
vcodec: str = "libsvtav1",
vcodec: str = "libopenh264",
pix_fmt: str = "yuv420p",
g: int | None = 2,
crf: int | None = 30,

4
lerobot/common/robot_devices/control_utils.py
View File

@@ -43,7 +43,7 @@ def log_control_info(robot: Robot, dt_s, episode_index=None, frame_index=None, f
log_dt("dt", dt_s)
# TODO(aliberts): move robot-specific logs logic in robot.print_logs()
if not robot.robot_type.startswith("stretch"):
if not robot.robot_type.startswith(("stretch", "piper")):
for name in robot.leader_arms:
key = f"read_leader_{name}_pos_dt_s"
if key in robot.logs:
@@ -301,7 +301,7 @@ def stop_recording(robot, listener, display_cameras):
def sanity_check_dataset_name(repo_id, policy_cfg):
_, dataset_name = repo_id.split("/")
dataset_name = repo_id.split("/")[-1]
# either repo_id doesnt start with "eval_" and there is no policy
# or repo_id starts with "eval_" and there is a policy

7
lerobot/common/robot_devices/motors/configs.py
View File

@@ -25,3 +25,10 @@ class FeetechMotorsBusConfig(MotorsBusConfig):
port: str
motors: dict[str, tuple[int, str]]
mock: bool = False
@MotorsBusConfig.register_subclass("piper")
@dataclass
class PiperMotorsBusConfig(MotorsBusConfig):
can_name: str
motors: dict[str, tuple[int, str]]

146
lerobot/common/robot_devices/motors/piper.py Normal file
View File

@@ -0,0 +1,146 @@
import time
from typing import Dict
from piper_sdk import *
from lerobot.common.robot_devices.motors.configs import PiperMotorsBusConfig
class PiperMotorsBus:
"""
对Piper SDK的二次封装
"""
def __init__(self,
config: PiperMotorsBusConfig):
self.piper = C_PiperInterface_V2(config.can_name)
self.piper.ConnectPort()
self.motors = config.motors
self.init_joint_position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [6 joints + 1 gripper] * 0.0
self.safe_disable_position = [0.0, 0.0, 0.0, 0.0, 0.52, 0.0, 0.0]
self.pose_factor = 1000 # 单位 0.001mm
self.joint_factor = 57324.840764 # 1000*180/3.14， rad -> 度（单位0.001度）
@property
def motor_names(self) -> list[str]:
return list(self.motors.keys())
@property
def motor_models(self) -> list[str]:
return [model for _, model in self.motors.values()]
@property
def motor_indices(self) -> list[int]:
return [idx for idx, _ in self.motors.values()]
def connect(self, enable:bool) -> bool:
'''
使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
'''
enable_flag = False
loop_flag = False
# 设置超时时间（秒）
timeout = 5
# 记录进入循环前的时间
start_time = time.time()
while not (loop_flag):
elapsed_time = time.time() - start_time
print(f"--------------------")
enable_list = []
enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status)
enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status)
enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status)
enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status)
enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status)
enable_list.append(self.piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status)
if(enable):
enable_flag = all(enable_list)
self.piper.EnableArm(7)
self.piper.GripperCtrl(0,1000,0x01, 0)
else:
# move to safe disconnect position
enable_flag = any(enable_list)
self.piper.DisableArm(7)
self.piper.GripperCtrl(0,1000,0x02, 0)
print(f"使能状态: {enable_flag}")
print(f"--------------------")
if(enable_flag == enable):
loop_flag = True
enable_flag = True
else:
loop_flag = False
enable_flag = False
# 检查是否超过超时时间
if elapsed_time > timeout:
print(f"超时....")
enable_flag = False
loop_flag = True
break
time.sleep(0.5)
resp = enable_flag
print(f"Returning response: {resp}")
return resp
def motor_names(self):
return
def set_calibration(self):
return
def revert_calibration(self):
return
def apply_calibration(self):
"""
移动到初始位置
"""
self.write(target_joint=self.init_joint_position)
def write(self, target_joint:list):
"""
Joint control
- target joint: in radians
joint_1 (float): 关节1角度 (-92000~92000) / 57324.840764
joint_2 (float): 关节2角度 -1300 ~ 90000 / 57324.840764
joint_3 (float): 关节3角度 2400 ~ -80000 / 57324.840764
joint_4 (float): 关节4角度 -90000~90000 / 57324.840764
joint_5 (float): 关节5角度 19000~-77000 / 57324.840764
joint_6 (float): 关节6角度 -90000~90000 / 57324.840764
gripper_range: 夹爪角度 0~0.08
"""
joint_0 = round(target_joint[0]*self.joint_factor)
joint_1 = round(target_joint[1]*self.joint_factor)
joint_2 = round(target_joint[2]*self.joint_factor)
joint_3 = round(target_joint[3]*self.joint_factor)
joint_4 = round(target_joint[4]*self.joint_factor)
joint_5 = round(target_joint[5]*self.joint_factor)
gripper_range = round(target_joint[6]*1000*1000)
self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) # joint control
self.piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5)
self.piper.GripperCtrl(abs(gripper_range), 1000, 0x01, 0) # 单位 0.001°
def read(self) -> Dict:
"""
- 机械臂关节消息,单位0.001度
- 机械臂夹爪消息
"""
joint_msg = self.piper.GetArmJointMsgs()
joint_state = joint_msg.joint_state
gripper_msg = self.piper.GetArmGripperMsgs()
gripper_state = gripper_msg.gripper_state
return {
"joint_1": joint_state.joint_1,
"joint_2": joint_state.joint_2,
"joint_3": joint_state.joint_3,
"joint_4": joint_state.joint_4,
"joint_5": joint_state.joint_5,
"joint_6": joint_state.joint_6,
"gripper": gripper_state.grippers_angle
}
def safe_disconnect(self):
"""
Move to safe disconnect position
"""
self.write(target_joint=self.safe_disable_position)

17
lerobot/common/robot_devices/motors/utils.py
View File

@@ -3,7 +3,8 @@ from typing import Protocol
from lerobot.common.robot_devices.motors.configs import (
DynamixelMotorsBusConfig,
FeetechMotorsBusConfig,
MotorsBusConfig,
PiperMotorsBusConfig,
MotorsBusConfig
)
@@ -30,6 +31,11 @@ def make_motors_buses_from_configs(motors_bus_configs: dict[str, MotorsBusConfig
motors_buses[key] = FeetechMotorsBus(cfg)
elif cfg.type == "piper":
from lerobot.common.robot_devices.motors.piper import PiperMotorsBus
motors_buses[key] = PiperMotorsBus(cfg)
else:
raise ValueError(f"The motor type '{cfg.type}' is not valid.")
@@ -48,6 +54,15 @@ def make_motors_bus(motor_type: str, **kwargs) -> MotorsBus:
config = FeetechMotorsBusConfig(**kwargs)
return FeetechMotorsBus(config)
elif motor_type == "piper":
from lerobot.common.robot_devices.motors.piper import PiperMotorsBus
config = PiperMotorsBusConfig(**kwargs)
return PiperMotorsBus(config)
else:
raise ValueError(f"The motor type '{motor_type}' is not valid.")
def get_motor_names(arm: dict[str, MotorsBus]) -> list:
return [f"{arm}_{motor}" for arm, bus in arm.items() for motor in bus.motors]

42
lerobot/common/robot_devices/robots/configs.py
View File

@@ -13,6 +13,7 @@ from lerobot.common.robot_devices.motors.configs import (
DynamixelMotorsBusConfig,
FeetechMotorsBusConfig,
MotorsBusConfig,
PiperMotorsBusConfig
)
@@ -597,3 +598,44 @@ class LeKiwiRobotConfig(RobotConfig):
)
mock: bool = False
@RobotConfig.register_subclass("piper")
@dataclass
class PiperRobotConfig(RobotConfig):
inference_time: bool
follower_arm: dict[str, MotorsBusConfig] = field(
default_factory=lambda: {
"main": PiperMotorsBusConfig(
can_name="can0",
motors={
# name: (index, model)
"joint_1": [1, "agilex_piper"],
"joint_2": [2, "agilex_piper"],
"joint_3": [3, "agilex_piper"],
"joint_4": [4, "agilex_piper"],
"joint_5": [5, "agilex_piper"],
"joint_6": [6, "agilex_piper"],
"gripper": (7, "agilex_piper"),
},
),
}
)
cameras: dict[str, CameraConfig] = field(
default_factory=lambda: {
"one": OpenCVCameraConfig(
camera_index=0,
fps=30,
width=640,
height=480,
),
"two": OpenCVCameraConfig(
camera_index=2,
fps=30,
width=640,
height=480,
),
}
)

235
lerobot/common/robot_devices/robots/piper.py Normal file
View File

@@ -0,0 +1,235 @@
"""
Teleoperation Agilex Piper with a PS5 controller
"""
import time
import torch
import numpy as np
from dataclasses import dataclass, field, replace
from lerobot.common.robot_devices.teleop.gamepad import SixAxisArmController
from lerobot.common.robot_devices.motors.utils import get_motor_names, make_motors_buses_from_configs
from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
from lerobot.common.robot_devices.robots.configs import PiperRobotConfig
class PiperRobot:
def __init__(self, config: PiperRobotConfig | None = None, **kwargs):
if config is None:
config = PiperRobotConfig()
# Overwrite config arguments using kwargs
self.config = replace(config, **kwargs)
self.robot_type = self.config.type
self.inference_time = self.config.inference_time # if it is inference time
# build cameras
self.cameras = make_cameras_from_configs(self.config.cameras)
# build piper motors
self.piper_motors = make_motors_buses_from_configs(self.config.follower_arm)
self.arm = self.piper_motors['main']
# build gamepad teleop
if not self.inference_time:
self.teleop = SixAxisArmController()
else:
self.teleop = None
self.logs = {}
self.is_connected = False
@property
def camera_features(self) -> dict:
cam_ft = {}
for cam_key, cam in self.cameras.items():
key = f"observation.images.{cam_key}"
cam_ft[key] = {
"shape": (cam.height, cam.width, cam.channels),
"names": ["height", "width", "channels"],
"info": None,
}
return cam_ft
@property
def motor_features(self) -> dict:
action_names = get_motor_names(self.piper_motors)
state_names = get_motor_names(self.piper_motors)
return {
"action": {
"dtype": "float32",
"shape": (len(action_names),),
"names": action_names,
},
"observation.state": {
"dtype": "float32",
"shape": (len(state_names),),
"names": state_names,
},
}
@property
def has_camera(self):
return len(self.cameras) > 0
@property
def num_cameras(self):
return len(self.cameras)
def connect(self) -> None:
"""Connect piper and cameras"""
if self.is_connected:
raise RobotDeviceAlreadyConnectedError(
"Piper is already connected. Do not run `robot.connect()` twice."
)
# connect piper
self.arm.connect(enable=True)
print("piper conneted")
# connect cameras
for name in self.cameras:
self.cameras[name].connect()
self.is_connected = self.is_connected and self.cameras[name].is_connected
print(f"camera {name} conneted")
print("All connected")
self.is_connected = True
self.run_calibration()
def disconnect(self) -> None:
"""move to home position, disenable piper and cameras"""
# move piper to home position, disable
if not self.inference_time:
self.teleop.stop()
# disconnect piper
self.arm.safe_disconnect()
print("piper disable after 5 seconds")
time.sleep(5)
self.arm.connect(enable=False)
# disconnect cameras
if len(self.cameras) > 0:
for cam in self.cameras.values():
cam.disconnect()
self.is_connected = False
def run_calibration(self):
"""move piper to the home position"""
if not self.is_connected:
raise ConnectionError()
self.arm.apply_calibration()
if not self.inference_time:
self.teleop.reset()
def teleop_step(
self, record_data=False
) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
if not self.is_connected:
raise ConnectionError()
if self.teleop is None and self.inference_time:
self.teleop = SixAxisArmController()
# read target pose state as
before_read_t = time.perf_counter()
state = self.arm.read() # read current joint position from robot
action = self.teleop.get_action() # target joint position from gamepad
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
# do action
before_write_t = time.perf_counter()
target_joints = list(action.values())
self.arm.write(target_joints)
self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
if not record_data:
return
state = torch.as_tensor(list(state.values()), dtype=torch.float32)
action = torch.as_tensor(list(action.values()), dtype=torch.float32)
# Capture images from cameras
images = {}
for name in self.cameras:
before_camread_t = time.perf_counter()
images[name] = self.cameras[name].async_read()
images[name] = torch.from_numpy(images[name])
self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs["delta_timestamp_s"]
self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
# Populate output dictionnaries
obs_dict, action_dict = {}, {}
obs_dict["observation.state"] = state
action_dict["action"] = action
for name in self.cameras:
obs_dict[f"observation.images.{name}"] = images[name]
return obs_dict, action_dict
def send_action(self, action: torch.Tensor) -> torch.Tensor:
"""Write the predicted actions from policy to the motors"""
if not self.is_connected:
raise RobotDeviceNotConnectedError(
"Piper is not connected. You need to run `robot.connect()`."
)
# send to motors, torch to list
target_joints = action.tolist()
self.arm.write(target_joints)
return action
def capture_observation(self) -> dict:
"""capture current images and joint positions"""
if not self.is_connected:
raise RobotDeviceNotConnectedError(
"Piper is not connected. You need to run `robot.connect()`."
)
# read current joint positions
before_read_t = time.perf_counter()
state = self.arm.read() # 6 joints + 1 gripper
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
state = torch.as_tensor(list(state.values()), dtype=torch.float32)
# read images from cameras
images = {}
for name in self.cameras:
before_camread_t = time.perf_counter()
images[name] = self.cameras[name].async_read()
images[name] = torch.from_numpy(images[name])
self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs["delta_timestamp_s"]
self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
# Populate output dictionnaries and format to pytorch
obs_dict = {}
obs_dict["observation.state"] = state
for name in self.cameras:
obs_dict[f"observation.images.{name}"] = images[name]
return obs_dict
def teleop_safety_stop(self):
""" move to home position after record one episode """
self.run_calibration()
def __del__(self):
if self.is_connected:
self.disconnect()
if not self.inference_time:
self.teleop.stop()

7
lerobot/common/robot_devices/robots/utils.py
View File

@@ -10,6 +10,7 @@ from lerobot.common.robot_devices.robots.configs import (
RobotConfig,
So100RobotConfig,
StretchRobotConfig,
PiperRobotConfig
)
@@ -48,6 +49,8 @@ def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:
return StretchRobotConfig(**kwargs)
elif robot_type == "lekiwi":
return LeKiwiRobotConfig(**kwargs)
elif robot_type == 'piper':
return PiperRobotConfig(**kwargs)
else:
raise ValueError(f"Robot type '{robot_type}' is not available.")
@@ -61,6 +64,10 @@ def make_robot_from_config(config: RobotConfig):
from lerobot.common.robot_devices.robots.mobile_manipulator import MobileManipulator
return MobileManipulator(config)
elif isinstance(config, PiperRobotConfig):
from lerobot.common.robot_devices.robots.piper import PiperRobot
return PiperRobot(config)
else:
from lerobot.common.robot_devices.robots.stretch import StretchRobot

134
lerobot/common/robot_devices/teleop/gamepad.py Executable file
View File

@@ -0,0 +1,134 @@
import pygame
import threading
import time
from typing import Dict
class SixAxisArmController:
def __init__(self):
# 初始化pygame和手柄
pygame.init()
pygame.joystick.init()
# 检查是否有连接的手柄
if pygame.joystick.get_count() == 0:
raise Exception("未检测到手柄")
# 初始化手柄
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
# 初始化关节和夹爪状态
self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节
self.gripper = 0.0 # 夹爪状态
self.speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节的速度
self.gripper_speed = 0.0 # 夹爪速度
# 定义关节弧度限制（计算好的范围）
self.joint_limits = [
(-92000 / 57324.840764, 92000 / 57324.840764), # joint1
(-1300 / 57324.840764, 90000 / 57324.840764), # joint2
(-80000 / 57324.840764, 0 / 57324.840764), # joint3
(-90000 / 57324.840764, 90000 / 57324.840764), # joint4
(-77000 / 57324.840764, 19000 / 57324.840764), # joint5
(-90000 / 57324.840764, 90000 / 57324.840764) # joint6
]
# 启动更新线程
self.running = True
self.thread = threading.Thread(target=self.update_joints)
self.thread.start()
def update_joints(self):
while self.running:
# 处理事件队列
try:
pygame.event.pump()
except Exception as e:
self.stop()
continue
# 获取摇杆和按钮输入
left_x = -self.joystick.get_axis(0) # 左摇杆x轴
if abs(left_x) < 0.5:
left_x = 0.0
left_y = -self.joystick.get_axis(1) # 左摇杆y轴（取反，因为y轴向下为正
if abs(left_y) < 0.5:
left_y = 0.0
right_x = -self.joystick.get_axis(3) # 右摇杆x轴（取反，因为y轴向下为正）
if abs(right_x) < 0.5:
right_x = 0.0
# 获取方向键输入
hat = self.joystick.get_hat(0)
up = hat[1] == 1
down = hat[1] == -1
left = hat[0] == -1
right = hat[0] == 1
# 获取按钮输入
circle = self.joystick.get_button(1) # 圈按钮
cross = self.joystick.get_button(0) # 叉按钮
triangle = self.joystick.get_button(2)
square = self.joystick.get_button(3)
# 映射输入到速度
self.speeds[0] = left_x * 0.01 # joint1速度
self.speeds[1] = left_y * 0.01 # joint2速度
self.speeds[2] = 0.01 if triangle else (-0.01 if square else 0.0) # joint3速度
self.speeds[3] = right_x * 0.01 # joint4速度
self.speeds[4] = 0.01 if up else (-0.01 if down else 0.0) # joint5速度
self.speeds[5] = 0.01 if right else (-0.01 if left else 0.0) # joint6速度
self.gripper_speed = 0.01 if circle else (-0.01 if cross else 0.0) # 夹爪速度
# 积分速度到关节位置
for i in range(6):
self.joints[i] += self.speeds[i]
self.gripper += self.gripper_speed
# 关节范围保护
for i in range(6):
min_val, max_val = self.joint_limits[i]
self.joints[i] = max(min_val, min(max_val, self.joints[i]))
# 夹爪范围保护（0~0.08弧度）
self.gripper = max(0.0, min(0.08, self.gripper))
# 控制更新频率
time.sleep(0.02)
def get_action(self) -> Dict:
# 返回机械臂的当前状态
return {
'joint0': self.joints[0],
'joint1': self.joints[1],
'joint2': self.joints[2],
'joint3': self.joints[3],
'joint4': self.joints[4],
'joint5': self.joints[5],
'gripper': self.gripper
}
def stop(self):
# 停止更新线程
self.running = False
self.thread.join()
pygame.quit()
print("Gamepad exits")
def reset(self):
self.joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节
self.gripper = 0.0 # 夹爪状态
self.speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 6个关节的速度
self.gripper_speed = 0.0 # 夹爪速度
# 使用示例
if __name__ == "__main__":
arm_controller = SixAxisArmController()
try:
while True:
print(arm_controller.get_action())
time.sleep(0.1)
except KeyboardInterrupt:
arm_controller.stop()