镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-21 02:27:04 +00:00
feat(teleop): add piper leader/follower adapter for LeRobot
- add piper_follower robot and piper_leader teleoperator implementations based on Piper SDK CAN APIs - wire piper types into teleoperate/record/replay/calibrate command imports and device factories - add Piper SDK import helper with auto-discovery and unit conversion utilities - add piper teleop tests and camera-compatible follower observation support for record/hil flows
这个提交包含在:
@ -0,0 +1,18 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
from .config_piper_follower import PiperFollowerConfig, PiperFollowerConfigBase
|
||||||
|
from .piper_follower import PiperFollower
|
||||||
@ -0,0 +1,77 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
|
||||||
|
from lerobot.cameras import CameraConfig
|
||||||
|
|
||||||
|
from ..config import RobotConfig
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class PiperFollowerConfigBase:
|
||||||
|
"""Configuration for a Piper follower arm controlled over CAN."""
|
||||||
|
|
||||||
|
# CAN interface name (e.g. "can0")
|
||||||
|
port: str
|
||||||
|
|
||||||
|
# Piper SDK connection options
|
||||||
|
judge_flag: bool = False
|
||||||
|
can_auto_init: bool = True
|
||||||
|
log_level: str = "WARNING"
|
||||||
|
startup_sleep_s: float = 0.1
|
||||||
|
|
||||||
|
# Optional role command on startup (0xFC = motion output arm)
|
||||||
|
set_slave_mode_on_connect: bool = False
|
||||||
|
|
||||||
|
# Motion mode for follower arm
|
||||||
|
speed_ratio: int = 100
|
||||||
|
high_follow: bool = True
|
||||||
|
mode_refresh_interval_s: float = 1.0
|
||||||
|
|
||||||
|
# Arm enable behavior
|
||||||
|
enable_on_connect: bool = True
|
||||||
|
enable_timeout_s: float = 3.0
|
||||||
|
|
||||||
|
# Gripper forwarding behavior
|
||||||
|
sync_gripper: bool = True
|
||||||
|
gripper_effort_default: int = 1000
|
||||||
|
gripper_status_code: int = 0x01
|
||||||
|
|
||||||
|
# Optional cameras
|
||||||
|
cameras: dict[str, CameraConfig] = field(default_factory=dict)
|
||||||
|
|
||||||
|
# Safety behavior on disconnect
|
||||||
|
disable_on_disconnect: bool = False
|
||||||
|
|
||||||
|
|
||||||
|
@RobotConfig.register_subclass("piper_follower")
|
||||||
|
@dataclass
|
||||||
|
class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase):
|
||||||
|
def __post_init__(self):
|
||||||
|
super().__post_init__()
|
||||||
|
if not (0 <= self.speed_ratio <= 100):
|
||||||
|
raise ValueError("`speed_ratio` must be between 0 and 100.")
|
||||||
|
if self.mode_refresh_interval_s < 0:
|
||||||
|
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||||
|
if self.enable_timeout_s < 0:
|
||||||
|
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||||
|
if self.startup_sleep_s < 0:
|
||||||
|
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||||
|
if not (0 <= self.gripper_effort_default <= 5000):
|
||||||
|
raise ValueError("`gripper_effort_default` must be between 0 and 5000.")
|
||||||
|
if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}:
|
||||||
|
raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.")
|
||||||
@ -0,0 +1,191 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
import logging
|
||||||
|
import time
|
||||||
|
from functools import cached_property
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from lerobot.cameras.utils import make_cameras_from_configs
|
||||||
|
from lerobot.processor import RobotAction, RobotObservation
|
||||||
|
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||||
|
from lerobot.utils.piper_sdk import (
|
||||||
|
PIPER_ACTION_KEYS,
|
||||||
|
PIPER_JOINT_NAMES,
|
||||||
|
get_piper_sdk,
|
||||||
|
milli_to_unit,
|
||||||
|
parse_piper_log_level,
|
||||||
|
unit_to_milli,
|
||||||
|
)
|
||||||
|
|
||||||
|
from ..robot import Robot
|
||||||
|
from .config_piper_follower import PiperFollowerConfig
|
||||||
|
|
||||||
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
|
|
||||||
|
class PiperFollower(Robot):
|
||||||
|
"""Piper follower arm controlled through the Piper SDK (CAN)."""
|
||||||
|
|
||||||
|
config_class = PiperFollowerConfig
|
||||||
|
name = "piper_follower"
|
||||||
|
|
||||||
|
def __init__(self, config: PiperFollowerConfig):
|
||||||
|
super().__init__(config)
|
||||||
|
self.config = config
|
||||||
|
self._is_connected = False
|
||||||
|
self._last_mode_refresh_t = 0.0
|
||||||
|
|
||||||
|
interface_cls, _ = get_piper_sdk()
|
||||||
|
self.arm = interface_cls(
|
||||||
|
can_name=self.config.port,
|
||||||
|
judge_flag=self.config.judge_flag,
|
||||||
|
can_auto_init=self.config.can_auto_init,
|
||||||
|
logger_level=parse_piper_log_level(self.config.log_level),
|
||||||
|
)
|
||||||
|
self.cameras = make_cameras_from_configs(config.cameras)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def _cameras_ft(self) -> dict[str, tuple]:
|
||||||
|
return {
|
||||||
|
cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras
|
||||||
|
}
|
||||||
|
|
||||||
|
@cached_property
|
||||||
|
def observation_features(self) -> dict[str, type | tuple]:
|
||||||
|
return {**{key: float for key in PIPER_ACTION_KEYS}, **self._cameras_ft}
|
||||||
|
|
||||||
|
@cached_property
|
||||||
|
def action_features(self) -> dict[str, type]:
|
||||||
|
return {key: float for key in PIPER_ACTION_KEYS}
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_connected(self) -> bool:
|
||||||
|
return self._is_connected and all(cam.is_connected for cam in self.cameras.values())
|
||||||
|
|
||||||
|
@check_if_already_connected
|
||||||
|
def connect(self, calibrate: bool = True) -> None:
|
||||||
|
del calibrate
|
||||||
|
self.arm.ConnectPort()
|
||||||
|
if self.config.startup_sleep_s > 0:
|
||||||
|
time.sleep(self.config.startup_sleep_s)
|
||||||
|
|
||||||
|
self._is_connected = True
|
||||||
|
try:
|
||||||
|
if self.config.set_slave_mode_on_connect:
|
||||||
|
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||||
|
time.sleep(0.05)
|
||||||
|
|
||||||
|
self.configure()
|
||||||
|
if self.config.enable_on_connect and not self._wait_enable(self.config.enable_timeout_s):
|
||||||
|
logger.warning("Piper follower did not report enabled state before timeout.")
|
||||||
|
|
||||||
|
for cam in self.cameras.values():
|
||||||
|
cam.connect()
|
||||||
|
except Exception:
|
||||||
|
self.arm.DisconnectPort()
|
||||||
|
self._is_connected = False
|
||||||
|
raise
|
||||||
|
|
||||||
|
logger.info("%s connected.", self)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_calibrated(self) -> bool:
|
||||||
|
return True
|
||||||
|
|
||||||
|
def calibrate(self) -> None:
|
||||||
|
logger.info("Piper follower does not require lerobot joint-range calibration. Skipping.")
|
||||||
|
|
||||||
|
def configure(self) -> None:
|
||||||
|
self._send_motion_mode()
|
||||||
|
|
||||||
|
def _send_motion_mode(self) -> None:
|
||||||
|
mit_mode = 0xAD if self.config.high_follow else 0x00
|
||||||
|
self.arm.MotionCtrl_2(0x01, 0x01, self.config.speed_ratio, mit_mode)
|
||||||
|
self._last_mode_refresh_t = time.monotonic()
|
||||||
|
|
||||||
|
def _refresh_motion_mode_if_needed(self) -> None:
|
||||||
|
interval_s = self.config.mode_refresh_interval_s
|
||||||
|
if interval_s <= 0:
|
||||||
|
return
|
||||||
|
now = time.monotonic()
|
||||||
|
if now - self._last_mode_refresh_t >= interval_s:
|
||||||
|
self._send_motion_mode()
|
||||||
|
|
||||||
|
def _wait_enable(self, timeout_s: float) -> bool:
|
||||||
|
deadline = time.monotonic() + timeout_s
|
||||||
|
while time.monotonic() < deadline:
|
||||||
|
if bool(self.arm.EnablePiper()):
|
||||||
|
return True
|
||||||
|
time.sleep(0.02)
|
||||||
|
return False
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def get_observation(self) -> RobotObservation:
|
||||||
|
joint_msg = self.arm.GetArmJointMsgs()
|
||||||
|
joint_state = getattr(joint_msg, "joint_state", None)
|
||||||
|
|
||||||
|
obs: dict[str, float] = {}
|
||||||
|
for joint_name in PIPER_JOINT_NAMES:
|
||||||
|
raw_value = getattr(joint_state, joint_name, 0)
|
||||||
|
obs[f"{joint_name}.pos"] = milli_to_unit(raw_value)
|
||||||
|
|
||||||
|
gripper_msg = self.arm.GetArmGripperMsgs()
|
||||||
|
gripper_state = getattr(gripper_msg, "gripper_state", None)
|
||||||
|
obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||||
|
|
||||||
|
for cam_key, cam in self.cameras.items():
|
||||||
|
obs[cam_key] = cam.async_read()
|
||||||
|
return obs
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def send_action(self, action: RobotAction) -> RobotAction:
|
||||||
|
self._refresh_motion_mode_if_needed()
|
||||||
|
|
||||||
|
sent_action: dict[str, float] = {}
|
||||||
|
|
||||||
|
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||||
|
has_all_joints = all(key in action for key in joint_keys)
|
||||||
|
if has_all_joints:
|
||||||
|
joint_commands = [unit_to_milli(action[key]) for key in joint_keys]
|
||||||
|
self.arm.JointCtrl(*joint_commands)
|
||||||
|
sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)})
|
||||||
|
elif any(key in action for key in joint_keys):
|
||||||
|
logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.")
|
||||||
|
|
||||||
|
if self.config.sync_gripper and "gripper.pos" in action:
|
||||||
|
gripper_pos_raw = unit_to_milli(abs(action["gripper.pos"]))
|
||||||
|
self.arm.GripperCtrl(
|
||||||
|
gripper_pos_raw,
|
||||||
|
self.config.gripper_effort_default,
|
||||||
|
self.config.gripper_status_code,
|
||||||
|
0x00,
|
||||||
|
)
|
||||||
|
sent_action["gripper.pos"] = milli_to_unit(gripper_pos_raw)
|
||||||
|
|
||||||
|
return sent_action
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def disconnect(self) -> None:
|
||||||
|
try:
|
||||||
|
if self.config.disable_on_disconnect:
|
||||||
|
self.arm.DisableArm(7)
|
||||||
|
finally:
|
||||||
|
self.arm.DisconnectPort()
|
||||||
|
for cam in self.cameras.values():
|
||||||
|
cam.disconnect()
|
||||||
|
self._is_connected = False
|
||||||
|
logger.info("%s disconnected.", self)
|
||||||
@ -64,6 +64,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot:
|
|||||||
from .openarm_follower import OpenArmFollower
|
from .openarm_follower import OpenArmFollower
|
||||||
|
|
||||||
return OpenArmFollower(config)
|
return OpenArmFollower(config)
|
||||||
|
elif config.type == "piper_follower":
|
||||||
|
from .piper_follower import PiperFollower
|
||||||
|
|
||||||
|
return PiperFollower(config)
|
||||||
elif config.type == "bi_openarm_follower":
|
elif config.type == "bi_openarm_follower":
|
||||||
from .bi_openarm_follower import BiOpenArmFollower
|
from .bi_openarm_follower import BiOpenArmFollower
|
||||||
|
|
||||||
|
|||||||
@ -44,6 +44,7 @@ from lerobot.robots import ( # noqa: F401
|
|||||||
make_robot_from_config,
|
make_robot_from_config,
|
||||||
omx_follower,
|
omx_follower,
|
||||||
openarm_follower,
|
openarm_follower,
|
||||||
|
piper_follower,
|
||||||
so_follower,
|
so_follower,
|
||||||
)
|
)
|
||||||
from lerobot.teleoperators import ( # noqa: F401
|
from lerobot.teleoperators import ( # noqa: F401
|
||||||
@ -56,6 +57,7 @@ from lerobot.teleoperators import ( # noqa: F401
|
|||||||
make_teleoperator_from_config,
|
make_teleoperator_from_config,
|
||||||
omx_leader,
|
omx_leader,
|
||||||
openarm_leader,
|
openarm_leader,
|
||||||
|
piper_leader,
|
||||||
so_leader,
|
so_leader,
|
||||||
unitree_g1,
|
unitree_g1,
|
||||||
)
|
)
|
||||||
|
|||||||
@ -93,6 +93,7 @@ from lerobot.robots import ( # noqa: F401
|
|||||||
make_robot_from_config,
|
make_robot_from_config,
|
||||||
omx_follower,
|
omx_follower,
|
||||||
openarm_follower,
|
openarm_follower,
|
||||||
|
piper_follower,
|
||||||
reachy2,
|
reachy2,
|
||||||
so_follower,
|
so_follower,
|
||||||
unitree_g1 as unitree_g1_robot,
|
unitree_g1 as unitree_g1_robot,
|
||||||
@ -113,6 +114,7 @@ from lerobot.teleoperators import ( # noqa: F401
|
|||||||
make_teleoperator_from_config,
|
make_teleoperator_from_config,
|
||||||
omx_leader,
|
omx_leader,
|
||||||
openarm_leader,
|
openarm_leader,
|
||||||
|
piper_leader,
|
||||||
reachy2_teleoperator,
|
reachy2_teleoperator,
|
||||||
so_leader,
|
so_leader,
|
||||||
unitree_g1,
|
unitree_g1,
|
||||||
|
|||||||
@ -61,6 +61,7 @@ from lerobot.robots import ( # noqa: F401
|
|||||||
make_robot_from_config,
|
make_robot_from_config,
|
||||||
omx_follower,
|
omx_follower,
|
||||||
openarm_follower,
|
openarm_follower,
|
||||||
|
piper_follower,
|
||||||
reachy2,
|
reachy2,
|
||||||
so_follower,
|
so_follower,
|
||||||
unitree_g1,
|
unitree_g1,
|
||||||
|
|||||||
@ -78,6 +78,7 @@ from lerobot.robots import ( # noqa: F401
|
|||||||
make_robot_from_config,
|
make_robot_from_config,
|
||||||
omx_follower,
|
omx_follower,
|
||||||
openarm_follower,
|
openarm_follower,
|
||||||
|
piper_follower,
|
||||||
reachy2,
|
reachy2,
|
||||||
so_follower,
|
so_follower,
|
||||||
unitree_g1 as unitree_g1_robot,
|
unitree_g1 as unitree_g1_robot,
|
||||||
@ -94,6 +95,7 @@ from lerobot.teleoperators import ( # noqa: F401
|
|||||||
make_teleoperator_from_config,
|
make_teleoperator_from_config,
|
||||||
omx_leader,
|
omx_leader,
|
||||||
openarm_leader,
|
openarm_leader,
|
||||||
|
piper_leader,
|
||||||
reachy2_teleoperator,
|
reachy2_teleoperator,
|
||||||
so_leader,
|
so_leader,
|
||||||
unitree_g1,
|
unitree_g1,
|
||||||
|
|||||||
@ -0,0 +1,18 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
from .config_piper_leader import PiperLeaderConfig, PiperLeaderConfigBase
|
||||||
|
from .piper_leader import PiperLeader
|
||||||
@ -0,0 +1,75 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
from ..config import TeleoperatorConfig
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class PiperLeaderConfigBase:
|
||||||
|
"""Configuration for a Piper leader arm used as teleoperator."""
|
||||||
|
|
||||||
|
# CAN interface name (e.g. "can1")
|
||||||
|
port: str
|
||||||
|
|
||||||
|
# Piper SDK connection options
|
||||||
|
judge_flag: bool = False
|
||||||
|
can_auto_init: bool = True
|
||||||
|
log_level: str = "WARNING"
|
||||||
|
startup_sleep_s: float = 0.1
|
||||||
|
|
||||||
|
# Optional role command on startup (0xFA = teaching input arm)
|
||||||
|
set_leader_mode_on_connect: bool = False
|
||||||
|
|
||||||
|
# Manual backdrivable mode for human teleop
|
||||||
|
manual_control: bool = True
|
||||||
|
|
||||||
|
# Read control messages from leader first, fallback to feedback state if missing
|
||||||
|
prefer_ctrl_messages: bool = True
|
||||||
|
fallback_to_feedback: bool = True
|
||||||
|
|
||||||
|
# Gripper handling
|
||||||
|
sync_gripper: bool = True
|
||||||
|
gripper_effort_default: int = 1000
|
||||||
|
gripper_status_code: int = 0x01
|
||||||
|
|
||||||
|
# Command mode for send_feedback
|
||||||
|
command_speed_ratio: int = 100
|
||||||
|
command_high_follow: bool = True
|
||||||
|
mode_refresh_interval_s: float = 1.0
|
||||||
|
enable_timeout_s: float = 3.0
|
||||||
|
|
||||||
|
# Safety behavior on disconnect
|
||||||
|
disable_on_disconnect: bool = False
|
||||||
|
|
||||||
|
|
||||||
|
@TeleoperatorConfig.register_subclass("piper_leader")
|
||||||
|
@dataclass
|
||||||
|
class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
||||||
|
def __post_init__(self):
|
||||||
|
if not (0 <= self.command_speed_ratio <= 100):
|
||||||
|
raise ValueError("`command_speed_ratio` must be between 0 and 100.")
|
||||||
|
if self.mode_refresh_interval_s < 0:
|
||||||
|
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||||
|
if self.enable_timeout_s < 0:
|
||||||
|
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||||
|
if self.startup_sleep_s < 0:
|
||||||
|
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||||
|
if not (0 <= self.gripper_effort_default <= 5000):
|
||||||
|
raise ValueError("`gripper_effort_default` must be between 0 and 5000.")
|
||||||
|
if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}:
|
||||||
|
raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.")
|
||||||
@ -0,0 +1,220 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
import logging
|
||||||
|
import time
|
||||||
|
from functools import cached_property
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from lerobot.processor import RobotAction
|
||||||
|
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||||
|
from lerobot.utils.piper_sdk import (
|
||||||
|
PIPER_ACTION_KEYS,
|
||||||
|
PIPER_JOINT_NAMES,
|
||||||
|
get_piper_sdk,
|
||||||
|
milli_to_unit,
|
||||||
|
parse_piper_log_level,
|
||||||
|
unit_to_milli,
|
||||||
|
)
|
||||||
|
|
||||||
|
from ..teleoperator import Teleoperator
|
||||||
|
from .config_piper_leader import PiperLeaderConfig
|
||||||
|
|
||||||
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
|
|
||||||
|
class PiperLeader(Teleoperator):
|
||||||
|
"""Piper leader arm used as a teleoperator through Piper SDK CAN messages."""
|
||||||
|
|
||||||
|
config_class = PiperLeaderConfig
|
||||||
|
name = "piper_leader"
|
||||||
|
|
||||||
|
def __init__(self, config: PiperLeaderConfig):
|
||||||
|
super().__init__(config)
|
||||||
|
self.config = config
|
||||||
|
self._is_connected = False
|
||||||
|
self._manual_control_enabled: bool | None = None
|
||||||
|
self._last_mode_refresh_t = 0.0
|
||||||
|
|
||||||
|
interface_cls, _ = get_piper_sdk()
|
||||||
|
self.arm = interface_cls(
|
||||||
|
can_name=self.config.port,
|
||||||
|
judge_flag=self.config.judge_flag,
|
||||||
|
can_auto_init=self.config.can_auto_init,
|
||||||
|
logger_level=parse_piper_log_level(self.config.log_level),
|
||||||
|
)
|
||||||
|
|
||||||
|
@cached_property
|
||||||
|
def action_features(self) -> dict[str, type]:
|
||||||
|
return {key: float for key in PIPER_ACTION_KEYS}
|
||||||
|
|
||||||
|
@cached_property
|
||||||
|
def feedback_features(self) -> dict[str, type]:
|
||||||
|
return {key: float for key in PIPER_ACTION_KEYS}
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_connected(self) -> bool:
|
||||||
|
return self._is_connected
|
||||||
|
|
||||||
|
@check_if_already_connected
|
||||||
|
def connect(self, calibrate: bool = True) -> None:
|
||||||
|
del calibrate
|
||||||
|
self.arm.ConnectPort()
|
||||||
|
if self.config.startup_sleep_s > 0:
|
||||||
|
time.sleep(self.config.startup_sleep_s)
|
||||||
|
|
||||||
|
self._is_connected = True
|
||||||
|
try:
|
||||||
|
if self.config.set_leader_mode_on_connect:
|
||||||
|
self.arm.MasterSlaveConfig(0xFA, 0x00, 0x00, 0x00)
|
||||||
|
time.sleep(0.05)
|
||||||
|
self.configure()
|
||||||
|
except Exception:
|
||||||
|
self.arm.DisconnectPort()
|
||||||
|
self._is_connected = False
|
||||||
|
raise
|
||||||
|
|
||||||
|
logger.info("%s connected.", self)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_calibrated(self) -> bool:
|
||||||
|
return True
|
||||||
|
|
||||||
|
def calibrate(self) -> None:
|
||||||
|
logger.info("Piper leader does not require lerobot joint-range calibration. Skipping.")
|
||||||
|
|
||||||
|
def _send_command_mode(self) -> None:
|
||||||
|
mit_mode = 0xAD if self.config.command_high_follow else 0x00
|
||||||
|
self.arm.MotionCtrl_2(0x01, 0x01, self.config.command_speed_ratio, mit_mode)
|
||||||
|
self._last_mode_refresh_t = time.monotonic()
|
||||||
|
|
||||||
|
def _refresh_command_mode_if_needed(self) -> None:
|
||||||
|
interval_s = self.config.mode_refresh_interval_s
|
||||||
|
if interval_s <= 0:
|
||||||
|
return
|
||||||
|
now = time.monotonic()
|
||||||
|
if now - self._last_mode_refresh_t >= interval_s:
|
||||||
|
self._send_command_mode()
|
||||||
|
|
||||||
|
def _wait_enable(self, timeout_s: float) -> bool:
|
||||||
|
deadline = time.monotonic() + timeout_s
|
||||||
|
while time.monotonic() < deadline:
|
||||||
|
if bool(self.arm.EnablePiper()):
|
||||||
|
return True
|
||||||
|
time.sleep(0.02)
|
||||||
|
return False
|
||||||
|
|
||||||
|
def set_manual_control(self, enabled: bool) -> None:
|
||||||
|
if not self._is_connected:
|
||||||
|
return
|
||||||
|
if enabled and self._manual_control_enabled is not True:
|
||||||
|
self.arm.DisableArm(7)
|
||||||
|
self._manual_control_enabled = True
|
||||||
|
return
|
||||||
|
if not enabled and self._manual_control_enabled is not False:
|
||||||
|
self._send_command_mode()
|
||||||
|
if not self._wait_enable(self.config.enable_timeout_s):
|
||||||
|
logger.warning("Piper leader did not report enabled state before timeout.")
|
||||||
|
self._manual_control_enabled = False
|
||||||
|
|
||||||
|
def configure(self) -> None:
|
||||||
|
self.set_manual_control(self.config.manual_control)
|
||||||
|
|
||||||
|
def _read_joint_from_ctrl(self) -> dict[str, float] | None:
|
||||||
|
joint_ctrl_msg = self.arm.GetArmJointCtrl()
|
||||||
|
if getattr(joint_ctrl_msg, "time_stamp", 0.0) <= 0:
|
||||||
|
return None
|
||||||
|
joint_ctrl = getattr(joint_ctrl_msg, "joint_ctrl", None)
|
||||||
|
return {
|
||||||
|
f"{joint_name}.pos": milli_to_unit(getattr(joint_ctrl, joint_name, 0))
|
||||||
|
for joint_name in PIPER_JOINT_NAMES
|
||||||
|
}
|
||||||
|
|
||||||
|
def _read_joint_from_feedback(self) -> dict[str, float] | None:
|
||||||
|
joint_msg = self.arm.GetArmJointMsgs()
|
||||||
|
joint_state = getattr(joint_msg, "joint_state", None)
|
||||||
|
if joint_state is None:
|
||||||
|
return None
|
||||||
|
return {
|
||||||
|
f"{joint_name}.pos": milli_to_unit(getattr(joint_state, joint_name, 0))
|
||||||
|
for joint_name in PIPER_JOINT_NAMES
|
||||||
|
}
|
||||||
|
|
||||||
|
def _read_gripper_from_ctrl(self) -> float | None:
|
||||||
|
gripper_ctrl_msg = self.arm.GetArmGripperCtrl()
|
||||||
|
if getattr(gripper_ctrl_msg, "time_stamp", 0.0) <= 0:
|
||||||
|
return None
|
||||||
|
gripper_ctrl = getattr(gripper_ctrl_msg, "gripper_ctrl", None)
|
||||||
|
return abs(milli_to_unit(getattr(gripper_ctrl, "grippers_angle", 0)))
|
||||||
|
|
||||||
|
def _read_gripper_from_feedback(self) -> float | None:
|
||||||
|
gripper_msg = self.arm.GetArmGripperMsgs()
|
||||||
|
gripper_state = getattr(gripper_msg, "gripper_state", None)
|
||||||
|
if gripper_state is None:
|
||||||
|
return None
|
||||||
|
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def get_action(self) -> RobotAction:
|
||||||
|
action: dict[str, float] | None = None
|
||||||
|
if self.config.prefer_ctrl_messages:
|
||||||
|
action = self._read_joint_from_ctrl()
|
||||||
|
|
||||||
|
if action is None and self.config.fallback_to_feedback:
|
||||||
|
action = self._read_joint_from_feedback()
|
||||||
|
|
||||||
|
if action is None:
|
||||||
|
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
|
||||||
|
|
||||||
|
if self.config.sync_gripper:
|
||||||
|
gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None
|
||||||
|
if gripper_pos is None and self.config.fallback_to_feedback:
|
||||||
|
gripper_pos = self._read_gripper_from_feedback()
|
||||||
|
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
||||||
|
else:
|
||||||
|
action["gripper.pos"] = 0.0
|
||||||
|
|
||||||
|
return action
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def send_feedback(self, feedback: dict[str, Any]) -> None:
|
||||||
|
self.set_manual_control(False)
|
||||||
|
self._refresh_command_mode_if_needed()
|
||||||
|
|
||||||
|
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||||
|
has_all_joints = all(key in feedback for key in joint_keys)
|
||||||
|
if has_all_joints:
|
||||||
|
joint_commands = [unit_to_milli(feedback[key]) for key in joint_keys]
|
||||||
|
self.arm.JointCtrl(*joint_commands)
|
||||||
|
|
||||||
|
if self.config.sync_gripper and "gripper.pos" in feedback:
|
||||||
|
gripper_pos_raw = unit_to_milli(abs(feedback["gripper.pos"]))
|
||||||
|
self.arm.GripperCtrl(
|
||||||
|
gripper_pos_raw,
|
||||||
|
self.config.gripper_effort_default,
|
||||||
|
self.config.gripper_status_code,
|
||||||
|
0x00,
|
||||||
|
)
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def disconnect(self) -> None:
|
||||||
|
try:
|
||||||
|
if self.config.disable_on_disconnect:
|
||||||
|
self.arm.DisableArm(7)
|
||||||
|
finally:
|
||||||
|
self.arm.DisconnectPort()
|
||||||
|
self._is_connected = False
|
||||||
|
logger.info("%s disconnected.", self)
|
||||||
@ -91,6 +91,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> "Teleoperator":
|
|||||||
from .openarm_leader import OpenArmLeader
|
from .openarm_leader import OpenArmLeader
|
||||||
|
|
||||||
return OpenArmLeader(config)
|
return OpenArmLeader(config)
|
||||||
|
elif config.type == "piper_leader":
|
||||||
|
from .piper_leader import PiperLeader
|
||||||
|
|
||||||
|
return PiperLeader(config)
|
||||||
elif config.type == "bi_openarm_leader":
|
elif config.type == "bi_openarm_leader":
|
||||||
from .bi_openarm_leader import BiOpenArmLeader
|
from .bi_openarm_leader import BiOpenArmLeader
|
||||||
|
|
||||||
|
|||||||
121
src/lerobot/utils/piper_sdk.py
普通文件
121
src/lerobot/utils/piper_sdk.py
普通文件
@ -0,0 +1,121 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
from functools import lru_cache
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
PIPER_JOINT_NAMES = (
|
||||||
|
"joint_1",
|
||||||
|
"joint_2",
|
||||||
|
"joint_3",
|
||||||
|
"joint_4",
|
||||||
|
"joint_5",
|
||||||
|
"joint_6",
|
||||||
|
)
|
||||||
|
PIPER_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) + ("gripper.pos",)
|
||||||
|
|
||||||
|
|
||||||
|
def milli_to_unit(value: float | int) -> float:
|
||||||
|
return float(value) * 1e-3
|
||||||
|
|
||||||
|
|
||||||
|
def unit_to_milli(value: float | int) -> int:
|
||||||
|
return int(round(float(value) * 1e3))
|
||||||
|
|
||||||
|
|
||||||
|
def _iter_candidate_paths() -> list[Path]:
|
||||||
|
repo_root = Path(__file__).resolve().parents[3]
|
||||||
|
env_path = os.environ.get("PIPER_SDK_PATH")
|
||||||
|
candidates: list[Path] = []
|
||||||
|
if env_path:
|
||||||
|
candidates.append(Path(env_path).expanduser())
|
||||||
|
|
||||||
|
candidates.extend(
|
||||||
|
[
|
||||||
|
Path.cwd(),
|
||||||
|
repo_root,
|
||||||
|
repo_root.parent,
|
||||||
|
repo_root / "piper_sdk",
|
||||||
|
repo_root.parent / "piper_sdk",
|
||||||
|
]
|
||||||
|
)
|
||||||
|
return candidates
|
||||||
|
|
||||||
|
|
||||||
|
def _add_path_if_matches(candidate: Path) -> bool:
|
||||||
|
candidate = candidate.resolve()
|
||||||
|
|
||||||
|
package_dir = candidate / "piper_sdk"
|
||||||
|
if package_dir.is_dir() and (package_dir / "__init__.py").is_file():
|
||||||
|
parent = str(candidate)
|
||||||
|
if parent not in sys.path:
|
||||||
|
sys.path.insert(0, parent)
|
||||||
|
return True
|
||||||
|
|
||||||
|
if candidate.name == "piper_sdk" and (candidate / "__init__.py").is_file():
|
||||||
|
parent = str(candidate.parent)
|
||||||
|
if parent not in sys.path:
|
||||||
|
sys.path.insert(0, parent)
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
@lru_cache(maxsize=1)
|
||||||
|
def get_piper_sdk() -> tuple[type[Any], Any]:
|
||||||
|
try:
|
||||||
|
from piper_sdk import C_PiperInterface_V2, LogLevel
|
||||||
|
|
||||||
|
return C_PiperInterface_V2, LogLevel
|
||||||
|
except ModuleNotFoundError:
|
||||||
|
pass
|
||||||
|
|
||||||
|
searched_paths: list[str] = []
|
||||||
|
for candidate in _iter_candidate_paths():
|
||||||
|
searched_paths.append(str(candidate))
|
||||||
|
if not candidate.exists():
|
||||||
|
continue
|
||||||
|
_add_path_if_matches(candidate)
|
||||||
|
try:
|
||||||
|
from piper_sdk import C_PiperInterface_V2, LogLevel
|
||||||
|
|
||||||
|
return C_PiperInterface_V2, LogLevel
|
||||||
|
except ModuleNotFoundError:
|
||||||
|
continue
|
||||||
|
|
||||||
|
searched = "\n- ".join(searched_paths)
|
||||||
|
raise ModuleNotFoundError(
|
||||||
|
"Could not import `piper_sdk`. Install it (for example: `pip install -e /path/to/piper_sdk`) "
|
||||||
|
"or set PIPER_SDK_PATH. Searched paths:\n- "
|
||||||
|
f"{searched}"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def parse_piper_log_level(level_name: str) -> Any:
|
||||||
|
_, log_level_enum = get_piper_sdk()
|
||||||
|
normalized = level_name.upper()
|
||||||
|
try:
|
||||||
|
return getattr(log_level_enum, normalized)
|
||||||
|
except AttributeError as exc:
|
||||||
|
raise ValueError(
|
||||||
|
f"Invalid Piper log level '{level_name}'. "
|
||||||
|
"Expected one of: DEBUG, INFO, WARNING, ERROR, CRITICAL, SILENT."
|
||||||
|
) from exc
|
||||||
175
tests/test_piper_teleop.py
普通文件
175
tests/test_piper_teleop.py
普通文件
@ -0,0 +1,175 @@
|
|||||||
|
from types import SimpleNamespace
|
||||||
|
|
||||||
|
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||||
|
from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig
|
||||||
|
from lerobot.robots.utils import make_robot_from_config
|
||||||
|
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig
|
||||||
|
from lerobot.teleoperators.utils import make_teleoperator_from_config
|
||||||
|
|
||||||
|
|
||||||
|
class FakeLogLevel:
|
||||||
|
DEBUG = "DEBUG"
|
||||||
|
INFO = "INFO"
|
||||||
|
WARNING = "WARNING"
|
||||||
|
ERROR = "ERROR"
|
||||||
|
CRITICAL = "CRITICAL"
|
||||||
|
SILENT = "SILENT"
|
||||||
|
|
||||||
|
|
||||||
|
class FakePiperInterface:
|
||||||
|
def __init__(self, can_name, judge_flag=False, can_auto_init=True, logger_level=None):
|
||||||
|
self.can_name = can_name
|
||||||
|
self.judge_flag = judge_flag
|
||||||
|
self.can_auto_init = can_auto_init
|
||||||
|
self.logger_level = logger_level
|
||||||
|
self.connected = False
|
||||||
|
self.mode_commands = []
|
||||||
|
self.role_commands = []
|
||||||
|
self.last_joint = None
|
||||||
|
self.last_gripper = None
|
||||||
|
self.enable_calls = 0
|
||||||
|
self.disable_calls = 0
|
||||||
|
|
||||||
|
self._joint_ctrl = SimpleNamespace(
|
||||||
|
time_stamp=1.0,
|
||||||
|
Hz=120.0,
|
||||||
|
joint_ctrl=SimpleNamespace(
|
||||||
|
joint_1=10000,
|
||||||
|
joint_2=20000,
|
||||||
|
joint_3=30000,
|
||||||
|
joint_4=40000,
|
||||||
|
joint_5=50000,
|
||||||
|
joint_6=60000,
|
||||||
|
),
|
||||||
|
)
|
||||||
|
self._joint_state = SimpleNamespace(
|
||||||
|
time_stamp=1.0,
|
||||||
|
Hz=120.0,
|
||||||
|
joint_state=SimpleNamespace(
|
||||||
|
joint_1=11000,
|
||||||
|
joint_2=21000,
|
||||||
|
joint_3=31000,
|
||||||
|
joint_4=41000,
|
||||||
|
joint_5=51000,
|
||||||
|
joint_6=61000,
|
||||||
|
),
|
||||||
|
)
|
||||||
|
self._gripper_ctrl = SimpleNamespace(
|
||||||
|
time_stamp=1.0,
|
||||||
|
Hz=120.0,
|
||||||
|
gripper_ctrl=SimpleNamespace(grippers_angle=42000, grippers_effort=1500, status_code=0x01),
|
||||||
|
)
|
||||||
|
self._gripper_state = SimpleNamespace(
|
||||||
|
time_stamp=1.0,
|
||||||
|
Hz=120.0,
|
||||||
|
gripper_state=SimpleNamespace(grippers_angle=43000, grippers_effort=1400, status_code=0x01),
|
||||||
|
)
|
||||||
|
|
||||||
|
def ConnectPort(self):
|
||||||
|
self.connected = True
|
||||||
|
|
||||||
|
def DisconnectPort(self, thread_timeout=0.1):
|
||||||
|
del thread_timeout
|
||||||
|
self.connected = False
|
||||||
|
|
||||||
|
def MotionCtrl_2(self, *args):
|
||||||
|
self.mode_commands.append(args)
|
||||||
|
|
||||||
|
def MasterSlaveConfig(self, *args):
|
||||||
|
self.role_commands.append(args)
|
||||||
|
|
||||||
|
def EnablePiper(self):
|
||||||
|
self.enable_calls += 1
|
||||||
|
return True
|
||||||
|
|
||||||
|
def DisableArm(self, motor_num):
|
||||||
|
del motor_num
|
||||||
|
self.disable_calls += 1
|
||||||
|
|
||||||
|
def JointCtrl(self, *args):
|
||||||
|
self.last_joint = args
|
||||||
|
|
||||||
|
def GripperCtrl(self, *args):
|
||||||
|
self.last_gripper = args
|
||||||
|
|
||||||
|
def GetArmJointCtrl(self):
|
||||||
|
return self._joint_ctrl
|
||||||
|
|
||||||
|
def GetArmJointMsgs(self):
|
||||||
|
return self._joint_state
|
||||||
|
|
||||||
|
def GetArmGripperCtrl(self):
|
||||||
|
return self._gripper_ctrl
|
||||||
|
|
||||||
|
def GetArmGripperMsgs(self):
|
||||||
|
return self._gripper_state
|
||||||
|
|
||||||
|
|
||||||
|
def patch_fake_sdk(monkeypatch):
|
||||||
|
monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", lambda: (FakePiperInterface, FakeLogLevel))
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
teleop_cfg = PiperLeaderConfig(
|
||||||
|
port="can1",
|
||||||
|
set_leader_mode_on_connect=True,
|
||||||
|
manual_control=False,
|
||||||
|
sync_gripper=True,
|
||||||
|
)
|
||||||
|
robot_cfg = PiperFollowerConfig(
|
||||||
|
port="can0",
|
||||||
|
set_slave_mode_on_connect=True,
|
||||||
|
sync_gripper=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
teleop = make_teleoperator_from_config(teleop_cfg)
|
||||||
|
robot = make_robot_from_config(robot_cfg)
|
||||||
|
|
||||||
|
assert isinstance(teleop, PiperLeader)
|
||||||
|
assert isinstance(robot, PiperFollower)
|
||||||
|
|
||||||
|
teleop.connect()
|
||||||
|
robot.connect()
|
||||||
|
try:
|
||||||
|
action = teleop.get_action()
|
||||||
|
sent = robot.send_action(action)
|
||||||
|
obs = robot.get_observation()
|
||||||
|
|
||||||
|
assert robot.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||||
|
assert robot.arm.last_gripper == (
|
||||||
|
42000,
|
||||||
|
robot_cfg.gripper_effort_default,
|
||||||
|
robot_cfg.gripper_status_code,
|
||||||
|
0x00,
|
||||||
|
)
|
||||||
|
assert sent["joint_1.pos"] == 10.0
|
||||||
|
assert sent["gripper.pos"] == 42.0
|
||||||
|
assert obs["joint_1.pos"] == 11.0
|
||||||
|
assert obs["gripper.pos"] == 43.0
|
||||||
|
|
||||||
|
teleop.send_feedback(action)
|
||||||
|
assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||||
|
assert teleop.arm.last_gripper == (
|
||||||
|
42000,
|
||||||
|
teleop_cfg.gripper_effort_default,
|
||||||
|
teleop_cfg.gripper_status_code,
|
||||||
|
0x00,
|
||||||
|
)
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
robot.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_calibration_is_noop(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||||
|
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
||||||
|
|
||||||
|
assert teleop.is_calibrated
|
||||||
|
assert robot.is_calibrated
|
||||||
|
|
||||||
|
teleop.calibrate()
|
||||||
|
robot.calibrate()
|
||||||
在新工单中引用
屏蔽一个用户