镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
feat(piper): switch leader roles for teleop and policy control (#33)
* feat(piper): support read-only PiPER-X leader teleop * trim PiPER-X read-only teleop scope * refactor(piper): switch leader roles via manual control * refactor(piper): remove calibration and gravity assets * simplify(piper): use direct bimanual follower lifecycle * simplify(piper): remove bimanual leader rollback logic * refactor(piper): configure arm roles directly * fix(piper): stabilize PiPER-X leader teleop startup * fix(piper): configure leader gripper drag sensitivity * fix(piper): forward bimanual gripper drag config * revert(piper): remove recent startup workarounds * fix(piper): make leader action reads non-blocking * simplify(piper): remove follower send-only mode * simplify(piper): remove follower mode refresh * simplify(piper): use default leader connection * simplify(piper): streamline leader role switching * simplify(piper): streamline leader action reads * simplify(piper): remove role switch delay * docs(piper): remove role switching details * feat(piper): address USB-CAN adapters by serial --------- Co-authored-by: Elvin-yk <180030703+Elvin-yk@users.noreply.github.com>
这个提交包含在:
2
.gitattributes
vendored
2
.gitattributes
vendored
@ -20,5 +20,3 @@
|
|||||||
tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text
|
tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text
|
||||||
*.bag filter=lfs diff=lfs merge=lfs -text
|
*.bag filter=lfs diff=lfs merge=lfs -text
|
||||||
*.gif filter=lfs diff=lfs merge=lfs -text
|
*.gif filter=lfs diff=lfs merge=lfs -text
|
||||||
src/lerobot/assets/piper_description/** filter=lfs diff=lfs merge=lfs -text
|
|
||||||
src/lerobot/assets/piper_x_description/** filter=lfs diff=lfs merge=lfs -text
|
|
||||||
|
|||||||
@ -1,4 +1,2 @@
|
|||||||
include src/lerobot/templates/lerobot_modelcard_template.md
|
include src/lerobot/templates/lerobot_modelcard_template.md
|
||||||
include src/lerobot/datasets/card_template.md
|
include src/lerobot/datasets/card_template.md
|
||||||
recursive-include src/lerobot/assets/piper_x_description *
|
|
||||||
recursive-include src/lerobot/assets/piper_description *
|
|
||||||
|
|||||||
50
README.md
50
README.md
@ -206,20 +206,22 @@ If needed, you can also use temporary device paths (for example `/dev/ttyACM*` a
|
|||||||
|
|
||||||
#### AgileX (PiPER/PiPER-X)
|
#### AgileX (PiPER/PiPER-X)
|
||||||
|
|
||||||
PiPER arms in master/teaching mode cannot receive external control commands, so all arms must be configured to follower/motion-output mode (0xFC), and firmware must be version 1.8.5 or above.
|
PiPER commands use the USB-CAN adapter's stable `ID_SERIAL_SHORT` instead of a temporary Linux
|
||||||
|
`canN` name. List the connected adapters with:
|
||||||
For PiPER-series robots, make sure Git LFS assets are pulled before running teleoperation:
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
git lfs pull --include="src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**" --exclude="*"
|
for device in /sys/class/net/*; do
|
||||||
git lfs checkout src/lerobot/assets/piper_description src/lerobot/assets/piper_x_description
|
[ "$(cat "$device/type" 2>/dev/null)" = "280" ] || continue
|
||||||
|
serial=$(udevadm info --query=property --path="$device" | sed -n 's/^ID_SERIAL_SHORT=//p')
|
||||||
|
echo "$(basename "$device"): $serial"
|
||||||
|
done
|
||||||
```
|
```
|
||||||
|
|
||||||
For PiPER setup, PiPER uses CAN interfaces instead of serial ports.
|
Then configure the adapters needed for the run:
|
||||||
So first run `lerobot-setup-can` to confirm CAN interfaces are available:
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
lerobot-setup-can --mode=setup --interfaces=<LEFT_FOLLOWER_CAN_PORT>,<LEFT_LEADER_CAN_PORT>,<RIGHT_FOLLOWER_CAN_PORT>,<RIGHT_LEADER_CAN_PORT>
|
lerobot-setup-can --mode=setup \
|
||||||
|
--usb_can_serials=<USB_CAN_SERIAL_1>,<USB_CAN_SERIAL_2>,<USB_CAN_SERIAL_3>,<USB_CAN_SERIAL_4>
|
||||||
```
|
```
|
||||||
|
|
||||||
For single-arm users, run the command below to verify the system is ready:
|
For single-arm users, run the command below to verify the system is ready:
|
||||||
@ -227,13 +229,11 @@ For single-arm users, run the command below to verify the system is ready:
|
|||||||
```bash
|
```bash
|
||||||
lerobot-teleoperate \
|
lerobot-teleoperate \
|
||||||
--robot.type=piperx_follower \
|
--robot.type=piperx_follower \
|
||||||
--robot.port=<FOLLOWER_CAN_PORT> \
|
--robot.port=<FOLLOWER_USB_CAN_SERIAL> \
|
||||||
--robot.id=my_piperx_follower \
|
--robot.id=my_piperx_follower \
|
||||||
--robot.require_calibration=false \
|
|
||||||
--teleop.type=piperx_leader \
|
--teleop.type=piperx_leader \
|
||||||
--teleop.port=<LEADER_CAN_PORT> \
|
--teleop.port=<LEADER_USB_CAN_SERIAL> \
|
||||||
--teleop.id=my_piperx_leader \
|
--teleop.id=my_piperx_leader
|
||||||
--teleop.require_calibration=false
|
|
||||||
```
|
```
|
||||||
|
|
||||||
For bimanual users, run this command to verify dual-arm teleoperation:
|
For bimanual users, run this command to verify dual-arm teleoperation:
|
||||||
@ -242,16 +242,12 @@ For bimanual users, run this command to verify dual-arm teleoperation:
|
|||||||
lerobot-teleoperate \
|
lerobot-teleoperate \
|
||||||
--robot.type=bi_piperx_follower \
|
--robot.type=bi_piperx_follower \
|
||||||
--robot.id=my_bi_piperx_follower \
|
--robot.id=my_bi_piperx_follower \
|
||||||
--robot.left_arm_config.port=<LEFT_FOLLOWER_CAN_PORT> \
|
--robot.left_arm_config.port=<LEFT_FOLLOWER_USB_CAN_SERIAL> \
|
||||||
--robot.right_arm_config.port=<RIGHT_FOLLOWER_CAN_PORT> \
|
--robot.right_arm_config.port=<RIGHT_FOLLOWER_USB_CAN_SERIAL> \
|
||||||
--robot.left_arm_config.require_calibration=false \
|
|
||||||
--robot.right_arm_config.require_calibration=false \
|
|
||||||
--teleop.type=bi_piperx_leader \
|
--teleop.type=bi_piperx_leader \
|
||||||
--teleop.id=my_bi_piperx_leader \
|
--teleop.id=my_bi_piperx_leader \
|
||||||
--teleop.left_arm_config.port=<LEFT_LEADER_CAN_PORT> \
|
--teleop.left_arm_config.port=<LEFT_LEADER_USB_CAN_SERIAL> \
|
||||||
--teleop.right_arm_config.port=<RIGHT_LEADER_CAN_PORT> \
|
--teleop.right_arm_config.port=<RIGHT_LEADER_USB_CAN_SERIAL>
|
||||||
--teleop.left_arm_config.require_calibration=false \
|
|
||||||
--teleop.right_arm_config.require_calibration=false
|
|
||||||
```
|
```
|
||||||
|
|
||||||
For PiPER (non-X), replace `bi_piperx_follower`/`bi_piperx_leader` with `bi_piper_follower`/`bi_piper_leader`.
|
For PiPER (non-X), replace `bi_piperx_follower`/`bi_piperx_leader` with `bi_piper_follower`/`bi_piper_leader`.
|
||||||
@ -297,16 +293,12 @@ Bimanual template (left/right, PiPER-X example):
|
|||||||
lerobot-human-inloop-record \
|
lerobot-human-inloop-record \
|
||||||
--robot.type=bi_piperx_follower \
|
--robot.type=bi_piperx_follower \
|
||||||
--robot.id=my_bi_piperx_follower \
|
--robot.id=my_bi_piperx_follower \
|
||||||
--robot.left_arm_config.port=<LEFT_FOLLOWER_CAN_PORT> \
|
--robot.left_arm_config.port=<LEFT_FOLLOWER_USB_CAN_SERIAL> \
|
||||||
--robot.right_arm_config.port=<RIGHT_FOLLOWER_CAN_PORT> \
|
--robot.right_arm_config.port=<RIGHT_FOLLOWER_USB_CAN_SERIAL> \
|
||||||
--robot.left_arm_config.require_calibration=false \
|
|
||||||
--robot.right_arm_config.require_calibration=false \
|
|
||||||
--teleop.type=bi_piperx_leader \
|
--teleop.type=bi_piperx_leader \
|
||||||
--teleop.id=my_bi_piperx_leader \
|
--teleop.id=my_bi_piperx_leader \
|
||||||
--teleop.left_arm_config.port=<LEFT_LEADER_CAN_PORT> \
|
--teleop.left_arm_config.port=<LEFT_LEADER_USB_CAN_SERIAL> \
|
||||||
--teleop.right_arm_config.port=<RIGHT_LEADER_CAN_PORT> \
|
--teleop.right_arm_config.port=<RIGHT_LEADER_USB_CAN_SERIAL> \
|
||||||
--teleop.left_arm_config.require_calibration=false \
|
|
||||||
--teleop.right_arm_config.require_calibration=false \
|
|
||||||
--dataset.repo_id=<HF_USERNAME_OR_ORG>/<DATASET_NAME> \
|
--dataset.repo_id=<HF_USERNAME_OR_ORG>/<DATASET_NAME> \
|
||||||
--dataset.single_task="<YOUR_TASK_DESCRIPTION>" \
|
--dataset.single_task="<YOUR_TASK_DESCRIPTION>" \
|
||||||
--dataset.num_episodes=<NUM_EPISODES> \
|
--dataset.num_episodes=<NUM_EPISODES> \
|
||||||
|
|||||||
@ -75,7 +75,6 @@ dependencies = [
|
|||||||
"pynput>=1.7.7,<1.9.0",
|
"pynput>=1.7.7,<1.9.0",
|
||||||
"pyserial>=3.5,<4.0",
|
"pyserial>=3.5,<4.0",
|
||||||
"piper_sdk>=0.6.1,<0.7.0",
|
"piper_sdk>=0.6.1,<0.7.0",
|
||||||
"pin>=3.0.0,<4.0.0",
|
|
||||||
"wandb>=0.24.0,<0.25.0",
|
"wandb>=0.24.0,<0.25.0",
|
||||||
|
|
||||||
"torch>=2.2.1,<2.8.0", # TODO: Bumb dependency
|
"torch>=2.2.1,<2.8.0", # TODO: Bumb dependency
|
||||||
@ -222,12 +221,6 @@ lerobot-export-boundary-frames="lerobot.scripts.lerobot_export_boundary_frames:m
|
|||||||
[tool.setuptools.packages.find]
|
[tool.setuptools.packages.find]
|
||||||
where = ["src"]
|
where = ["src"]
|
||||||
|
|
||||||
[tool.setuptools.package-data]
|
|
||||||
lerobot = [
|
|
||||||
"assets/piper_description/**/*",
|
|
||||||
"assets/piper_x_description/**/*",
|
|
||||||
]
|
|
||||||
|
|
||||||
[tool.ruff]
|
[tool.ruff]
|
||||||
target-version = "py310"
|
target-version = "py310"
|
||||||
line-length = 110
|
line-length = 110
|
||||||
|
|||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:fd4a3d6d0266205fe3f725f048645be66592c23564db22e08b0807d5569b2c64
|
|
||||||
size 607384
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:9ee6e1d67c0241f42a0dbaf5c676d19230375ed078ee1943bb8c4c37ab303d14
|
|
||||||
size 447784
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:0d37675b699b643f2e15348de99aed535fac10dc5f09a79b8e4b0acfaf4e0378
|
|
||||||
size 3658384
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:e8ae51f17c0c7f6d67b753922e2cc269f5e8372a65734f859231639a64fe8fad
|
|
||||||
size 1845784
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:efa33022976218f246f736a6133494b5d0e5486e8b50b52900c42f285ae85558
|
|
||||||
size 877984
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:a1c69d1c7f6f8c60e2c82331c0f444725200ed4053d3e5ec23aa82cb5db5b29f
|
|
||||||
size 833684
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:88374973bc3bd9e7389b5853cd6e555af9731c30b658b1ee6f85be34a126cb92
|
|
||||||
size 58884
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:a93c1e3fc245d9d336a427fcc506cb1fc15615268053392b249816f90c011e50
|
|
||||||
size 8273
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:0861e122e406f891a57a550551ad1ce0ef4ca5babf58f476cf8914395e144319
|
|
||||||
size 162084
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:e78dee99bc84857c09183463f3fb3fdea292ea7570f1bfb0001a79674bb69d45
|
|
||||||
size 2323489
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:97e9079580af6e423317c33b2a0f4fe0f6dbc67c3e1d9e6f4836c87c6c8f34a2
|
|
||||||
size 14480060
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:db5b39eef745655a67e57870d8ee56f26a03651d2161635ca2ec3c58c9269907
|
|
||||||
size 5098355
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:ccc08d3ef1cb8bdfb8ab5395ff65893d13386ad526d7bc954d468b7dc885d557
|
|
||||||
size 2037373
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:78718679ffdf9d81130047c4267ed66a081315eff36f216833bc3781c6241417
|
|
||||||
size 2155609
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:486dddcd07f072b2e3978087dc2f9b641a91722bccb1bcb3421c4c7ce64a39ed
|
|
||||||
size 1001764
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
version https://git-lfs.github.com/spec/v1
|
|
||||||
oid sha256:d6dc6d31b0fa56341b2323fee5e8a375c2500ae6fb02e31fb52e8e917b608439
|
|
||||||
size 8169
|
|
||||||
@ -14,7 +14,6 @@
|
|||||||
# See the License for the specific language governing permissions and
|
# See the License for the specific language governing permissions and
|
||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
|
|
||||||
import logging
|
|
||||||
from functools import cached_property
|
from functools import cached_property
|
||||||
|
|
||||||
from lerobot.processor import RobotAction, RobotObservation
|
from lerobot.processor import RobotAction, RobotObservation
|
||||||
@ -29,8 +28,6 @@ from lerobot.utils.decorators import check_if_already_connected, check_if_not_co
|
|||||||
from ..robot import Robot
|
from ..robot import Robot
|
||||||
from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig
|
from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
|
||||||
|
|
||||||
|
|
||||||
class BiPiperFollower(Robot):
|
class BiPiperFollower(Robot):
|
||||||
"""Bimanual PiPER/PiPER-X follower arms."""
|
"""Bimanual PiPER/PiPER-X follower arms."""
|
||||||
@ -45,11 +42,8 @@ class BiPiperFollower(Robot):
|
|||||||
"startup_sleep_s",
|
"startup_sleep_s",
|
||||||
"speed_ratio",
|
"speed_ratio",
|
||||||
"high_follow",
|
"high_follow",
|
||||||
"mode_refresh_interval_s",
|
|
||||||
"enable_on_connect",
|
"enable_on_connect",
|
||||||
"enable_timeout_s",
|
"enable_timeout_s",
|
||||||
"calibration_scale",
|
|
||||||
"require_calibration",
|
|
||||||
"sync_gripper",
|
"sync_gripper",
|
||||||
"gripper_effort_default",
|
"gripper_effort_default",
|
||||||
"gripper_status_code",
|
"gripper_status_code",
|
||||||
@ -60,11 +54,11 @@ class BiPiperFollower(Robot):
|
|||||||
def _build_arm_config(self, arm_config_cls, side_cfg, side: str):
|
def _build_arm_config(self, arm_config_cls, side_cfg, side: str):
|
||||||
kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names}
|
kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names}
|
||||||
kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None
|
kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None
|
||||||
kwargs["calibration_dir"] = self.config.calibration_dir
|
|
||||||
return arm_config_cls(**kwargs)
|
return arm_config_cls(**kwargs)
|
||||||
|
|
||||||
def __init__(self, config: BiPiperFollowerConfig | BiPiperXFollowerConfig):
|
def __init__(self, config: BiPiperFollowerConfig | BiPiperXFollowerConfig):
|
||||||
super().__init__(config)
|
self.robot_type = self.name
|
||||||
|
self.id = config.id
|
||||||
self.config = config
|
self.config = config
|
||||||
|
|
||||||
if config.type == "bi_piperx_follower":
|
if config.type == "bi_piperx_follower":
|
||||||
@ -115,20 +109,16 @@ class BiPiperFollower(Robot):
|
|||||||
|
|
||||||
@check_if_already_connected
|
@check_if_already_connected
|
||||||
def connect(self, calibrate: bool = True) -> None:
|
def connect(self, calibrate: bool = True) -> None:
|
||||||
self.left_arm.connect(calibrate)
|
del calibrate
|
||||||
self.right_arm.connect(calibrate)
|
self.left_arm.connect()
|
||||||
|
self.right_arm.connect()
|
||||||
def set_teleop_send_only_mode(self, enabled: bool) -> None:
|
|
||||||
self.left_arm.set_teleop_send_only_mode(enabled)
|
|
||||||
self.right_arm.set_teleop_send_only_mode(enabled)
|
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
return self.left_arm.is_calibrated and self.right_arm.is_calibrated
|
return True
|
||||||
|
|
||||||
def calibrate(self) -> None:
|
def calibrate(self) -> None:
|
||||||
self.left_arm.calibrate()
|
pass
|
||||||
self.right_arm.calibrate()
|
|
||||||
|
|
||||||
def configure(self) -> None:
|
def configure(self) -> None:
|
||||||
self.left_arm.configure()
|
self.left_arm.configure()
|
||||||
|
|||||||
@ -25,7 +25,7 @@ from ..config import RobotConfig
|
|||||||
class PiperFollowerConfigBase:
|
class PiperFollowerConfigBase:
|
||||||
"""Configuration for a Piper follower arm controlled over CAN."""
|
"""Configuration for a Piper follower arm controlled over CAN."""
|
||||||
|
|
||||||
# CAN interface name (e.g. "can0")
|
# USB-CAN adapter ID_SERIAL_SHORT
|
||||||
port: str
|
port: str
|
||||||
|
|
||||||
# Piper SDK connection options
|
# Piper SDK connection options
|
||||||
@ -37,18 +37,11 @@ class PiperFollowerConfigBase:
|
|||||||
# Motion mode for follower arm
|
# Motion mode for follower arm
|
||||||
speed_ratio: int = 100
|
speed_ratio: int = 100
|
||||||
high_follow: bool = True
|
high_follow: bool = True
|
||||||
mode_refresh_interval_s: float = 1.0
|
|
||||||
|
|
||||||
# Arm enable behavior
|
# Arm enable behavior
|
||||||
enable_on_connect: bool = True
|
enable_on_connect: bool = True
|
||||||
enable_timeout_s: float = 3.0
|
enable_timeout_s: float = 3.0
|
||||||
|
|
||||||
# Calibration precision:
|
|
||||||
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
|
||||||
calibration_scale: int = 1000
|
|
||||||
# Whether calibration is required on connect when no calibration file exists
|
|
||||||
require_calibration: bool = True
|
|
||||||
|
|
||||||
# Gripper forwarding behavior
|
# Gripper forwarding behavior
|
||||||
sync_gripper: bool = True
|
sync_gripper: bool = True
|
||||||
gripper_effort_default: int = 1000
|
gripper_effort_default: int = 1000
|
||||||
@ -64,14 +57,8 @@ class PiperFollowerConfigBase:
|
|||||||
def _validate_piper_follower_config(config: PiperFollowerConfigBase) -> None:
|
def _validate_piper_follower_config(config: PiperFollowerConfigBase) -> None:
|
||||||
if not (0 <= config.speed_ratio <= 100):
|
if not (0 <= config.speed_ratio <= 100):
|
||||||
raise ValueError("`speed_ratio` must be between 0 and 100.")
|
raise ValueError("`speed_ratio` must be between 0 and 100.")
|
||||||
if config.mode_refresh_interval_s < 0:
|
|
||||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
|
||||||
if config.enable_timeout_s < 0:
|
if config.enable_timeout_s < 0:
|
||||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||||
if config.calibration_scale <= 0:
|
|
||||||
raise ValueError("`calibration_scale` must be > 0.")
|
|
||||||
if not isinstance(config.require_calibration, bool):
|
|
||||||
raise ValueError("require_calibration must be true or false.")
|
|
||||||
if config.startup_sleep_s < 0:
|
if config.startup_sleep_s < 0:
|
||||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||||
if not (0 <= config.gripper_effort_default <= 5000):
|
if not (0 <= config.gripper_effort_default <= 5000):
|
||||||
|
|||||||
@ -19,28 +19,26 @@ import time
|
|||||||
from functools import cached_property
|
from functools import cached_property
|
||||||
|
|
||||||
from lerobot.cameras.utils import make_cameras_from_configs
|
from lerobot.cameras.utils import make_cameras_from_configs
|
||||||
from lerobot.motors import MotorCalibration
|
|
||||||
from lerobot.processor import RobotAction, RobotObservation
|
from lerobot.processor import RobotAction, RobotObservation
|
||||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||||
from lerobot.utils.piper_sdk import (
|
from lerobot.utils.piper_sdk import (
|
||||||
PIPER_ACTION_KEYS,
|
PIPER_ACTION_KEYS,
|
||||||
PIPER_JOINT_ACTION_KEYS,
|
PIPER_JOINT_ACTION_KEYS,
|
||||||
PIPER_JOINT_NAMES,
|
PIPER_JOINT_NAMES,
|
||||||
|
PIPER_ROLE_FOLLOWER,
|
||||||
get_piper_sdk,
|
get_piper_sdk,
|
||||||
guard_piper_ctrl_mode_on_connect,
|
|
||||||
milli_to_unit,
|
milli_to_unit,
|
||||||
parse_piper_log_level,
|
parse_piper_log_level,
|
||||||
|
resolve_piper_can_interface,
|
||||||
|
set_piper_role,
|
||||||
unit_to_milli,
|
unit_to_milli,
|
||||||
wait_enable_piper,
|
wait_enable_piper,
|
||||||
)
|
)
|
||||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
|
||||||
|
|
||||||
from ..robot import Robot
|
from ..robot import Robot
|
||||||
from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig
|
from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS)
|
|
||||||
PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)}
|
|
||||||
|
|
||||||
|
|
||||||
class PiperFollower(Robot):
|
class PiperFollower(Robot):
|
||||||
@ -50,15 +48,14 @@ class PiperFollower(Robot):
|
|||||||
name = "piper_follower"
|
name = "piper_follower"
|
||||||
|
|
||||||
def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig):
|
def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig):
|
||||||
super().__init__(config)
|
self.robot_type = self.name
|
||||||
|
self.id = config.id
|
||||||
self.config = config
|
self.config = config
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
self._last_mode_refresh_t = 0.0
|
|
||||||
self._teleop_send_only_mode = False
|
|
||||||
|
|
||||||
interface_cls, _ = get_piper_sdk()
|
interface_cls, _ = get_piper_sdk()
|
||||||
self.arm = interface_cls(
|
self.arm = interface_cls(
|
||||||
can_name=self.config.port,
|
can_name=resolve_piper_can_interface(self.config.port),
|
||||||
judge_flag=self.config.judge_flag,
|
judge_flag=self.config.judge_flag,
|
||||||
can_auto_init=self.config.can_auto_init,
|
can_auto_init=self.config.can_auto_init,
|
||||||
logger_level=parse_piper_log_level(self.config.log_level),
|
logger_level=parse_piper_log_level(self.config.log_level),
|
||||||
@ -85,47 +82,27 @@ class PiperFollower(Robot):
|
|||||||
|
|
||||||
@property
|
@property
|
||||||
def is_connected(self) -> bool:
|
def is_connected(self) -> bool:
|
||||||
return self._is_connected and (
|
return self._is_connected and all(cam.is_connected for cam in self.cameras.values())
|
||||||
self._teleop_send_only_mode or all(cam.is_connected for cam in self.cameras.values())
|
|
||||||
)
|
|
||||||
|
|
||||||
def set_teleop_send_only_mode(self, enabled: bool) -> None:
|
|
||||||
if self._is_connected:
|
|
||||||
raise RuntimeError("teleop send-only mode must be configured before connecting the robot.")
|
|
||||||
self._teleop_send_only_mode = enabled
|
|
||||||
|
|
||||||
@check_if_already_connected
|
@check_if_already_connected
|
||||||
def connect(self, calibrate: bool = True) -> None:
|
def connect(self, calibrate: bool = True) -> None:
|
||||||
self.arm.ConnectPort(start_thread=not self._teleop_send_only_mode)
|
del calibrate
|
||||||
if self.config.startup_sleep_s > 0:
|
self.arm.ConnectPort()
|
||||||
time.sleep(self.config.startup_sleep_s)
|
|
||||||
if not self._teleop_send_only_mode:
|
|
||||||
guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port)
|
|
||||||
|
|
||||||
self._is_connected = True
|
|
||||||
connected_cameras = []
|
connected_cameras = []
|
||||||
try:
|
try:
|
||||||
self.configure()
|
if self.config.startup_sleep_s > 0:
|
||||||
should_auto_calibrate = not self.is_calibrated and calibrate and self.config.require_calibration
|
time.sleep(self.config.startup_sleep_s)
|
||||||
if should_auto_calibrate:
|
|
||||||
logger.info(
|
|
||||||
"No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.",
|
|
||||||
self.id,
|
|
||||||
)
|
|
||||||
self.calibrate()
|
|
||||||
# Enable behavior should be controlled by enable_on_connect, independent from calibrate flag.
|
|
||||||
# This keeps connect(calibrate=False) commandable for callers that only want to skip interactive calibration.
|
|
||||||
should_enable = self.config.enable_on_connect
|
|
||||||
if should_enable:
|
|
||||||
if self._teleop_send_only_mode:
|
|
||||||
self.arm.EnablePiper()
|
|
||||||
elif not self._wait_enable(self.config.enable_timeout_s):
|
|
||||||
logger.warning("Piper follower did not report enabled state before timeout.")
|
|
||||||
|
|
||||||
if not self._teleop_send_only_mode:
|
self._is_connected = True
|
||||||
for cam in self.cameras.values():
|
self.configure()
|
||||||
cam.connect()
|
if self.config.enable_on_connect and not wait_enable_piper(
|
||||||
connected_cameras.append(cam)
|
self.arm, self.config.enable_timeout_s
|
||||||
|
):
|
||||||
|
logger.warning("Piper follower did not report enabled state before timeout.")
|
||||||
|
|
||||||
|
for cam in self.cameras.values():
|
||||||
|
cam.connect()
|
||||||
|
connected_cameras.append(cam)
|
||||||
except Exception:
|
except Exception:
|
||||||
self.arm.DisconnectPort()
|
self.arm.DisconnectPort()
|
||||||
for cam in connected_cameras:
|
for cam in connected_cameras:
|
||||||
@ -135,76 +112,20 @@ class PiperFollower(Robot):
|
|||||||
|
|
||||||
logger.info("%s connected.", self)
|
logger.info("%s connected.", self)
|
||||||
|
|
||||||
def _use_uncalibrated_passthrough(self) -> bool:
|
|
||||||
return not self.is_calibrated and not self.config.require_calibration
|
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
if not all(key in self.calibration for key in PIPER_CALIB_KEYS):
|
|
||||||
return False
|
|
||||||
for key in PIPER_CALIB_KEYS:
|
|
||||||
cal = self.calibration[key]
|
|
||||||
if cal.range_max <= cal.range_min:
|
|
||||||
return False
|
|
||||||
return True
|
return True
|
||||||
|
|
||||||
def calibrate(self) -> None:
|
def calibrate(self) -> None:
|
||||||
if self.calibration and self.is_calibrated:
|
pass
|
||||||
user_input = input(
|
|
||||||
f"Press ENTER to use existing calibration file for id '{self.id}', "
|
|
||||||
"or type 'c' and press ENTER to run a new calibration: "
|
|
||||||
)
|
|
||||||
if user_input.strip().lower() != "c":
|
|
||||||
return
|
|
||||||
|
|
||||||
logger.info("Running calibration for %s", self)
|
|
||||||
# Calibration for Piper follower should be sampled in pure backdrivable mode.
|
|
||||||
self.arm.DisableArm(7)
|
|
||||||
time.sleep(0.1)
|
|
||||||
input("Move piper-follower to your desired neutral/center pose, then press ENTER...")
|
|
||||||
neutral = self._read_raw_observation()
|
|
||||||
print("Move all piper-follower joints through full range. Press ENTER to stop recording...")
|
|
||||||
range_mins, range_maxes = self._record_ranges_of_motion()
|
|
||||||
|
|
||||||
self.calibration = {}
|
|
||||||
for key in PIPER_CALIB_KEYS:
|
|
||||||
min_deg = range_mins[key]
|
|
||||||
max_deg = range_maxes[key]
|
|
||||||
if max_deg <= min_deg:
|
|
||||||
raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}")
|
|
||||||
|
|
||||||
neutral_deg = min(max_deg, max(min_deg, neutral[key]))
|
|
||||||
self.calibration[key] = MotorCalibration(
|
|
||||||
id=PIPER_CALIB_IDS[key],
|
|
||||||
drive_mode=0,
|
|
||||||
homing_offset=self._to_calibration_units(neutral_deg),
|
|
||||||
range_min=self._to_calibration_units(min_deg),
|
|
||||||
range_max=self._to_calibration_units(max_deg),
|
|
||||||
)
|
|
||||||
|
|
||||||
self._save_calibration()
|
|
||||||
print(f"Calibration saved to {self.calibration_fpath}")
|
|
||||||
|
|
||||||
def configure(self) -> None:
|
def configure(self) -> None:
|
||||||
self._send_motion_mode()
|
set_piper_role(self.arm, PIPER_ROLE_FOLLOWER)
|
||||||
|
|
||||||
def _send_motion_mode(self) -> None:
|
|
||||||
mit_mode = 0xAD if self.config.high_follow else 0x00
|
mit_mode = 0xAD if self.config.high_follow else 0x00
|
||||||
self.arm.MotionCtrl_2(0x01, 0x01, self.config.speed_ratio, mit_mode)
|
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:
|
@check_if_not_connected
|
||||||
interval_s = self.config.mode_refresh_interval_s
|
def get_observation(self) -> RobotObservation:
|
||||||
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:
|
|
||||||
return wait_enable_piper(self.arm, timeout_s)
|
|
||||||
|
|
||||||
def _read_raw_observation(self) -> RobotObservation:
|
|
||||||
joint_msg = self.arm.GetArmJointMsgs()
|
joint_msg = self.arm.GetArmJointMsgs()
|
||||||
joint_state = getattr(joint_msg, "joint_state", None)
|
joint_state = getattr(joint_msg, "joint_state", None)
|
||||||
|
|
||||||
@ -216,51 +137,6 @@ class PiperFollower(Robot):
|
|||||||
gripper_msg = self.arm.GetArmGripperMsgs()
|
gripper_msg = self.arm.GetArmGripperMsgs()
|
||||||
gripper_state = getattr(gripper_msg, "gripper_state", None)
|
gripper_state = getattr(gripper_msg, "gripper_state", None)
|
||||||
obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||||
return obs
|
|
||||||
|
|
||||||
def _to_calibration_units(self, angle_deg: float) -> int:
|
|
||||||
return int(round(angle_deg * self.config.calibration_scale))
|
|
||||||
|
|
||||||
def _from_calibration_units(self, value: int) -> float:
|
|
||||||
return float(value) / float(self.config.calibration_scale)
|
|
||||||
|
|
||||||
def _offset_to_target(self, key: str, offset_deg: float) -> float:
|
|
||||||
cal = self.calibration[key]
|
|
||||||
min_deg = self._from_calibration_units(cal.range_min)
|
|
||||||
max_deg = self._from_calibration_units(cal.range_max)
|
|
||||||
home_deg = self._from_calibration_units(cal.homing_offset)
|
|
||||||
centered = -offset_deg if cal.drive_mode else offset_deg
|
|
||||||
target = home_deg + centered
|
|
||||||
return min(max_deg, max(min_deg, target))
|
|
||||||
|
|
||||||
def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]:
|
|
||||||
current = self._read_raw_observation()
|
|
||||||
mins = current.copy()
|
|
||||||
maxes = current.copy()
|
|
||||||
|
|
||||||
while True:
|
|
||||||
current = self._read_raw_observation()
|
|
||||||
mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS}
|
|
||||||
maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS}
|
|
||||||
|
|
||||||
print("\n-----------------------------")
|
|
||||||
print("JOINT | MIN | POS | MAX")
|
|
||||||
for key in PIPER_CALIB_KEYS:
|
|
||||||
print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}")
|
|
||||||
|
|
||||||
if enter_pressed():
|
|
||||||
break
|
|
||||||
move_cursor_up(len(PIPER_CALIB_KEYS) + 3)
|
|
||||||
|
|
||||||
return mins, maxes
|
|
||||||
|
|
||||||
@check_if_not_connected
|
|
||||||
def get_observation(self) -> RobotObservation:
|
|
||||||
if self._teleop_send_only_mode:
|
|
||||||
raise RuntimeError(
|
|
||||||
f"{self} was connected in teleop send-only mode, so follower observations are unavailable."
|
|
||||||
)
|
|
||||||
obs: dict[str, float] = self._read_raw_observation()
|
|
||||||
|
|
||||||
for cam_key, cam in self.cameras.items():
|
for cam_key, cam in self.cameras.items():
|
||||||
obs[cam_key] = cam.async_read()
|
obs[cam_key] = cam.async_read()
|
||||||
@ -268,22 +144,12 @@ class PiperFollower(Robot):
|
|||||||
|
|
||||||
@check_if_not_connected
|
@check_if_not_connected
|
||||||
def send_action(self, action: RobotAction) -> RobotAction:
|
def send_action(self, action: RobotAction) -> RobotAction:
|
||||||
if not self.is_calibrated and not self._use_uncalibrated_passthrough():
|
|
||||||
raise RuntimeError(
|
|
||||||
f"{self} is not calibrated. Run `lerobot-calibrate --robot.type={self.config.type} --robot.id={self.id}` first."
|
|
||||||
)
|
|
||||||
|
|
||||||
self._refresh_motion_mode_if_needed()
|
|
||||||
|
|
||||||
sent_action: dict[str, float] = {}
|
sent_action: dict[str, float] = {}
|
||||||
|
|
||||||
joint_keys = PIPER_JOINT_ACTION_KEYS
|
joint_keys = PIPER_JOINT_ACTION_KEYS
|
||||||
has_all_joints = all(key in action for key in joint_keys)
|
has_all_joints = all(key in action for key in joint_keys)
|
||||||
if has_all_joints:
|
if has_all_joints:
|
||||||
if self._use_uncalibrated_passthrough():
|
joint_targets = [action[key] for key in joint_keys]
|
||||||
joint_targets = [action[key] for key in joint_keys]
|
|
||||||
else:
|
|
||||||
joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys]
|
|
||||||
joint_commands = [unit_to_milli(value) for value in joint_targets]
|
joint_commands = [unit_to_milli(value) for value in joint_targets]
|
||||||
self.arm.JointCtrl(*joint_commands)
|
self.arm.JointCtrl(*joint_commands)
|
||||||
sent_action.update(
|
sent_action.update(
|
||||||
@ -293,11 +159,7 @@ class PiperFollower(Robot):
|
|||||||
logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.")
|
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:
|
if self.config.sync_gripper and "gripper.pos" in action:
|
||||||
if self._use_uncalibrated_passthrough():
|
gripper_pos_raw = unit_to_milli(action["gripper.pos"])
|
||||||
gripper_target = action["gripper.pos"]
|
|
||||||
else:
|
|
||||||
gripper_target = self._offset_to_target("gripper.pos", action["gripper.pos"])
|
|
||||||
gripper_pos_raw = unit_to_milli(gripper_target)
|
|
||||||
self.arm.GripperCtrl(
|
self.arm.GripperCtrl(
|
||||||
gripper_pos_raw,
|
gripper_pos_raw,
|
||||||
self.config.gripper_effort_default,
|
self.config.gripper_effort_default,
|
||||||
@ -316,8 +178,7 @@ class PiperFollower(Robot):
|
|||||||
finally:
|
finally:
|
||||||
self.arm.DisconnectPort()
|
self.arm.DisconnectPort()
|
||||||
for cam in self.cameras.values():
|
for cam in self.cameras.values():
|
||||||
if cam.is_connected:
|
cam.disconnect()
|
||||||
cam.disconnect()
|
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
logger.info("%s disconnected.", self)
|
logger.info("%s disconnected.", self)
|
||||||
|
|
||||||
|
|||||||
@ -13,7 +13,7 @@
|
|||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
|
|
||||||
"""
|
"""
|
||||||
Setup and debug CAN interfaces for Damiao motors (e.g., OpenArms).
|
Setup SocketCAN interfaces and debug Damiao motors (e.g., OpenArms).
|
||||||
|
|
||||||
Examples:
|
Examples:
|
||||||
|
|
||||||
@ -22,6 +22,11 @@ Setup CAN interfaces with CAN FD:
|
|||||||
lerobot-setup-can --mode=setup --interfaces=can0,can1,can2,can3
|
lerobot-setup-can --mode=setup --interfaces=can0,can1,can2,can3
|
||||||
```
|
```
|
||||||
|
|
||||||
|
Setup PiPER interfaces by USB-CAN serial number:
|
||||||
|
```shell
|
||||||
|
lerobot-setup-can --mode=setup --usb_can_serials=<SERIAL_1>,<SERIAL_2>
|
||||||
|
```
|
||||||
|
|
||||||
Test motors on a single interface:
|
Test motors on a single interface:
|
||||||
```shell
|
```shell
|
||||||
lerobot-setup-can --mode=test --interfaces=can0
|
lerobot-setup-can --mode=test --interfaces=can0
|
||||||
@ -46,6 +51,7 @@ from dataclasses import dataclass, field
|
|||||||
import draccus
|
import draccus
|
||||||
|
|
||||||
from lerobot.utils.import_utils import _can_available
|
from lerobot.utils.import_utils import _can_available
|
||||||
|
from lerobot.utils.piper_sdk import resolve_piper_can_interface
|
||||||
|
|
||||||
MOTOR_NAMES = {
|
MOTOR_NAMES = {
|
||||||
0x01: "joint_1",
|
0x01: "joint_1",
|
||||||
@ -63,6 +69,7 @@ MOTOR_NAMES = {
|
|||||||
class CANSetupConfig:
|
class CANSetupConfig:
|
||||||
mode: str = "test"
|
mode: str = "test"
|
||||||
interfaces: str = "can0" # Comma-separated, e.g. "can0,can1,can2,can3"
|
interfaces: str = "can0" # Comma-separated, e.g. "can0,can1,can2,can3"
|
||||||
|
usb_can_serials: str | None = None
|
||||||
bitrate: int = 1000000
|
bitrate: int = 1000000
|
||||||
data_bitrate: int = 5000000
|
data_bitrate: int = 5000000
|
||||||
use_fd: bool = True
|
use_fd: bool = True
|
||||||
@ -71,6 +78,13 @@ class CANSetupConfig:
|
|||||||
speed_iterations: int = 100
|
speed_iterations: int = 100
|
||||||
|
|
||||||
def get_interfaces(self) -> list[str]:
|
def get_interfaces(self) -> list[str]:
|
||||||
|
if self.usb_can_serials is not None:
|
||||||
|
if self.mode != "setup":
|
||||||
|
raise ValueError("`usb_can_serials` is only supported with `mode=setup`.")
|
||||||
|
serials = [value.strip() for value in self.usb_can_serials.split(",") if value.strip()]
|
||||||
|
if not serials:
|
||||||
|
raise ValueError("`usb_can_serials` must contain at least one serial number.")
|
||||||
|
return [resolve_piper_can_interface(serial) for serial in serials]
|
||||||
return [i.strip() for i in self.interfaces.split(",") if i.strip()]
|
return [i.strip() for i in self.interfaces.split(",") if i.strip()]
|
||||||
|
|
||||||
|
|
||||||
@ -121,13 +135,15 @@ def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: boo
|
|||||||
|
|
||||||
def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[bool, bool | None, str]:
|
def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[bool, bool | None, str]:
|
||||||
"""Try primary mode first, then fallback mode. Returns (success, used_fd, summary)."""
|
"""Try primary mode first, then fallback mode. Returns (success, used_fd, summary)."""
|
||||||
attempts = [cfg.use_fd, not cfg.use_fd]
|
is_piper = cfg.usb_can_serials is not None
|
||||||
|
attempts = [False] if is_piper else [cfg.use_fd, not cfg.use_fd]
|
||||||
|
bitrate = 1000000 if is_piper else cfg.bitrate
|
||||||
labels = {True: "CAN FD", False: "CAN 2.0"}
|
labels = {True: "CAN FD", False: "CAN 2.0"}
|
||||||
attempt_msgs: list[str] = []
|
attempt_msgs: list[str] = []
|
||||||
|
|
||||||
for idx, use_fd in enumerate(attempts, start=1):
|
for idx, use_fd in enumerate(attempts, start=1):
|
||||||
print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}")
|
print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}")
|
||||||
ok, err = setup_interface(interface, cfg.bitrate, cfg.data_bitrate, use_fd)
|
ok, err = setup_interface(interface, bitrate, cfg.data_bitrate, use_fd)
|
||||||
if ok:
|
if ok:
|
||||||
is_up, status, is_fd_iface = check_interface_status(interface)
|
is_up, status, is_fd_iface = check_interface_status(interface)
|
||||||
if is_up:
|
if is_up:
|
||||||
@ -305,13 +321,16 @@ def speed_test(cfg: CANSetupConfig, interface: str):
|
|||||||
|
|
||||||
def run_setup(cfg: CANSetupConfig) -> bool:
|
def run_setup(cfg: CANSetupConfig) -> bool:
|
||||||
"""Setup CAN interfaces."""
|
"""Setup CAN interfaces."""
|
||||||
|
use_fd = False if cfg.usb_can_serials is not None else cfg.use_fd
|
||||||
|
bitrate = 1000000 if cfg.usb_can_serials is not None else cfg.bitrate
|
||||||
print("=" * 50)
|
print("=" * 50)
|
||||||
print("CAN Interface Setup")
|
print("CAN Interface Setup")
|
||||||
print("=" * 50)
|
print("=" * 50)
|
||||||
print(f"Primary mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}")
|
print(f"Primary mode: {'CAN FD' if use_fd else 'CAN 2.0'}")
|
||||||
print(f"Fallback mode: {'CAN 2.0' if cfg.use_fd else 'CAN FD'}")
|
if cfg.usb_can_serials is None:
|
||||||
print(f"Bitrate: {cfg.bitrate / 1_000_000:.1f} Mbps")
|
print(f"Fallback mode: {'CAN 2.0' if use_fd else 'CAN FD'}")
|
||||||
if cfg.use_fd:
|
print(f"Bitrate: {bitrate / 1_000_000:.1f} Mbps")
|
||||||
|
if use_fd:
|
||||||
print(f"Data bitrate: {cfg.data_bitrate / 1_000_000:.1f} Mbps")
|
print(f"Data bitrate: {cfg.data_bitrate / 1_000_000:.1f} Mbps")
|
||||||
print()
|
print()
|
||||||
|
|
||||||
@ -351,9 +370,10 @@ def run_setup(cfg: CANSetupConfig) -> bool:
|
|||||||
|
|
||||||
print()
|
print()
|
||||||
print("✓ Setup succeeded")
|
print("✓ Setup succeeded")
|
||||||
print()
|
if cfg.usb_can_serials is None:
|
||||||
print("Next: Test motors with:")
|
print()
|
||||||
print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}")
|
print("Next: Test motors with:")
|
||||||
|
print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -173,10 +173,7 @@ def record_loop(
|
|||||||
return
|
return
|
||||||
if not hasattr(teleop_arm_for_mode_switch, "set_manual_control"):
|
if not hasattr(teleop_arm_for_mode_switch, "set_manual_control"):
|
||||||
return
|
return
|
||||||
try:
|
teleop_arm_for_mode_switch.set_manual_control(enabled)
|
||||||
teleop_arm_for_mode_switch.set_manual_control(enabled)
|
|
||||||
except Exception:
|
|
||||||
logging.exception("Failed to switch teleop manual-control mode to %s", enabled)
|
|
||||||
|
|
||||||
if policy is None:
|
if policy is None:
|
||||||
# During reset/teleop-only loops keep leader backdrivable for manual dragging.
|
# During reset/teleop-only loops keep leader backdrivable for manual dragging.
|
||||||
@ -251,12 +248,12 @@ def record_loop(
|
|||||||
events["toggle_intervention"] = False
|
events["toggle_intervention"] = False
|
||||||
if intervention_enabled:
|
if intervention_enabled:
|
||||||
if intervention_state == INTERVENTION_STATE_POLICY:
|
if intervention_state == INTERVENTION_STATE_POLICY:
|
||||||
intervention_state = INTERVENTION_STATE_ACTIVE
|
|
||||||
set_teleop_manual_control(True)
|
set_teleop_manual_control(True)
|
||||||
|
intervention_state = INTERVENTION_STATE_ACTIVE
|
||||||
logging.info("Intervention enabled (S1): teleop actions now override policy execution.")
|
logging.info("Intervention enabled (S1): teleop actions now override policy execution.")
|
||||||
else:
|
else:
|
||||||
intervention_state = INTERVENTION_STATE_RELEASE
|
|
||||||
set_teleop_manual_control(False)
|
set_teleop_manual_control(False)
|
||||||
|
intervention_state = INTERVENTION_STATE_RELEASE
|
||||||
if policy is not None and preprocessor is not None and postprocessor is not None:
|
if policy is not None and preprocessor is not None and postprocessor is not None:
|
||||||
policy.reset()
|
policy.reset()
|
||||||
preprocessor.reset()
|
preprocessor.reset()
|
||||||
|
|||||||
@ -14,9 +14,9 @@
|
|||||||
# See the License for the specific language governing permissions and
|
# See the License for the specific language governing permissions and
|
||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
|
|
||||||
import logging
|
|
||||||
import multiprocessing as mp
|
import multiprocessing as mp
|
||||||
import traceback
|
import traceback
|
||||||
|
from contextlib import suppress
|
||||||
from functools import cached_property
|
from functools import cached_property
|
||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
@ -27,14 +27,12 @@ from lerobot.teleoperators.piper_leader import (
|
|||||||
PiperXLeader,
|
PiperXLeader,
|
||||||
PiperXLeaderConfig,
|
PiperXLeaderConfig,
|
||||||
)
|
)
|
||||||
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
|
||||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||||
|
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
||||||
|
|
||||||
from ..teleoperator import Teleoperator
|
from ..teleoperator import Teleoperator
|
||||||
from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig
|
from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
|
||||||
|
|
||||||
|
|
||||||
def _bi_piper_leader_worker(conn, arm_cls, arm_config) -> None:
|
def _bi_piper_leader_worker(conn, arm_cls, arm_config) -> None:
|
||||||
arm = arm_cls(arm_config)
|
arm = arm_cls(arm_config)
|
||||||
@ -46,12 +44,9 @@ def _bi_piper_leader_worker(conn, arm_cls, arm_config) -> None:
|
|||||||
break
|
break
|
||||||
|
|
||||||
try:
|
try:
|
||||||
if command == "__get_is_calibrated__":
|
args = request.get("args", ())
|
||||||
result = arm.is_calibrated
|
kwargs = request.get("kwargs", {})
|
||||||
else:
|
result = getattr(arm, command)(*args, **kwargs)
|
||||||
args = request.get("args", ())
|
|
||||||
kwargs = request.get("kwargs", {})
|
|
||||||
result = getattr(arm, command)(*args, **kwargs)
|
|
||||||
conn.send({"ok": True, "result": result})
|
conn.send({"ok": True, "result": result})
|
||||||
except Exception as exc: # noqa: BLE001
|
except Exception as exc: # noqa: BLE001
|
||||||
conn.send(
|
conn.send(
|
||||||
@ -91,12 +86,6 @@ class _PiperLeaderProcessProxy:
|
|||||||
def is_connected(self) -> bool:
|
def is_connected(self) -> bool:
|
||||||
return self._is_connected
|
return self._is_connected
|
||||||
|
|
||||||
@property
|
|
||||||
def is_calibrated(self) -> bool:
|
|
||||||
if self._process is None:
|
|
||||||
return False
|
|
||||||
return bool(self._call("__get_is_calibrated__"))
|
|
||||||
|
|
||||||
def _ensure_process(self) -> None:
|
def _ensure_process(self) -> None:
|
||||||
if self._process is not None and self._process.is_alive():
|
if self._process is not None and self._process.is_alive():
|
||||||
return
|
return
|
||||||
@ -123,17 +112,14 @@ class _PiperLeaderProcessProxy:
|
|||||||
f"{response['traceback']}"
|
f"{response['traceback']}"
|
||||||
)
|
)
|
||||||
|
|
||||||
def connect(self, calibrate: bool = True) -> None:
|
def connect(self) -> None:
|
||||||
try:
|
try:
|
||||||
self._call("connect", calibrate)
|
self._call("connect")
|
||||||
self._is_connected = True
|
self._is_connected = True
|
||||||
except Exception:
|
except Exception:
|
||||||
self.disconnect()
|
self.disconnect()
|
||||||
raise
|
raise
|
||||||
|
|
||||||
def calibrate(self) -> None:
|
|
||||||
self._call("calibrate")
|
|
||||||
|
|
||||||
def configure(self) -> None:
|
def configure(self) -> None:
|
||||||
self._call("configure")
|
self._call("configure")
|
||||||
|
|
||||||
@ -155,19 +141,13 @@ class _PiperLeaderProcessProxy:
|
|||||||
return
|
return
|
||||||
|
|
||||||
if self._parent_conn is not None:
|
if self._parent_conn is not None:
|
||||||
try:
|
with suppress(Exception):
|
||||||
if self._is_connected:
|
if self._is_connected:
|
||||||
self._call("disconnect")
|
self._call("disconnect")
|
||||||
except Exception:
|
with suppress(Exception):
|
||||||
pass
|
|
||||||
try:
|
|
||||||
self._parent_conn.send({"command": "__close__"})
|
self._parent_conn.send({"command": "__close__"})
|
||||||
except Exception:
|
with suppress(Exception):
|
||||||
pass
|
|
||||||
try:
|
|
||||||
self._parent_conn.close()
|
self._parent_conn.close()
|
||||||
except Exception:
|
|
||||||
pass
|
|
||||||
|
|
||||||
if self._process.is_alive():
|
if self._process.is_alive():
|
||||||
self._process.join(timeout=2.0)
|
self._process.join(timeout=2.0)
|
||||||
@ -192,8 +172,6 @@ class BiPiperLeader(Teleoperator):
|
|||||||
"log_level",
|
"log_level",
|
||||||
"startup_sleep_s",
|
"startup_sleep_s",
|
||||||
"manual_control",
|
"manual_control",
|
||||||
"prefer_ctrl_messages",
|
|
||||||
"fallback_to_feedback",
|
|
||||||
"sync_gripper",
|
"sync_gripper",
|
||||||
"gripper_effort_default",
|
"gripper_effort_default",
|
||||||
"gripper_status_code",
|
"gripper_status_code",
|
||||||
@ -201,25 +179,16 @@ class BiPiperLeader(Teleoperator):
|
|||||||
"command_high_follow",
|
"command_high_follow",
|
||||||
"mode_refresh_interval_s",
|
"mode_refresh_interval_s",
|
||||||
"enable_timeout_s",
|
"enable_timeout_s",
|
||||||
"gravity_comp_control_hz",
|
|
||||||
"gravity_comp_tx_ratio",
|
|
||||||
"gravity_comp_torque_limit",
|
|
||||||
"gravity_comp_mit_kp",
|
|
||||||
"gravity_comp_mit_kd",
|
|
||||||
"gravity_comp_base_rpy_deg",
|
|
||||||
"calibration_scale",
|
|
||||||
"require_calibration",
|
|
||||||
"disable_on_disconnect",
|
"disable_on_disconnect",
|
||||||
)
|
)
|
||||||
|
|
||||||
def _build_arm_config(self, arm_config_cls, side_cfg, side: str):
|
def _build_arm_config(self, arm_config_cls, side_cfg, side: str):
|
||||||
kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names}
|
kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names}
|
||||||
kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None
|
kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None
|
||||||
kwargs["calibration_dir"] = self.config.calibration_dir
|
|
||||||
return arm_config_cls(**kwargs)
|
return arm_config_cls(**kwargs)
|
||||||
|
|
||||||
def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig):
|
def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig):
|
||||||
super().__init__(config)
|
self.id = config.id
|
||||||
self.config = config
|
self.config = config
|
||||||
self._use_process_isolation = config.process_isolation
|
self._use_process_isolation = config.process_isolation
|
||||||
|
|
||||||
@ -259,16 +228,16 @@ class BiPiperLeader(Teleoperator):
|
|||||||
|
|
||||||
@check_if_already_connected
|
@check_if_already_connected
|
||||||
def connect(self, calibrate: bool = True) -> None:
|
def connect(self, calibrate: bool = True) -> None:
|
||||||
self.left_arm.connect(calibrate)
|
del calibrate
|
||||||
self.right_arm.connect(calibrate)
|
self.left_arm.connect()
|
||||||
|
self.right_arm.connect()
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
return self.left_arm.is_calibrated and self.right_arm.is_calibrated
|
return True
|
||||||
|
|
||||||
def calibrate(self) -> None:
|
def calibrate(self) -> None:
|
||||||
self.left_arm.calibrate()
|
pass
|
||||||
self.right_arm.calibrate()
|
|
||||||
|
|
||||||
def configure(self) -> None:
|
def configure(self) -> None:
|
||||||
self.left_arm.configure()
|
self.left_arm.configure()
|
||||||
|
|||||||
@ -18,15 +18,12 @@ from dataclasses import dataclass
|
|||||||
|
|
||||||
from ..config import TeleoperatorConfig
|
from ..config import TeleoperatorConfig
|
||||||
|
|
||||||
DEFAULT_PIPER_GRAVITY_COMP_TX_RATIO = (0.2, 0.2, 0.2, 0.2, 0.2, 0.2)
|
|
||||||
DEFAULT_PIPERX_GRAVITY_COMP_TX_RATIO = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class PiperLeaderConfigBase:
|
class PiperLeaderConfigBase:
|
||||||
"""Configuration for a Piper leader arm used as teleoperator."""
|
"""Configuration for a Piper leader arm used as teleoperator."""
|
||||||
|
|
||||||
# CAN interface name (e.g. "can1")
|
# USB-CAN adapter ID_SERIAL_SHORT
|
||||||
port: str
|
port: str
|
||||||
|
|
||||||
# Piper SDK connection options
|
# Piper SDK connection options
|
||||||
@ -35,13 +32,9 @@ class PiperLeaderConfigBase:
|
|||||||
log_level: str = "WARNING"
|
log_level: str = "WARNING"
|
||||||
startup_sleep_s: float = 0.1
|
startup_sleep_s: float = 0.1
|
||||||
|
|
||||||
# Manual backdrivable mode for human teleop
|
# Initial role: True uses hardware leader drag (0xFA); False accepts policy commands (0xFC).
|
||||||
manual_control: bool = True
|
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
|
# Gripper handling
|
||||||
sync_gripper: bool = True
|
sync_gripper: bool = True
|
||||||
gripper_effort_default: int = 1000
|
gripper_effort_default: int = 1000
|
||||||
@ -53,20 +46,6 @@ class PiperLeaderConfigBase:
|
|||||||
mode_refresh_interval_s: float = 1.0
|
mode_refresh_interval_s: float = 1.0
|
||||||
enable_timeout_s: float = 3.0
|
enable_timeout_s: float = 3.0
|
||||||
|
|
||||||
# Gravity compensation settings (used when manual_control=true)
|
|
||||||
gravity_comp_control_hz: float = 200.0
|
|
||||||
gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = DEFAULT_PIPER_GRAVITY_COMP_TX_RATIO
|
|
||||||
gravity_comp_torque_limit: float = 8.0
|
|
||||||
gravity_comp_mit_kp: float = 0.0
|
|
||||||
gravity_comp_mit_kd: float = 0.0
|
|
||||||
gravity_comp_base_rpy_deg: tuple[float, float, float] = (0.0, 0.0, 0.0)
|
|
||||||
|
|
||||||
# Calibration precision:
|
|
||||||
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
|
||||||
calibration_scale: int = 1000
|
|
||||||
# Whether calibration is required on connect when no calibration file exists
|
|
||||||
require_calibration: bool = True
|
|
||||||
|
|
||||||
# Safety behavior on disconnect
|
# Safety behavior on disconnect
|
||||||
disable_on_disconnect: bool = False
|
disable_on_disconnect: bool = False
|
||||||
|
|
||||||
@ -78,18 +57,6 @@ def _validate_piper_leader_config(config: PiperLeaderConfigBase) -> None:
|
|||||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||||
if config.enable_timeout_s < 0:
|
if config.enable_timeout_s < 0:
|
||||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||||
if config.gravity_comp_control_hz <= 0:
|
|
||||||
raise ValueError("`gravity_comp_control_hz` must be > 0.")
|
|
||||||
if len(config.gravity_comp_tx_ratio) != 6:
|
|
||||||
raise ValueError("`gravity_comp_tx_ratio` must contain exactly 6 values.")
|
|
||||||
if config.gravity_comp_torque_limit <= 0:
|
|
||||||
raise ValueError("`gravity_comp_torque_limit` must be > 0.")
|
|
||||||
if len(config.gravity_comp_base_rpy_deg) != 3:
|
|
||||||
raise ValueError("`gravity_comp_base_rpy_deg` must contain exactly 3 values.")
|
|
||||||
if config.calibration_scale <= 0:
|
|
||||||
raise ValueError("`calibration_scale` must be > 0.")
|
|
||||||
if not isinstance(config.require_calibration, bool):
|
|
||||||
raise ValueError("require_calibration must be true or false.")
|
|
||||||
if config.startup_sleep_s < 0:
|
if config.startup_sleep_s < 0:
|
||||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||||
if not (0 <= config.gripper_effort_default <= 5000):
|
if not (0 <= config.gripper_effort_default <= 5000):
|
||||||
@ -107,7 +74,7 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
|||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class PiperXLeaderConfigBase(PiperLeaderConfigBase):
|
class PiperXLeaderConfigBase(PiperLeaderConfigBase):
|
||||||
gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = DEFAULT_PIPERX_GRAVITY_COMP_TX_RATIO
|
pass
|
||||||
|
|
||||||
|
|
||||||
@TeleoperatorConfig.register_subclass("piperx_leader")
|
@TeleoperatorConfig.register_subclass("piperx_leader")
|
||||||
|
|||||||
@ -1,192 +0,0 @@
|
|||||||
#!/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 logging
|
|
||||||
import math
|
|
||||||
import threading
|
|
||||||
import time
|
|
||||||
from pathlib import Path
|
|
||||||
from typing import Any
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
|
||||||
|
|
||||||
|
|
||||||
def _rpy_deg_to_rot(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray:
|
|
||||||
roll = math.radians(roll_deg)
|
|
||||||
pitch = math.radians(pitch_deg)
|
|
||||||
yaw = math.radians(yaw_deg)
|
|
||||||
|
|
||||||
cx, sx = math.cos(roll), math.sin(roll)
|
|
||||||
cy, sy = math.cos(pitch), math.sin(pitch)
|
|
||||||
cz, sz = math.cos(yaw), math.sin(yaw)
|
|
||||||
|
|
||||||
rot_x = np.array([[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]], dtype=np.float64)
|
|
||||||
rot_y = np.array([[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]], dtype=np.float64)
|
|
||||||
rot_z = np.array([[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64)
|
|
||||||
return rot_z @ rot_y @ rot_x
|
|
||||||
|
|
||||||
|
|
||||||
class PiperGravityCompensationLoop:
|
|
||||||
"""Background MIT torque loop for gravity compensation on Piper leader arm."""
|
|
||||||
|
|
||||||
def __init__(
|
|
||||||
self,
|
|
||||||
*,
|
|
||||||
arm: Any,
|
|
||||||
urdf_path: str,
|
|
||||||
control_hz: float,
|
|
||||||
tx_ratio: tuple[float, float, float, float, float, float],
|
|
||||||
torque_limit: float,
|
|
||||||
mit_kp: float,
|
|
||||||
mit_kd: float,
|
|
||||||
base_rpy_deg: tuple[float, float, float],
|
|
||||||
mode_refresh_interval_s: float,
|
|
||||||
move_speed_ratio: int,
|
|
||||||
) -> None:
|
|
||||||
import pinocchio as pin
|
|
||||||
|
|
||||||
self._pin = pin
|
|
||||||
self._arm = arm
|
|
||||||
self._control_hz = control_hz
|
|
||||||
self._tx_ratio = np.asarray(tx_ratio, dtype=np.float64)
|
|
||||||
self._torque_limit = float(torque_limit)
|
|
||||||
self._mit_kp = float(mit_kp)
|
|
||||||
self._mit_kd = float(mit_kd)
|
|
||||||
self._mode_refresh_interval_s = max(0.0, float(mode_refresh_interval_s))
|
|
||||||
self._move_speed_ratio = int(move_speed_ratio)
|
|
||||||
|
|
||||||
urdf = Path(urdf_path).expanduser().resolve()
|
|
||||||
if not urdf.is_file():
|
|
||||||
raise FileNotFoundError(f"gravity compensation URDF does not exist: {urdf}")
|
|
||||||
|
|
||||||
package_dirs = [str(urdf.parent.parent)]
|
|
||||||
self._robot = pin.RobotWrapper.BuildFromURDF(str(urdf), package_dirs)
|
|
||||||
self._robot.data = self._robot.model.createData()
|
|
||||||
self._nq = 6
|
|
||||||
|
|
||||||
rot_world_base = _rpy_deg_to_rot(*base_rpy_deg)
|
|
||||||
gravity_world = np.array([0.0, 0.0, -9.81], dtype=np.float64)
|
|
||||||
gravity_base = rot_world_base.T @ gravity_world
|
|
||||||
self._robot.model.gravity.linear = gravity_base
|
|
||||||
|
|
||||||
self._thread: threading.Thread | None = None
|
|
||||||
self._stop_event = threading.Event()
|
|
||||||
self._last_mode_refresh_t = 0.0
|
|
||||||
|
|
||||||
def start(self) -> None:
|
|
||||||
if self._thread is not None and self._thread.is_alive():
|
|
||||||
return
|
|
||||||
self._stop_event.clear()
|
|
||||||
self._thread = threading.Thread(
|
|
||||||
target=self._run,
|
|
||||||
name="piper-gravity-comp-loop",
|
|
||||||
daemon=True,
|
|
||||||
)
|
|
||||||
self._thread.start()
|
|
||||||
|
|
||||||
def stop(self) -> None:
|
|
||||||
self._stop_event.set()
|
|
||||||
if self._thread is None:
|
|
||||||
return
|
|
||||||
self._thread.join(timeout=1.0)
|
|
||||||
if self._thread.is_alive():
|
|
||||||
logger.warning("Piper gravity compensation thread did not stop within timeout.")
|
|
||||||
self._thread = None
|
|
||||||
|
|
||||||
def _send_mit_mode(self) -> None:
|
|
||||||
# Use MOVE M + MIT to ensure torque pass-through mode is active.
|
|
||||||
self._arm.MotionCtrl_2(0x01, 0x04, self._move_speed_ratio, 0xAD)
|
|
||||||
self._last_mode_refresh_t = time.monotonic()
|
|
||||||
|
|
||||||
def _refresh_mit_mode_if_needed(self) -> None:
|
|
||||||
if self._mode_refresh_interval_s <= 0.0:
|
|
||||||
return
|
|
||||||
now = time.monotonic()
|
|
||||||
if now - self._last_mode_refresh_t >= self._mode_refresh_interval_s:
|
|
||||||
self._send_mit_mode()
|
|
||||||
|
|
||||||
def _read_q_v(self) -> tuple[np.ndarray, np.ndarray]:
|
|
||||||
joint_msg = self._arm.GetArmJointMsgs()
|
|
||||||
joint_state = joint_msg.joint_state
|
|
||||||
q_deg = np.array(
|
|
||||||
[
|
|
||||||
float(joint_state.joint_1) * 1e-3,
|
|
||||||
float(joint_state.joint_2) * 1e-3,
|
|
||||||
float(joint_state.joint_3) * 1e-3,
|
|
||||||
float(joint_state.joint_4) * 1e-3,
|
|
||||||
float(joint_state.joint_5) * 1e-3,
|
|
||||||
float(joint_state.joint_6) * 1e-3,
|
|
||||||
],
|
|
||||||
dtype=np.float64,
|
|
||||||
)
|
|
||||||
q_rad = np.deg2rad(q_deg)
|
|
||||||
|
|
||||||
hs = self._arm.GetArmHighSpdInfoMsgs()
|
|
||||||
v_rad = np.array(
|
|
||||||
[
|
|
||||||
float(hs.motor_1.motor_speed) * 1e-3,
|
|
||||||
float(hs.motor_2.motor_speed) * 1e-3,
|
|
||||||
float(hs.motor_3.motor_speed) * 1e-3,
|
|
||||||
float(hs.motor_4.motor_speed) * 1e-3,
|
|
||||||
float(hs.motor_5.motor_speed) * 1e-3,
|
|
||||||
float(hs.motor_6.motor_speed) * 1e-3,
|
|
||||||
],
|
|
||||||
dtype=np.float64,
|
|
||||||
)
|
|
||||||
return q_rad, v_rad
|
|
||||||
|
|
||||||
def _compute_gravity_torque(self, q_rad: np.ndarray, v_rad: np.ndarray) -> np.ndarray:
|
|
||||||
q_full = np.zeros(self._robot.model.nq, dtype=np.float64)
|
|
||||||
v_full = np.zeros(self._robot.model.nv, dtype=np.float64)
|
|
||||||
q_full[: self._nq] = q_rad
|
|
||||||
v_full[: self._nq] = v_rad
|
|
||||||
tau_full = self._pin.rnea(
|
|
||||||
self._robot.model,
|
|
||||||
self._robot.data,
|
|
||||||
q_full,
|
|
||||||
v_full,
|
|
||||||
np.zeros(self._robot.model.nv, dtype=np.float64),
|
|
||||||
)
|
|
||||||
return np.asarray(tau_full[: self._nq], dtype=np.float64)
|
|
||||||
|
|
||||||
def _run(self) -> None:
|
|
||||||
self._send_mit_mode()
|
|
||||||
dt = 1.0 / self._control_hz
|
|
||||||
while not self._stop_event.is_set():
|
|
||||||
start_t = time.perf_counter()
|
|
||||||
self._refresh_mit_mode_if_needed()
|
|
||||||
|
|
||||||
q_rad, v_rad = self._read_q_v()
|
|
||||||
tau = self._compute_gravity_torque(q_rad, v_rad)
|
|
||||||
tau = np.clip(self._tx_ratio * tau, -self._torque_limit, self._torque_limit)
|
|
||||||
|
|
||||||
for idx in range(6):
|
|
||||||
self._arm.JointMitCtrl(
|
|
||||||
idx + 1,
|
|
||||||
0.0,
|
|
||||||
0.0,
|
|
||||||
self._mit_kp,
|
|
||||||
self._mit_kd,
|
|
||||||
float(tau[idx]),
|
|
||||||
)
|
|
||||||
|
|
||||||
remain = dt - (time.perf_counter() - start_t)
|
|
||||||
if remain > 0:
|
|
||||||
time.sleep(remain)
|
|
||||||
@ -17,51 +17,29 @@
|
|||||||
import logging
|
import logging
|
||||||
import time
|
import time
|
||||||
from functools import cached_property
|
from functools import cached_property
|
||||||
from importlib import resources
|
|
||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
from lerobot.motors import MotorCalibration
|
|
||||||
from lerobot.processor import RobotAction
|
from lerobot.processor import RobotAction
|
||||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||||
from lerobot.utils.piper_sdk import (
|
from lerobot.utils.piper_sdk import (
|
||||||
PIPER_ACTION_KEYS,
|
PIPER_ACTION_KEYS,
|
||||||
PIPER_JOINT_ACTION_KEYS,
|
PIPER_JOINT_ACTION_KEYS,
|
||||||
PIPER_JOINT_NAMES,
|
PIPER_JOINT_NAMES,
|
||||||
|
PIPER_ROLE_FOLLOWER,
|
||||||
|
PIPER_ROLE_LEADER,
|
||||||
get_piper_sdk,
|
get_piper_sdk,
|
||||||
guard_piper_ctrl_mode_on_connect,
|
|
||||||
milli_to_unit,
|
milli_to_unit,
|
||||||
parse_piper_log_level,
|
parse_piper_log_level,
|
||||||
|
resolve_piper_can_interface,
|
||||||
|
set_piper_role,
|
||||||
unit_to_milli,
|
unit_to_milli,
|
||||||
wait_enable_piper,
|
wait_enable_piper,
|
||||||
)
|
)
|
||||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
|
||||||
|
|
||||||
from ..teleoperator import Teleoperator
|
from ..teleoperator import Teleoperator
|
||||||
from .config_piper_leader import PiperLeaderConfig, PiperXLeaderConfig
|
from .config_piper_leader import PiperLeaderConfig, PiperXLeaderConfig
|
||||||
from .gravity_compensation import PiperGravityCompensationLoop
|
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS)
|
|
||||||
PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)}
|
|
||||||
DEFAULT_PIPER_GRAVITY_URDF = "assets/piper_description/urdf/piper_no_gripper_description.urdf"
|
|
||||||
DEFAULT_PIPERX_GRAVITY_URDF = "assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf"
|
|
||||||
GIT_LFS_POINTER_PREFIX = b"version https://git-lfs.github.com/spec/v1"
|
|
||||||
MANUAL_CONTROL_FRESH_FEEDBACK_TIMEOUT_S = 0.03
|
|
||||||
MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S = 0.001
|
|
||||||
|
|
||||||
|
|
||||||
def _ensure_not_lfs_pointer(path_obj: Any, relpath: str) -> None:
|
|
||||||
with path_obj.open("rb") as f:
|
|
||||||
head = f.read(128)
|
|
||||||
if head.startswith(GIT_LFS_POINTER_PREFIX):
|
|
||||||
raise RuntimeError(
|
|
||||||
"Bundled PiPER assets are still Git LFS pointer files. "
|
|
||||||
"Please run `git lfs pull --include="
|
|
||||||
'"src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**" '
|
|
||||||
'--exclude="*"` and `git lfs checkout src/lerobot/assets/piper_description '
|
|
||||||
"src/lerobot/assets/piper_x_description`, then retry. "
|
|
||||||
f"(file: {relpath})"
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
class PiperLeader(Teleoperator):
|
class PiperLeader(Teleoperator):
|
||||||
@ -69,21 +47,17 @@ class PiperLeader(Teleoperator):
|
|||||||
|
|
||||||
config_class = PiperLeaderConfig
|
config_class = PiperLeaderConfig
|
||||||
name = "piper_leader"
|
name = "piper_leader"
|
||||||
gravity_comp_urdf_relpath = DEFAULT_PIPER_GRAVITY_URDF
|
|
||||||
|
|
||||||
def __init__(self, config: PiperLeaderConfig | PiperXLeaderConfig):
|
def __init__(self, config: PiperLeaderConfig | PiperXLeaderConfig):
|
||||||
super().__init__(config)
|
self.id = config.id
|
||||||
self.config = config
|
self.config = config
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
self._manual_control_enabled: bool | None = None
|
self._manual_control_enabled: bool | None = None
|
||||||
self._last_mode_refresh_t = 0.0
|
self._last_mode_refresh_t = 0.0
|
||||||
self._gravity_comp_loop: PiperGravityCompensationLoop | None = None
|
|
||||||
self._last_feedback_joint_timestamp = 0.0
|
|
||||||
self._last_feedback_gripper_timestamp = 0.0
|
|
||||||
|
|
||||||
interface_cls, _ = get_piper_sdk()
|
interface_cls, _ = get_piper_sdk()
|
||||||
self.arm = interface_cls(
|
self.arm = interface_cls(
|
||||||
can_name=self.config.port,
|
can_name=resolve_piper_can_interface(self.config.port),
|
||||||
judge_flag=self.config.judge_flag,
|
judge_flag=self.config.judge_flag,
|
||||||
can_auto_init=self.config.can_auto_init,
|
can_auto_init=self.config.can_auto_init,
|
||||||
logger_level=parse_piper_log_level(self.config.log_level),
|
logger_level=parse_piper_log_level(self.config.log_level),
|
||||||
@ -103,78 +77,29 @@ class PiperLeader(Teleoperator):
|
|||||||
|
|
||||||
@check_if_already_connected
|
@check_if_already_connected
|
||||||
def connect(self, calibrate: bool = True) -> None:
|
def connect(self, calibrate: bool = True) -> None:
|
||||||
|
del calibrate
|
||||||
self.arm.ConnectPort()
|
self.arm.ConnectPort()
|
||||||
if self.config.startup_sleep_s > 0:
|
if self.config.startup_sleep_s > 0:
|
||||||
time.sleep(self.config.startup_sleep_s)
|
time.sleep(self.config.startup_sleep_s)
|
||||||
guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port)
|
|
||||||
|
|
||||||
self._is_connected = True
|
self._is_connected = True
|
||||||
# Recompute control mode on every fresh connection.
|
|
||||||
self._manual_control_enabled = None
|
self._manual_control_enabled = None
|
||||||
self._last_feedback_joint_timestamp = 0.0
|
|
||||||
self._last_feedback_gripper_timestamp = 0.0
|
|
||||||
try:
|
try:
|
||||||
self.configure()
|
self.configure()
|
||||||
if not self.is_calibrated and calibrate and self.config.require_calibration:
|
|
||||||
logger.info(
|
|
||||||
"No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.",
|
|
||||||
self.id,
|
|
||||||
)
|
|
||||||
self.calibrate()
|
|
||||||
except Exception:
|
except Exception:
|
||||||
self._stop_gravity_comp_loop_if_needed()
|
|
||||||
self.arm.DisconnectPort()
|
self.arm.DisconnectPort()
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
|
self._manual_control_enabled = None
|
||||||
raise
|
raise
|
||||||
|
|
||||||
logger.info("%s connected.", self)
|
logger.info("%s connected.", self)
|
||||||
|
|
||||||
def _use_uncalibrated_passthrough(self) -> bool:
|
|
||||||
return not self.is_calibrated and not self.config.require_calibration
|
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
if not all(key in self.calibration for key in PIPER_CALIB_KEYS):
|
|
||||||
return False
|
|
||||||
for key in PIPER_CALIB_KEYS:
|
|
||||||
cal = self.calibration[key]
|
|
||||||
if cal.range_max <= cal.range_min:
|
|
||||||
return False
|
|
||||||
return True
|
return True
|
||||||
|
|
||||||
def calibrate(self) -> None:
|
def calibrate(self) -> None:
|
||||||
if self.calibration and self.is_calibrated:
|
pass
|
||||||
user_input = input(
|
|
||||||
f"Press ENTER to use existing calibration file for id '{self.id}', "
|
|
||||||
"or type 'c' and press ENTER to run a new calibration: "
|
|
||||||
)
|
|
||||||
if user_input.strip().lower() != "c":
|
|
||||||
return
|
|
||||||
|
|
||||||
logger.info("Running calibration for %s", self)
|
|
||||||
input("Move piper-leader to your desired neutral/center pose, then press ENTER...")
|
|
||||||
neutral = self._read_raw_action()
|
|
||||||
print("Move all piper-leader joints through full range. Press ENTER to stop recording...")
|
|
||||||
range_mins, range_maxes = self._record_ranges_of_motion()
|
|
||||||
|
|
||||||
self.calibration = {}
|
|
||||||
for key in PIPER_CALIB_KEYS:
|
|
||||||
min_deg = range_mins[key]
|
|
||||||
max_deg = range_maxes[key]
|
|
||||||
if max_deg <= min_deg:
|
|
||||||
raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}")
|
|
||||||
|
|
||||||
neutral_deg = min(max_deg, max(min_deg, neutral[key]))
|
|
||||||
self.calibration[key] = MotorCalibration(
|
|
||||||
id=PIPER_CALIB_IDS[key],
|
|
||||||
drive_mode=0,
|
|
||||||
homing_offset=self._to_calibration_units(neutral_deg),
|
|
||||||
range_min=self._to_calibration_units(min_deg),
|
|
||||||
range_max=self._to_calibration_units(max_deg),
|
|
||||||
)
|
|
||||||
|
|
||||||
self._save_calibration()
|
|
||||||
print(f"Calibration saved to {self.calibration_fpath}")
|
|
||||||
|
|
||||||
def _send_command_mode(self) -> None:
|
def _send_command_mode(self) -> None:
|
||||||
mit_mode = 0xAD if self.config.command_high_follow else 0x00
|
mit_mode = 0xAD if self.config.command_high_follow else 0x00
|
||||||
@ -189,9 +114,6 @@ class PiperLeader(Teleoperator):
|
|||||||
if now - self._last_mode_refresh_t >= interval_s:
|
if now - self._last_mode_refresh_t >= interval_s:
|
||||||
self._send_command_mode()
|
self._send_command_mode()
|
||||||
|
|
||||||
def _wait_enable(self, timeout_s: float) -> bool:
|
|
||||||
return wait_enable_piper(self.arm, timeout_s)
|
|
||||||
|
|
||||||
def _send_gripper_ctrl(self, gripper_pos_raw: int, enabled: bool) -> None:
|
def _send_gripper_ctrl(self, gripper_pos_raw: int, enabled: bool) -> None:
|
||||||
self.arm.GripperCtrl(
|
self.arm.GripperCtrl(
|
||||||
gripper_pos_raw,
|
gripper_pos_raw,
|
||||||
@ -213,249 +135,102 @@ class PiperLeader(Teleoperator):
|
|||||||
|
|
||||||
def set_manual_control(self, enabled: bool) -> None:
|
def set_manual_control(self, enabled: bool) -> None:
|
||||||
if not self._is_connected:
|
if not self._is_connected:
|
||||||
|
raise RuntimeError(f"{self} is not connected.")
|
||||||
|
if enabled == self._manual_control_enabled:
|
||||||
return
|
return
|
||||||
if enabled and self._manual_control_enabled is not True:
|
|
||||||
if not self._wait_enable(self.config.enable_timeout_s):
|
self._manual_control_enabled = None
|
||||||
logger.warning(
|
if enabled:
|
||||||
"Piper leader did not report enabled state before entering gravity compensation."
|
set_piper_role(self.arm, PIPER_ROLE_LEADER)
|
||||||
)
|
else:
|
||||||
if self.config.sync_gripper:
|
set_piper_role(self.arm, PIPER_ROLE_FOLLOWER)
|
||||||
self._set_gripper_enabled(False)
|
|
||||||
self._ensure_gravity_comp_loop().start()
|
|
||||||
self._manual_control_enabled = True
|
|
||||||
return
|
|
||||||
if not enabled and self._manual_control_enabled is not False:
|
|
||||||
self._stop_gravity_comp_loop_if_needed()
|
|
||||||
self._send_command_mode()
|
self._send_command_mode()
|
||||||
if not self._wait_enable(self.config.enable_timeout_s):
|
if not wait_enable_piper(self.arm, self.config.enable_timeout_s):
|
||||||
logger.warning("Piper leader did not report enabled state before timeout.")
|
raise RuntimeError(
|
||||||
|
f"[{self.config.port}] Piper leader did not enable after switching to follower role."
|
||||||
|
)
|
||||||
if self.config.sync_gripper:
|
if self.config.sync_gripper:
|
||||||
self._set_gripper_enabled(True)
|
self._set_gripper_enabled(True)
|
||||||
self._manual_control_enabled = False
|
|
||||||
|
|
||||||
def _ensure_gravity_comp_loop(self) -> PiperGravityCompensationLoop:
|
self._manual_control_enabled = enabled
|
||||||
if self._gravity_comp_loop is None:
|
|
||||||
default_urdf = resources.files("lerobot").joinpath(self.gravity_comp_urdf_relpath)
|
|
||||||
if not default_urdf.is_file():
|
|
||||||
raise FileNotFoundError(
|
|
||||||
"Bundled gravity compensation URDF is missing: "
|
|
||||||
f"{self.gravity_comp_urdf_relpath}. Reinstall the package."
|
|
||||||
)
|
|
||||||
_ensure_not_lfs_pointer(default_urdf, self.gravity_comp_urdf_relpath)
|
|
||||||
urdf_path = str(default_urdf)
|
|
||||||
self._gravity_comp_loop = PiperGravityCompensationLoop(
|
|
||||||
arm=self.arm,
|
|
||||||
urdf_path=urdf_path,
|
|
||||||
control_hz=self.config.gravity_comp_control_hz,
|
|
||||||
tx_ratio=self.config.gravity_comp_tx_ratio,
|
|
||||||
torque_limit=self.config.gravity_comp_torque_limit,
|
|
||||||
mit_kp=self.config.gravity_comp_mit_kp,
|
|
||||||
mit_kd=self.config.gravity_comp_mit_kd,
|
|
||||||
base_rpy_deg=self.config.gravity_comp_base_rpy_deg,
|
|
||||||
mode_refresh_interval_s=self.config.mode_refresh_interval_s,
|
|
||||||
move_speed_ratio=self.config.command_speed_ratio,
|
|
||||||
)
|
|
||||||
return self._gravity_comp_loop
|
|
||||||
|
|
||||||
def _stop_gravity_comp_loop_if_needed(self) -> None:
|
|
||||||
if self._gravity_comp_loop is not None:
|
|
||||||
self._gravity_comp_loop.stop()
|
|
||||||
|
|
||||||
def configure(self) -> None:
|
def configure(self) -> None:
|
||||||
self.set_manual_control(self.config.manual_control)
|
self.set_manual_control(self.config.manual_control)
|
||||||
|
|
||||||
def _read_joint_from_ctrl(self) -> dict[str, float] | None:
|
def _read_joint_from_ctrl(self) -> dict[str, float] | None:
|
||||||
joint_ctrl_msg = self.arm.GetArmJointCtrl()
|
message = self.arm.GetArmJointCtrl()
|
||||||
if getattr(joint_ctrl_msg, "time_stamp", 0.0) <= 0:
|
if message.time_stamp <= 0:
|
||||||
return None
|
return None
|
||||||
joint_ctrl = getattr(joint_ctrl_msg, "joint_ctrl", None)
|
|
||||||
return {
|
return {
|
||||||
f"{joint_name}.pos": milli_to_unit(getattr(joint_ctrl, joint_name, 0))
|
f"{joint_name}.pos": milli_to_unit(getattr(message.joint_ctrl, joint_name))
|
||||||
for joint_name in PIPER_JOINT_NAMES
|
for joint_name in PIPER_JOINT_NAMES
|
||||||
}
|
}
|
||||||
|
|
||||||
def _read_joint_from_feedback(self) -> dict[str, float] | None:
|
def _read_joint_from_feedback(self) -> dict[str, float] | None:
|
||||||
joint_msg = self.arm.GetArmJointMsgs()
|
message = self.arm.GetArmJointMsgs()
|
||||||
joint_state = getattr(joint_msg, "joint_state", None)
|
if message.time_stamp <= 0:
|
||||||
if joint_state is None:
|
|
||||||
return None
|
return None
|
||||||
return {
|
return {
|
||||||
f"{joint_name}.pos": milli_to_unit(getattr(joint_state, joint_name, 0))
|
f"{joint_name}.pos": milli_to_unit(getattr(message.joint_state, joint_name))
|
||||||
for joint_name in PIPER_JOINT_NAMES
|
for joint_name in PIPER_JOINT_NAMES
|
||||||
}
|
}
|
||||||
|
|
||||||
def _read_gripper_from_ctrl(self) -> float | None:
|
def _read_gripper_from_ctrl(self) -> float | None:
|
||||||
gripper_ctrl_msg = self.arm.GetArmGripperCtrl()
|
message = self.arm.GetArmGripperCtrl()
|
||||||
if getattr(gripper_ctrl_msg, "time_stamp", 0.0) <= 0:
|
if message.time_stamp <= 0:
|
||||||
return None
|
return None
|
||||||
gripper_ctrl = getattr(gripper_ctrl_msg, "gripper_ctrl", None)
|
return abs(milli_to_unit(message.gripper_ctrl.grippers_angle))
|
||||||
return abs(milli_to_unit(getattr(gripper_ctrl, "grippers_angle", 0)))
|
|
||||||
|
|
||||||
def _read_gripper_from_feedback(self) -> float | None:
|
def _read_gripper_from_feedback(self) -> float | None:
|
||||||
gripper_msg = self.arm.GetArmGripperMsgs()
|
message = self.arm.GetArmGripperMsgs()
|
||||||
gripper_state = getattr(gripper_msg, "gripper_state", None)
|
if message.time_stamp <= 0:
|
||||||
if gripper_state is None:
|
|
||||||
return None
|
return None
|
||||||
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
return abs(milli_to_unit(message.gripper_state.grippers_angle))
|
||||||
|
|
||||||
def _wait_for_fresh_feedback_if_needed(self) -> None:
|
|
||||||
if self._manual_control_enabled is not True:
|
|
||||||
return
|
|
||||||
|
|
||||||
needs_fresh_gripper = self.config.sync_gripper and self.config.fallback_to_feedback
|
|
||||||
deadline = time.monotonic() + MANUAL_CONTROL_FRESH_FEEDBACK_TIMEOUT_S
|
|
||||||
while time.monotonic() < deadline:
|
|
||||||
joint_ts = float(getattr(self.arm.GetArmJointMsgs(), "time_stamp", 0.0) or 0.0)
|
|
||||||
has_fresh_joint = joint_ts > self._last_feedback_joint_timestamp
|
|
||||||
if not has_fresh_joint:
|
|
||||||
time.sleep(MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S)
|
|
||||||
continue
|
|
||||||
if not needs_fresh_gripper:
|
|
||||||
return
|
|
||||||
|
|
||||||
gripper_ts = float(getattr(self.arm.GetArmGripperMsgs(), "time_stamp", 0.0) or 0.0)
|
|
||||||
has_fresh_gripper = gripper_ts > self._last_feedback_gripper_timestamp
|
|
||||||
if has_fresh_gripper:
|
|
||||||
return
|
|
||||||
time.sleep(MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S)
|
|
||||||
|
|
||||||
def _read_raw_action(self) -> RobotAction:
|
def _read_raw_action(self) -> RobotAction:
|
||||||
used_feedback_for_joints = False
|
if self._manual_control_enabled is True:
|
||||||
action: dict[str, float] | None = None
|
|
||||||
|
|
||||||
# In manual-control teleop, the operator physically backdrives the leader arm while a
|
|
||||||
# background gravity-compensation thread emits MIT commands. In that mode, `GetArmJointCtrl`
|
|
||||||
# reflects transmitted control targets rather than the operator's live pose, which can lag
|
|
||||||
# or jump under dual-arm load. Prefer direct joint feedback so teleop follows the real arm.
|
|
||||||
prefer_feedback = self._manual_control_enabled is True
|
|
||||||
if prefer_feedback:
|
|
||||||
self._wait_for_fresh_feedback_if_needed()
|
|
||||||
action = self._read_joint_from_feedback()
|
|
||||||
used_feedback_for_joints = action is not None
|
|
||||||
self._last_feedback_joint_timestamp = float(
|
|
||||||
getattr(self.arm.GetArmJointMsgs(), "time_stamp", 0.0) or 0.0
|
|
||||||
)
|
|
||||||
|
|
||||||
if action is None and self.config.prefer_ctrl_messages:
|
|
||||||
action = self._read_joint_from_ctrl()
|
action = self._read_joint_from_ctrl()
|
||||||
|
gripper_pos = self._read_gripper_from_ctrl() if self.config.sync_gripper else None
|
||||||
if action is None and self.config.fallback_to_feedback and not used_feedback_for_joints:
|
elif self._manual_control_enabled is False:
|
||||||
action = self._read_joint_from_feedback()
|
action = self._read_joint_from_feedback()
|
||||||
used_feedback_for_joints = action is not None
|
gripper_pos = self._read_gripper_from_feedback() if self.config.sync_gripper else None
|
||||||
|
else:
|
||||||
|
raise RuntimeError(f"[{self.config.port}] Piper leader control mode is unknown.")
|
||||||
|
|
||||||
if action is None:
|
if action is None:
|
||||||
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
|
return {}
|
||||||
|
if self.config.sync_gripper:
|
||||||
use_ctrl_for_gripper = (
|
if gripper_pos is None:
|
||||||
self.config.prefer_ctrl_messages and not prefer_feedback and not used_feedback_for_joints
|
return {}
|
||||||
)
|
action["gripper.pos"] = gripper_pos
|
||||||
gripper_pos = self._read_gripper_from_ctrl() if use_ctrl_for_gripper else None
|
else:
|
||||||
if gripper_pos is None and self.config.fallback_to_feedback:
|
|
||||||
gripper_pos = self._read_gripper_from_feedback()
|
|
||||||
self._last_feedback_gripper_timestamp = float(
|
|
||||||
getattr(self.arm.GetArmGripperMsgs(), "time_stamp", 0.0) or 0.0
|
|
||||||
)
|
|
||||||
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
|
||||||
return action
|
|
||||||
|
|
||||||
def _to_calibration_units(self, angle_deg: float) -> int:
|
|
||||||
return int(round(angle_deg * self.config.calibration_scale))
|
|
||||||
|
|
||||||
def _from_calibration_units(self, value: int) -> float:
|
|
||||||
return float(value) / float(self.config.calibration_scale)
|
|
||||||
|
|
||||||
def _calibrated_to_offset(self, key: str, raw_deg: float) -> float:
|
|
||||||
cal = self.calibration[key]
|
|
||||||
min_deg = self._from_calibration_units(cal.range_min)
|
|
||||||
max_deg = self._from_calibration_units(cal.range_max)
|
|
||||||
home_deg = self._from_calibration_units(cal.homing_offset)
|
|
||||||
bounded = min(max_deg, max(min_deg, raw_deg))
|
|
||||||
centered = bounded - home_deg
|
|
||||||
return -centered if cal.drive_mode else centered
|
|
||||||
|
|
||||||
def _offset_to_calibrated(self, key: str, offset_deg: float) -> float:
|
|
||||||
cal = self.calibration[key]
|
|
||||||
min_deg = self._from_calibration_units(cal.range_min)
|
|
||||||
max_deg = self._from_calibration_units(cal.range_max)
|
|
||||||
home_deg = self._from_calibration_units(cal.homing_offset)
|
|
||||||
centered = -offset_deg if cal.drive_mode else offset_deg
|
|
||||||
target = home_deg + centered
|
|
||||||
return min(max_deg, max(min_deg, target))
|
|
||||||
|
|
||||||
def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]:
|
|
||||||
current = self._read_raw_action()
|
|
||||||
mins = current.copy()
|
|
||||||
maxes = current.copy()
|
|
||||||
|
|
||||||
while True:
|
|
||||||
current = self._read_raw_action()
|
|
||||||
mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS}
|
|
||||||
maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS}
|
|
||||||
|
|
||||||
print("\n-----------------------------")
|
|
||||||
print("JOINT | MIN | POS | MAX")
|
|
||||||
for key in PIPER_CALIB_KEYS:
|
|
||||||
print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}")
|
|
||||||
|
|
||||||
if enter_pressed():
|
|
||||||
break
|
|
||||||
move_cursor_up(len(PIPER_CALIB_KEYS) + 3)
|
|
||||||
|
|
||||||
return mins, maxes
|
|
||||||
|
|
||||||
@check_if_not_connected
|
|
||||||
def get_action(self) -> RobotAction:
|
|
||||||
if not self.is_calibrated and not self._use_uncalibrated_passthrough():
|
|
||||||
raise RuntimeError(
|
|
||||||
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first."
|
|
||||||
)
|
|
||||||
|
|
||||||
raw_action = self._read_raw_action()
|
|
||||||
if self._use_uncalibrated_passthrough():
|
|
||||||
action: RobotAction = dict(raw_action)
|
|
||||||
if not self.config.sync_gripper:
|
|
||||||
action["gripper.pos"] = 0.0
|
|
||||||
return action
|
|
||||||
|
|
||||||
action: RobotAction = {
|
|
||||||
key: self._calibrated_to_offset(key, raw_action[key]) for key in PIPER_CALIB_KEYS
|
|
||||||
}
|
|
||||||
if not self.config.sync_gripper:
|
|
||||||
action["gripper.pos"] = 0.0
|
action["gripper.pos"] = 0.0
|
||||||
return action
|
return action
|
||||||
|
|
||||||
@check_if_not_connected
|
@check_if_not_connected
|
||||||
def send_feedback(self, feedback: dict[str, Any]) -> None:
|
def get_action(self) -> RobotAction:
|
||||||
if not self.is_calibrated and not self._use_uncalibrated_passthrough():
|
return self._read_raw_action()
|
||||||
raise RuntimeError(
|
|
||||||
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first."
|
|
||||||
)
|
|
||||||
|
|
||||||
|
@check_if_not_connected
|
||||||
|
def send_feedback(self, feedback: dict[str, Any]) -> None:
|
||||||
self.set_manual_control(False)
|
self.set_manual_control(False)
|
||||||
self._refresh_command_mode_if_needed()
|
self._refresh_command_mode_if_needed()
|
||||||
|
|
||||||
joint_keys = PIPER_JOINT_ACTION_KEYS
|
joint_keys = PIPER_JOINT_ACTION_KEYS
|
||||||
has_all_joints = all(key in feedback for key in joint_keys)
|
has_all_joints = all(key in feedback for key in joint_keys)
|
||||||
if has_all_joints:
|
if has_all_joints:
|
||||||
if self._use_uncalibrated_passthrough():
|
joint_targets = [feedback[key] for key in joint_keys]
|
||||||
joint_targets = [feedback[key] for key in joint_keys]
|
|
||||||
else:
|
|
||||||
joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys]
|
|
||||||
joint_commands = [unit_to_milli(value) for value in joint_targets]
|
joint_commands = [unit_to_milli(value) for value in joint_targets]
|
||||||
self.arm.JointCtrl(*joint_commands)
|
self.arm.JointCtrl(*joint_commands)
|
||||||
|
|
||||||
if self.config.sync_gripper and "gripper.pos" in feedback:
|
if self.config.sync_gripper and "gripper.pos" in feedback:
|
||||||
if self._use_uncalibrated_passthrough():
|
gripper_pos_raw = unit_to_milli(feedback["gripper.pos"])
|
||||||
gripper_target = feedback["gripper.pos"]
|
|
||||||
else:
|
|
||||||
gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"])
|
|
||||||
gripper_pos_raw = unit_to_milli(gripper_target)
|
|
||||||
self._send_gripper_ctrl(gripper_pos_raw, enabled=True)
|
self._send_gripper_ctrl(gripper_pos_raw, enabled=True)
|
||||||
|
|
||||||
@check_if_not_connected
|
@check_if_not_connected
|
||||||
def disconnect(self) -> None:
|
def disconnect(self) -> None:
|
||||||
try:
|
try:
|
||||||
self._stop_gravity_comp_loop_if_needed()
|
self.set_manual_control(True)
|
||||||
if self.config.disable_on_disconnect:
|
if self.config.disable_on_disconnect:
|
||||||
self.arm.DisableArm(7)
|
self.arm.DisableArm(7)
|
||||||
finally:
|
finally:
|
||||||
@ -468,4 +243,3 @@ class PiperLeader(Teleoperator):
|
|||||||
class PiperXLeader(PiperLeader):
|
class PiperXLeader(PiperLeader):
|
||||||
config_class = PiperXLeaderConfig
|
config_class = PiperXLeaderConfig
|
||||||
name = "piperx_leader"
|
name = "piperx_leader"
|
||||||
gravity_comp_urdf_relpath = DEFAULT_PIPERX_GRAVITY_URDF
|
|
||||||
|
|||||||
@ -16,8 +16,10 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import subprocess
|
||||||
import time
|
import time
|
||||||
from functools import lru_cache
|
from functools import lru_cache
|
||||||
|
from pathlib import Path
|
||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
PIPER_JOINT_NAMES = (
|
PIPER_JOINT_NAMES = (
|
||||||
@ -30,8 +32,33 @@ PIPER_JOINT_NAMES = (
|
|||||||
)
|
)
|
||||||
PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES)
|
PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES)
|
||||||
PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",)
|
PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",)
|
||||||
PIPER_CTRL_MODE_TEACH = 0x02
|
PIPER_ROLE_LEADER = 0xFA
|
||||||
PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06
|
PIPER_ROLE_FOLLOWER = 0xFC
|
||||||
|
_SYS_CLASS_NET = Path("/sys/class/net")
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_piper_can_interface(serial_number: str) -> str:
|
||||||
|
"""Return the SocketCAN interface for a USB-CAN adapter serial number."""
|
||||||
|
for interface_path in _SYS_CLASS_NET.iterdir():
|
||||||
|
try:
|
||||||
|
if (interface_path / "type").read_text().strip() != "280":
|
||||||
|
continue
|
||||||
|
except OSError:
|
||||||
|
continue
|
||||||
|
|
||||||
|
try:
|
||||||
|
result = subprocess.run( # nosec B607
|
||||||
|
["udevadm", "info", "--query=property", f"--path={interface_path}"],
|
||||||
|
capture_output=True,
|
||||||
|
text=True,
|
||||||
|
)
|
||||||
|
except FileNotFoundError as exc:
|
||||||
|
raise RuntimeError("`udevadm` is required to find the PiPER USB-CAN adapter.") from exc
|
||||||
|
|
||||||
|
if f"ID_SERIAL_SHORT={serial_number}" in result.stdout.splitlines():
|
||||||
|
return interface_path.name
|
||||||
|
|
||||||
|
raise RuntimeError(f"No PiPER USB-CAN adapter found with serial '{serial_number}'.")
|
||||||
|
|
||||||
|
|
||||||
def milli_to_unit(value: float | int) -> float:
|
def milli_to_unit(value: float | int) -> float:
|
||||||
@ -79,51 +106,5 @@ def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2)
|
|||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def _piper_mode_to_int(value: Any) -> int | None:
|
def set_piper_role(arm: Any, role: int) -> None:
|
||||||
if isinstance(value, bool):
|
arm.MasterSlaveConfig(role, 0x00, 0x00, 0x00)
|
||||||
return int(value)
|
|
||||||
if isinstance(value, int):
|
|
||||||
return int(value)
|
|
||||||
if hasattr(value, "value") and isinstance(value.value, int):
|
|
||||||
return int(value.value)
|
|
||||||
if hasattr(value, "__int__"):
|
|
||||||
return int(value)
|
|
||||||
return None
|
|
||||||
|
|
||||||
|
|
||||||
def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None:
|
|
||||||
deadline = time.monotonic() + max(0.0, timeout_s)
|
|
||||||
while time.monotonic() < deadline:
|
|
||||||
status_msg = arm.GetArmStatus()
|
|
||||||
if getattr(status_msg, "time_stamp", 0.0) > 0.0:
|
|
||||||
arm_status = getattr(status_msg, "arm_status", None)
|
|
||||||
if arm_status is not None:
|
|
||||||
mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None))
|
|
||||||
if mode is not None:
|
|
||||||
return mode
|
|
||||||
time.sleep(max(0.005, poll_s))
|
|
||||||
return None
|
|
||||||
|
|
||||||
|
|
||||||
def guard_piper_ctrl_mode_on_connect(
|
|
||||||
arm: Any,
|
|
||||||
*,
|
|
||||||
interface_name: str,
|
|
||||||
timeout_s: float = 0.5,
|
|
||||||
poll_s: float = 0.02,
|
|
||||||
settle_s: float = 0.05,
|
|
||||||
) -> None:
|
|
||||||
mode = read_piper_ctrl_mode(arm, timeout_s=timeout_s, poll_s=poll_s)
|
|
||||||
if mode is None:
|
|
||||||
raise RuntimeError(
|
|
||||||
f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. "
|
|
||||||
"Check CAN wiring/power and rerun."
|
|
||||||
)
|
|
||||||
if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
|
|
||||||
arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
|
||||||
if settle_s > 0:
|
|
||||||
time.sleep(settle_s)
|
|
||||||
raise RuntimeError(
|
|
||||||
f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). "
|
|
||||||
"Follower role command (0xFC) has been sent. Power-cycle this arm, then retry."
|
|
||||||
)
|
|
||||||
|
|||||||
@ -12,14 +12,10 @@ def test_teleoperate_parses_bi_piperx_types():
|
|||||||
"--robot.id=my_bi_piperx_follower",
|
"--robot.id=my_bi_piperx_follower",
|
||||||
"--robot.left_arm_config.port=can0",
|
"--robot.left_arm_config.port=can0",
|
||||||
"--robot.right_arm_config.port=can2",
|
"--robot.right_arm_config.port=can2",
|
||||||
"--robot.left_arm_config.require_calibration=false",
|
|
||||||
"--robot.right_arm_config.require_calibration=false",
|
|
||||||
"--teleop.type=bi_piperx_leader",
|
"--teleop.type=bi_piperx_leader",
|
||||||
"--teleop.id=my_bi_piperx_leader",
|
"--teleop.id=my_bi_piperx_leader",
|
||||||
"--teleop.left_arm_config.port=can1",
|
"--teleop.left_arm_config.port=can1",
|
||||||
"--teleop.right_arm_config.port=can3",
|
"--teleop.right_arm_config.port=can3",
|
||||||
"--teleop.left_arm_config.require_calibration=false",
|
|
||||||
"--teleop.right_arm_config.require_calibration=false",
|
|
||||||
]
|
]
|
||||||
cfg = draccus.parse(config_class=TeleoperateConfig, config_path=None, args=args)
|
cfg = draccus.parse(config_class=TeleoperateConfig, config_path=None, args=args)
|
||||||
assert cfg.robot.type == "bi_piperx_follower"
|
assert cfg.robot.type == "bi_piperx_follower"
|
||||||
@ -32,14 +28,10 @@ def test_record_rejects_mixed_bi_piper_pair():
|
|||||||
"--robot.id=my_bi_piper_follower",
|
"--robot.id=my_bi_piper_follower",
|
||||||
"--robot.left_arm_config.port=can0",
|
"--robot.left_arm_config.port=can0",
|
||||||
"--robot.right_arm_config.port=can2",
|
"--robot.right_arm_config.port=can2",
|
||||||
"--robot.left_arm_config.require_calibration=false",
|
|
||||||
"--robot.right_arm_config.require_calibration=false",
|
|
||||||
"--teleop.type=bi_piperx_leader",
|
"--teleop.type=bi_piperx_leader",
|
||||||
"--teleop.id=my_bi_piperx_leader",
|
"--teleop.id=my_bi_piperx_leader",
|
||||||
"--teleop.left_arm_config.port=can1",
|
"--teleop.left_arm_config.port=can1",
|
||||||
"--teleop.right_arm_config.port=can3",
|
"--teleop.right_arm_config.port=can3",
|
||||||
"--teleop.left_arm_config.require_calibration=false",
|
|
||||||
"--teleop.right_arm_config.require_calibration=false",
|
|
||||||
"--dataset.repo_id=dummy/dummy",
|
"--dataset.repo_id=dummy/dummy",
|
||||||
"--dataset.single_task=test",
|
"--dataset.single_task=test",
|
||||||
"--dataset.num_episodes=1",
|
"--dataset.num_episodes=1",
|
||||||
@ -62,8 +54,6 @@ def test_record_rejects_bimanual_piper_teleop_with_non_bimanual_robot():
|
|||||||
"--teleop.id=my_bi_piperx_leader",
|
"--teleop.id=my_bi_piperx_leader",
|
||||||
"--teleop.left_arm_config.port=can1",
|
"--teleop.left_arm_config.port=can1",
|
||||||
"--teleop.right_arm_config.port=can3",
|
"--teleop.right_arm_config.port=can3",
|
||||||
"--teleop.left_arm_config.require_calibration=false",
|
|
||||||
"--teleop.right_arm_config.require_calibration=false",
|
|
||||||
"--dataset.repo_id=dummy/dummy",
|
"--dataset.repo_id=dummy/dummy",
|
||||||
"--dataset.single_task=test",
|
"--dataset.single_task=test",
|
||||||
"--dataset.num_episodes=1",
|
"--dataset.num_episodes=1",
|
||||||
|
|||||||
@ -1,7 +1,4 @@
|
|||||||
# ruff: noqa: N802
|
# ruff: noqa: N802
|
||||||
import sys
|
|
||||||
import threading
|
|
||||||
import time
|
|
||||||
from types import SimpleNamespace
|
from types import SimpleNamespace
|
||||||
|
|
||||||
import pytest
|
import pytest
|
||||||
@ -10,7 +7,6 @@ import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
|||||||
import lerobot.teleoperators.bi_piper_leader.bi_piper_leader as bi_piper_leader_module
|
import lerobot.teleoperators.bi_piper_leader.bi_piper_leader as bi_piper_leader_module
|
||||||
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
||||||
import lerobot.utils.piper_sdk as piper_sdk_utils
|
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||||
from lerobot.motors import MotorCalibration
|
|
||||||
from lerobot.processor import make_default_processors
|
from lerobot.processor import make_default_processors
|
||||||
from lerobot.robots.bi_piper_follower import (
|
from lerobot.robots.bi_piper_follower import (
|
||||||
BiPiperFollower,
|
BiPiperFollower,
|
||||||
@ -26,6 +22,7 @@ from lerobot.robots.piper_follower import (
|
|||||||
PiperXFollowerConfig,
|
PiperXFollowerConfig,
|
||||||
)
|
)
|
||||||
from lerobot.robots.utils import make_robot_from_config
|
from lerobot.robots.utils import make_robot_from_config
|
||||||
|
from lerobot.scripts.lerobot_teleoperate import teleop_loop
|
||||||
from lerobot.teleoperators.bi_piper_leader import (
|
from lerobot.teleoperators.bi_piper_leader import (
|
||||||
BiPiperLeader,
|
BiPiperLeader,
|
||||||
BiPiperLeaderConfig,
|
BiPiperLeaderConfig,
|
||||||
@ -40,7 +37,6 @@ from lerobot.teleoperators.piper_leader import (
|
|||||||
PiperXLeaderConfig,
|
PiperXLeaderConfig,
|
||||||
PiperXLeaderConfigBase,
|
PiperXLeaderConfigBase,
|
||||||
)
|
)
|
||||||
from lerobot.scripts.lerobot_teleoperate import teleop_loop
|
|
||||||
from lerobot.teleoperators.utils import make_teleoperator_from_config
|
from lerobot.teleoperators.utils import make_teleoperator_from_config
|
||||||
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
||||||
|
|
||||||
@ -71,6 +67,7 @@ class FakePiperInterface:
|
|||||||
self.disable_calls = 0
|
self.disable_calls = 0
|
||||||
self.is_enabled = False
|
self.is_enabled = False
|
||||||
self.connect_calls = []
|
self.connect_calls = []
|
||||||
|
self.command_log = []
|
||||||
|
|
||||||
self._joint_ctrl = SimpleNamespace(
|
self._joint_ctrl = SimpleNamespace(
|
||||||
time_stamp=1.0,
|
time_stamp=1.0,
|
||||||
@ -134,14 +131,19 @@ class FakePiperInterface:
|
|||||||
|
|
||||||
def MotionCtrl_2(self, *args):
|
def MotionCtrl_2(self, *args):
|
||||||
self.mode_commands.append(args)
|
self.mode_commands.append(args)
|
||||||
|
self.command_log.append(("motion_mode", args))
|
||||||
|
|
||||||
def MasterSlaveConfig(self, *args):
|
def MasterSlaveConfig(self, *args):
|
||||||
self.role_commands.append(args)
|
self.role_commands.append(args)
|
||||||
|
self.command_log.append(("role", args))
|
||||||
if args and args[0] == 0xFC:
|
if args and args[0] == 0xFC:
|
||||||
self._arm_status.arm_status.ctrl_mode = 0x01
|
self._arm_status.arm_status.ctrl_mode = 0x01
|
||||||
|
elif args and args[0] == 0xFA:
|
||||||
|
self._arm_status.arm_status.ctrl_mode = 0x06
|
||||||
|
|
||||||
def EnablePiper(self):
|
def EnablePiper(self):
|
||||||
self.enable_calls += 1
|
self.enable_calls += 1
|
||||||
|
self.command_log.append(("enable", ()))
|
||||||
self.is_enabled = True
|
self.is_enabled = True
|
||||||
return True
|
return True
|
||||||
|
|
||||||
@ -152,6 +154,7 @@ class FakePiperInterface:
|
|||||||
|
|
||||||
def JointCtrl(self, *args):
|
def JointCtrl(self, *args):
|
||||||
self.last_joint = args
|
self.last_joint = args
|
||||||
|
self.command_log.append(("joint", args))
|
||||||
|
|
||||||
def JointMitCtrl(self, *args):
|
def JointMitCtrl(self, *args):
|
||||||
self.joint_mit_calls.append(args)
|
self.joint_mit_calls.append(args)
|
||||||
@ -159,6 +162,7 @@ class FakePiperInterface:
|
|||||||
def GripperCtrl(self, *args):
|
def GripperCtrl(self, *args):
|
||||||
self.last_gripper = args
|
self.last_gripper = args
|
||||||
self.gripper_calls.append(args)
|
self.gripper_calls.append(args)
|
||||||
|
self.command_log.append(("gripper", args))
|
||||||
|
|
||||||
def GetArmJointCtrl(self):
|
def GetArmJointCtrl(self):
|
||||||
return self._joint_ctrl
|
return self._joint_ctrl
|
||||||
@ -186,41 +190,21 @@ def patch_fake_sdk(monkeypatch):
|
|||||||
monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader)
|
monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader)
|
||||||
monkeypatch.setattr(piper_follower_module, "get_piper_sdk", fake_loader)
|
monkeypatch.setattr(piper_follower_module, "get_piper_sdk", fake_loader)
|
||||||
monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader)
|
monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader)
|
||||||
|
monkeypatch.setattr(piper_follower_module, "resolve_piper_can_interface", lambda serial: serial)
|
||||||
|
monkeypatch.setattr(piper_leader_module, "resolve_piper_can_interface", lambda serial: serial)
|
||||||
def make_identity_calibration():
|
|
||||||
return {
|
|
||||||
key: MotorCalibration(
|
|
||||||
id=idx,
|
|
||||||
drive_mode=0,
|
|
||||||
homing_offset=0,
|
|
||||||
range_min=-200000,
|
|
||||||
range_max=200000,
|
|
||||||
)
|
|
||||||
for idx, key in enumerate(PIPER_ACTION_KEYS)
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def wait_until(predicate, timeout_s: float = 0.2, poll_s: float = 0.005) -> bool:
|
|
||||||
deadline = time.monotonic() + timeout_s
|
|
||||||
while time.monotonic() < deadline:
|
|
||||||
if predicate():
|
|
||||||
return True
|
|
||||||
time.sleep(poll_s)
|
|
||||||
return False
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize(
|
@pytest.mark.parametrize(
|
||||||
("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"),
|
("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"),
|
||||||
[
|
[
|
||||||
(
|
(
|
||||||
PiperLeaderConfig(port="can1", manual_control=False, sync_gripper=True),
|
PiperLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
|
||||||
PiperFollowerConfig(port="can0", sync_gripper=True),
|
PiperFollowerConfig(port="can0", sync_gripper=True),
|
||||||
PiperLeader,
|
PiperLeader,
|
||||||
PiperFollower,
|
PiperFollower,
|
||||||
),
|
),
|
||||||
(
|
(
|
||||||
PiperXLeaderConfig(port="can1", manual_control=False, sync_gripper=True),
|
PiperXLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
|
||||||
PiperXFollowerConfig(port="can0", sync_gripper=True),
|
PiperXFollowerConfig(port="can0", sync_gripper=True),
|
||||||
PiperXLeader,
|
PiperXLeader,
|
||||||
PiperXFollower,
|
PiperXFollower,
|
||||||
@ -236,12 +220,11 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
|
|||||||
assert isinstance(teleop, teleop_cls)
|
assert isinstance(teleop, teleop_cls)
|
||||||
assert isinstance(robot, robot_cls)
|
assert isinstance(robot, robot_cls)
|
||||||
|
|
||||||
teleop.calibration = make_identity_calibration()
|
teleop.connect()
|
||||||
robot.calibration = make_identity_calibration()
|
robot.connect()
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
|
||||||
robot.connect(calibrate=False)
|
|
||||||
try:
|
try:
|
||||||
|
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
|
||||||
|
assert robot.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
|
||||||
action = teleop.get_action()
|
action = teleop.get_action()
|
||||||
sent = robot.send_action(action)
|
sent = robot.send_action(action)
|
||||||
obs = robot.get_observation()
|
obs = robot.get_observation()
|
||||||
@ -259,6 +242,7 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
|
|||||||
assert obs["gripper.pos"] == 43.0
|
assert obs["gripper.pos"] == 43.0
|
||||||
|
|
||||||
teleop.send_feedback(action)
|
teleop.send_feedback(action)
|
||||||
|
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
|
||||||
assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||||
assert teleop.arm.last_gripper == (
|
assert teleop.arm.last_gripper == (
|
||||||
42000,
|
42000,
|
||||||
@ -269,38 +253,49 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
|
|||||||
finally:
|
finally:
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
robot.disconnect()
|
robot.disconnect()
|
||||||
|
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
|
||||||
|
|
||||||
|
|
||||||
def test_piper_requires_calibration(monkeypatch):
|
def test_piper_does_not_initialize_calibration_state(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
leader = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||||
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
follower = PiperFollower(PiperFollowerConfig(port="can0"))
|
||||||
|
bi_leader = BiPiperLeader(
|
||||||
|
BiPiperLeaderConfig(
|
||||||
|
left_arm_config=PiperLeaderConfigBase(port="can1"),
|
||||||
|
right_arm_config=PiperLeaderConfigBase(port="can3"),
|
||||||
|
process_isolation=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
bi_follower = BiPiperFollower(
|
||||||
|
BiPiperFollowerConfig(
|
||||||
|
left_arm_config=PiperFollowerConfigBase(port="can0"),
|
||||||
|
right_arm_config=PiperFollowerConfigBase(port="can2"),
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
assert not teleop.is_calibrated
|
devices = [
|
||||||
assert not robot.is_calibrated
|
leader,
|
||||||
|
follower,
|
||||||
teleop.connect(calibrate=False)
|
bi_leader,
|
||||||
robot.connect(calibrate=False)
|
bi_leader.left_arm,
|
||||||
try:
|
bi_leader.right_arm,
|
||||||
with pytest.raises(RuntimeError, match="is not calibrated"):
|
bi_follower,
|
||||||
teleop.get_action()
|
bi_follower.left_arm,
|
||||||
with pytest.raises(RuntimeError, match="is not calibrated"):
|
bi_follower.right_arm,
|
||||||
robot.send_action({"joint_1.pos": 0.0})
|
]
|
||||||
finally:
|
assert all(not hasattr(device, "calibration") for device in devices)
|
||||||
teleop.disconnect()
|
assert all(not hasattr(device, "calibration_fpath") for device in devices)
|
||||||
robot.disconnect()
|
|
||||||
|
|
||||||
|
|
||||||
def test_piper_leader_reconnect_reapplies_mode(monkeypatch):
|
def test_piper_leader_reconnect_reapplies_mode(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
||||||
teleop.calibration = make_identity_calibration()
|
teleop.connect()
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
teleop.connect(calibrate=False)
|
teleop.connect()
|
||||||
try:
|
try:
|
||||||
assert teleop.arm.enable_calls == 2
|
assert teleop.arm.enable_calls == 2
|
||||||
finally:
|
finally:
|
||||||
@ -337,51 +332,73 @@ def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch):
|
|||||||
)
|
)
|
||||||
|
|
||||||
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
||||||
robot.calibration = make_identity_calibration()
|
|
||||||
|
|
||||||
with pytest.raises(RuntimeError, match="camera connect failed"):
|
with pytest.raises(RuntimeError, match="camera connect failed"):
|
||||||
robot.connect(calibrate=False)
|
robot.connect()
|
||||||
|
|
||||||
assert cam_ok.disconnect_calls == 1
|
assert cam_ok.disconnect_calls == 1
|
||||||
assert not cam_ok.is_connected
|
assert not cam_ok.is_connected
|
||||||
assert not robot.is_connected
|
assert not robot.is_connected
|
||||||
|
|
||||||
|
|
||||||
def test_piper_require_calibration_false_allows_uncalibrated_control(monkeypatch):
|
def test_piper_leader_default_uses_hardware_leader_role(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||||
|
|
||||||
teleop = PiperLeader(PiperLeaderConfig(port="can1", require_calibration=False))
|
teleop.connect()
|
||||||
robot = PiperFollower(PiperFollowerConfig(port="can0", require_calibration=False))
|
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
|
||||||
robot.connect(calibrate=False)
|
|
||||||
try:
|
try:
|
||||||
|
assert teleop.arm.connect_calls[-1]["piper_init"] is False
|
||||||
|
assert teleop.arm.role_commands == [(0xFA, 0x00, 0x00, 0x00)]
|
||||||
|
assert teleop.arm.enable_calls == 0
|
||||||
|
assert teleop.arm.mode_commands == []
|
||||||
|
assert teleop.arm.gripper_calls == []
|
||||||
|
assert teleop.arm.joint_mit_calls == []
|
||||||
action = teleop.get_action()
|
action = teleop.get_action()
|
||||||
sent = robot.send_action(action)
|
assert action["joint_1.pos"] == 10.0
|
||||||
assert sent["joint_1.pos"] == 11.0
|
assert action["joint_2.pos"] == 20.0
|
||||||
assert sent["gripper.pos"] == 43.0
|
assert action["gripper.pos"] == 42.0
|
||||||
finally:
|
finally:
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
robot.disconnect()
|
|
||||||
|
|
||||||
|
|
||||||
def test_piper_gravity_comp_defaults_are_model_specific():
|
def test_piper_leader_switches_between_manual_and_policy_control(monkeypatch):
|
||||||
assert PiperLeaderConfig(port="can1").gravity_comp_tx_ratio == (0.2, 0.2, 0.2, 0.2, 0.2, 0.2)
|
|
||||||
assert PiperXLeaderConfig(port="can1").gravity_comp_tx_ratio == (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
|
|
||||||
|
|
||||||
|
|
||||||
def test_piper_leader_manual_control_prefers_feedback_state(monkeypatch):
|
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||||
|
teleop.connect()
|
||||||
|
try:
|
||||||
|
teleop.arm.command_log.clear()
|
||||||
|
teleop.set_manual_control(False)
|
||||||
|
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
|
||||||
|
assert teleop.arm.mode_commands[-1][:3] == (0x01, 0x01, teleop.config.command_speed_ratio)
|
||||||
|
assert teleop.arm.enable_calls == 1
|
||||||
|
assert [name for name, _ in teleop.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
|
||||||
|
|
||||||
teleop = PiperLeader(
|
role_count = len(teleop.arm.role_commands)
|
||||||
PiperLeaderConfig(
|
action = {
|
||||||
port="can1",
|
"joint_1.pos": 1.0,
|
||||||
manual_control=True,
|
"joint_2.pos": 2.0,
|
||||||
require_calibration=False,
|
"joint_3.pos": 3.0,
|
||||||
)
|
"joint_4.pos": 4.0,
|
||||||
)
|
"joint_5.pos": 5.0,
|
||||||
|
"joint_6.pos": 6.0,
|
||||||
|
"gripper.pos": 7.0,
|
||||||
|
}
|
||||||
|
teleop.send_feedback(action)
|
||||||
|
assert len(teleop.arm.role_commands) == role_count
|
||||||
|
assert teleop.arm.last_joint == (1000, 2000, 3000, 4000, 5000, 6000)
|
||||||
|
assert teleop.arm.last_gripper[0] == 7000
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
teleop.set_manual_control(True)
|
||||||
|
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
|
||||||
|
assert teleop.arm.joint_mit_calls == []
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def test_piper_leader_policy_mode_reads_feedback(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
||||||
|
|
||||||
|
teleop.connect()
|
||||||
try:
|
try:
|
||||||
action = teleop.get_action()
|
action = teleop.get_action()
|
||||||
assert action["joint_1.pos"] == 11.0
|
assert action["joint_1.pos"] == 11.0
|
||||||
@ -391,134 +408,51 @@ def test_piper_leader_manual_control_prefers_feedback_state(monkeypatch):
|
|||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test_piper_leader_manual_control_waits_for_fresh_feedback(monkeypatch):
|
def test_piper_leader_returns_empty_action_without_manual_frames(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||||
teleop = PiperLeader(
|
teleop.connect()
|
||||||
PiperLeaderConfig(
|
|
||||||
port="can1",
|
|
||||||
manual_control=True,
|
|
||||||
require_calibration=False,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
|
||||||
try:
|
try:
|
||||||
initial = teleop.get_action()
|
teleop.arm._joint_ctrl.time_stamp = 0.0
|
||||||
assert initial["joint_1.pos"] == 11.0
|
assert teleop.get_action() == {}
|
||||||
|
|
||||||
def publish_fresh_feedback():
|
|
||||||
time.sleep(0.01)
|
|
||||||
teleop.arm._joint_state.time_stamp = 2.0
|
|
||||||
teleop.arm._joint_state.joint_state.joint_1 = 12000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_2 = 22000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_3 = 32000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_4 = 42000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_5 = 52000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_6 = 62000
|
|
||||||
teleop.arm._gripper_state.time_stamp = 2.0
|
|
||||||
teleop.arm._gripper_state.gripper_state.grippers_angle = 44000
|
|
||||||
|
|
||||||
publisher = threading.Thread(target=publish_fresh_feedback, daemon=True)
|
|
||||||
publisher.start()
|
|
||||||
|
|
||||||
t0 = time.monotonic()
|
|
||||||
action = teleop.get_action()
|
|
||||||
publisher.join(timeout=0.1)
|
|
||||||
|
|
||||||
assert time.monotonic() - t0 >= 0.008
|
|
||||||
assert action["joint_1.pos"] == 12.0
|
|
||||||
assert action["joint_2.pos"] == 22.0
|
|
||||||
assert action["gripper.pos"] == 44.0
|
|
||||||
finally:
|
finally:
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test_piper_leader_manual_control_does_not_unblock_on_gripper_feedback_only(monkeypatch):
|
def test_piper_leader_failed_policy_switch_restores_manual_role(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", enable_timeout_s=0.0))
|
||||||
teleop = PiperLeader(
|
teleop.connect()
|
||||||
PiperLeaderConfig(
|
|
||||||
port="can1",
|
|
||||||
manual_control=True,
|
|
||||||
require_calibration=False,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
|
||||||
try:
|
try:
|
||||||
initial = teleop.get_action()
|
with pytest.raises(RuntimeError, match="did not enable"):
|
||||||
assert initial["joint_1.pos"] == 11.0
|
teleop.set_manual_control(False)
|
||||||
|
assert teleop._manual_control_enabled is True
|
||||||
def publish_feedback():
|
assert teleop.arm.role_commands[-1][0] == 0xFA
|
||||||
time.sleep(0.005)
|
|
||||||
teleop.arm._gripper_state.time_stamp = 2.0
|
|
||||||
teleop.arm._gripper_state.gripper_state.grippers_angle = 45000
|
|
||||||
time.sleep(0.01)
|
|
||||||
teleop.arm._joint_state.time_stamp = 2.0
|
|
||||||
teleop.arm._joint_state.joint_state.joint_1 = 13000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_2 = 23000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_3 = 33000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_4 = 43000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_5 = 53000
|
|
||||||
teleop.arm._joint_state.joint_state.joint_6 = 63000
|
|
||||||
|
|
||||||
publisher = threading.Thread(target=publish_feedback, daemon=True)
|
|
||||||
publisher.start()
|
|
||||||
|
|
||||||
t0 = time.monotonic()
|
|
||||||
action = teleop.get_action()
|
|
||||||
publisher.join(timeout=0.1)
|
|
||||||
|
|
||||||
assert time.monotonic() - t0 >= 0.013
|
|
||||||
assert action["joint_1.pos"] == 13.0
|
|
||||||
finally:
|
finally:
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path):
|
def test_piper_leader_disconnect_disables_if_role_restore_fails(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
||||||
|
teleop.connect()
|
||||||
|
|
||||||
robot = PiperFollower(
|
def fail_role_restore(*args):
|
||||||
PiperFollowerConfig(
|
raise RuntimeError("leader role restore failed")
|
||||||
port="can0",
|
|
||||||
id="connect_reenable_after_calibration",
|
|
||||||
calibration_dir=tmp_path,
|
|
||||||
require_calibration=True,
|
|
||||||
enable_on_connect=True,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
def fake_calibrate():
|
monkeypatch.setattr(teleop.arm, "MasterSlaveConfig", fail_role_restore)
|
||||||
# Mirror real calibration's drag-mode behavior.
|
|
||||||
robot.arm.DisableArm(7)
|
|
||||||
robot.calibration = make_identity_calibration()
|
|
||||||
|
|
||||||
monkeypatch.setattr(robot, "calibrate", fake_calibrate)
|
with pytest.raises(RuntimeError, match="leader role restore failed"):
|
||||||
|
teleop.disconnect()
|
||||||
robot.connect(calibrate=True)
|
assert teleop.arm.disable_calls == 1
|
||||||
try:
|
assert not teleop.arm.connected
|
||||||
assert robot.arm.disable_calls == 1
|
|
||||||
assert robot.arm.enable_calls == 1
|
|
||||||
assert robot.arm.is_enabled
|
|
||||||
finally:
|
|
||||||
robot.disconnect()
|
|
||||||
|
|
||||||
|
|
||||||
def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, tmp_path):
|
def test_piper_follower_connect_enables_arm(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
robot = PiperFollower(PiperFollowerConfig(port="can0", enable_on_connect=True))
|
||||||
|
|
||||||
robot = PiperFollower(
|
robot.connect()
|
||||||
PiperFollowerConfig(
|
|
||||||
port="can0",
|
|
||||||
id="connect_enable_without_calibration",
|
|
||||||
calibration_dir=tmp_path,
|
|
||||||
require_calibration=True,
|
|
||||||
enable_on_connect=True,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
robot.connect(calibrate=False)
|
|
||||||
try:
|
try:
|
||||||
assert robot.arm.enable_calls == 1
|
assert robot.arm.enable_calls == 1
|
||||||
assert robot.arm.is_enabled
|
assert robot.arm.is_enabled
|
||||||
@ -527,90 +461,17 @@ def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, t
|
|||||||
robot.disconnect()
|
robot.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test_piper_leader_gravity_comp_manual_control_uses_mit(monkeypatch):
|
def test_piper_follower_connect_sets_follower_role_without_restart(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
device = PiperFollower(PiperFollowerConfig(port="can0"))
|
||||||
class FakeModel:
|
|
||||||
def __init__(self):
|
|
||||||
self.nq = 6
|
|
||||||
self.nv = 6
|
|
||||||
self.gravity = SimpleNamespace(linear=None)
|
|
||||||
|
|
||||||
def createData(self):
|
|
||||||
return object()
|
|
||||||
|
|
||||||
class FakeRobotWrapper:
|
|
||||||
@staticmethod
|
|
||||||
def BuildFromURDF(urdf_path, package_dirs):
|
|
||||||
del urdf_path, package_dirs
|
|
||||||
return SimpleNamespace(model=FakeModel(), data=object())
|
|
||||||
|
|
||||||
class FakePinocchio:
|
|
||||||
RobotWrapper = FakeRobotWrapper
|
|
||||||
|
|
||||||
@staticmethod
|
|
||||||
def rnea(model, data, q, v, a):
|
|
||||||
del model, data, q, v, a
|
|
||||||
return [0.1] * 6
|
|
||||||
|
|
||||||
monkeypatch.setitem(sys.modules, "pinocchio", FakePinocchio)
|
|
||||||
|
|
||||||
teleop = PiperLeader(
|
|
||||||
PiperLeaderConfig(
|
|
||||||
port="can1",
|
|
||||||
manual_control=True,
|
|
||||||
gravity_comp_control_hz=200.0,
|
|
||||||
mode_refresh_interval_s=0.01,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
teleop.calibration = make_identity_calibration()
|
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
|
||||||
try:
|
|
||||||
assert wait_until(lambda: len(teleop.arm.joint_mit_calls) > 0)
|
|
||||||
assert teleop.arm.disable_calls == 0
|
|
||||||
assert len(teleop.arm.joint_mit_calls) > 0
|
|
||||||
assert len(teleop.arm.gripper_calls) > 0
|
|
||||||
assert teleop.arm.gripper_calls[0][2] == 0x00
|
|
||||||
|
|
||||||
teleop.set_manual_control(False)
|
|
||||||
calls_after_stop = len(teleop.arm.joint_mit_calls)
|
|
||||||
time.sleep(0.03)
|
|
||||||
assert len(teleop.arm.joint_mit_calls) == calls_after_stop
|
|
||||||
assert teleop.arm.gripper_calls[-1][2] == 0x01
|
|
||||||
finally:
|
|
||||||
teleop.disconnect()
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize(
|
|
||||||
("device_kind", "port", "robot_id"),
|
|
||||||
[("leader", "can1", None), ("follower", "can0", "piper_role_guard")],
|
|
||||||
)
|
|
||||||
def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode(
|
|
||||||
monkeypatch, device_kind, port, robot_id
|
|
||||||
):
|
|
||||||
patch_fake_sdk(monkeypatch)
|
|
||||||
|
|
||||||
if device_kind == "leader":
|
|
||||||
device = PiperLeader(PiperLeaderConfig(port=port))
|
|
||||||
else:
|
|
||||||
device = PiperFollower(PiperFollowerConfig(port=port, id=robot_id))
|
|
||||||
device.arm._arm_status.arm_status.ctrl_mode = 0x06
|
device.arm._arm_status.arm_status.ctrl_mode = 0x06
|
||||||
|
|
||||||
with pytest.raises(RuntimeError, match="Follower role command .* sent.*Power-cycle"):
|
device.connect()
|
||||||
device.connect(calibrate=False)
|
try:
|
||||||
|
assert device.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
|
||||||
assert device.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
|
assert [name for name, _ in device.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
|
||||||
|
finally:
|
||||||
|
device.disconnect()
|
||||||
def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
|
|
||||||
pointer_file = tmp_path / "lfs_pointer.urdf"
|
|
||||||
pointer_file.write_text("version https://git-lfs.github.com/spec/v1\noid sha256:deadbeef\nsize 123\n")
|
|
||||||
|
|
||||||
with pytest.raises(RuntimeError, match="Git LFS pointer files"):
|
|
||||||
piper_leader_module._ensure_not_lfs_pointer(
|
|
||||||
pointer_file, "assets/piper_description/urdf/pointer.urdf"
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize(
|
@pytest.mark.parametrize(
|
||||||
@ -627,8 +488,8 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
|
|||||||
[
|
[
|
||||||
(
|
(
|
||||||
BiPiperLeaderConfig(
|
BiPiperLeaderConfig(
|
||||||
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
|
||||||
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
|
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
|
||||||
process_isolation=False,
|
process_isolation=False,
|
||||||
),
|
),
|
||||||
BiPiperFollowerConfig(
|
BiPiperFollowerConfig(
|
||||||
@ -644,8 +505,8 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
|
|||||||
),
|
),
|
||||||
(
|
(
|
||||||
BiPiperXLeaderConfig(
|
BiPiperXLeaderConfig(
|
||||||
left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
|
||||||
right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
|
right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
|
||||||
process_isolation=False,
|
process_isolation=False,
|
||||||
),
|
),
|
||||||
BiPiperXFollowerConfig(
|
BiPiperXFollowerConfig(
|
||||||
@ -684,14 +545,11 @@ def test_bimanual_piper_leader_follower_roundtrip(
|
|||||||
assert isinstance(robot.left_arm, left_robot_cls)
|
assert isinstance(robot.left_arm, left_robot_cls)
|
||||||
assert isinstance(robot.right_arm, right_robot_cls)
|
assert isinstance(robot.right_arm, right_robot_cls)
|
||||||
|
|
||||||
teleop.left_arm.calibration = make_identity_calibration()
|
teleop.connect()
|
||||||
teleop.right_arm.calibration = make_identity_calibration()
|
robot.connect()
|
||||||
robot.left_arm.calibration = make_identity_calibration()
|
|
||||||
robot.right_arm.calibration = make_identity_calibration()
|
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
|
||||||
robot.connect(calibrate=False)
|
|
||||||
try:
|
try:
|
||||||
|
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
|
||||||
|
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
|
||||||
action = teleop.get_action()
|
action = teleop.get_action()
|
||||||
assert "left_joint_1.pos" in action
|
assert "left_joint_1.pos" in action
|
||||||
assert "right_joint_1.pos" in action
|
assert "right_joint_1.pos" in action
|
||||||
@ -714,6 +572,8 @@ def test_bimanual_piper_leader_follower_roundtrip(
|
|||||||
assert obs["right_gripper.pos"] == 43.0
|
assert obs["right_gripper.pos"] == 43.0
|
||||||
|
|
||||||
teleop.send_feedback(action)
|
teleop.send_feedback(action)
|
||||||
|
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFC
|
||||||
|
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFC
|
||||||
assert teleop.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
assert teleop.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||||
assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||||
assert teleop.left_arm.arm.last_gripper[0] == 42000
|
assert teleop.left_arm.arm.last_gripper[0] == 42000
|
||||||
@ -721,6 +581,29 @@ def test_bimanual_piper_leader_follower_roundtrip(
|
|||||||
finally:
|
finally:
|
||||||
teleop.disconnect()
|
teleop.disconnect()
|
||||||
robot.disconnect()
|
robot.disconnect()
|
||||||
|
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
|
||||||
|
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
|
||||||
|
|
||||||
|
|
||||||
|
def test_bimanual_piper_get_action_returns_available_side(monkeypatch):
|
||||||
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
|
teleop = make_teleoperator_from_config(
|
||||||
|
BiPiperLeaderConfig(
|
||||||
|
left_arm_config=PiperLeaderConfigBase(port="can2"),
|
||||||
|
right_arm_config=PiperLeaderConfigBase(port="can3"),
|
||||||
|
process_isolation=False,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
teleop.connect()
|
||||||
|
try:
|
||||||
|
teleop.right_arm.arm._joint_ctrl.time_stamp = 0.0
|
||||||
|
action = teleop.get_action()
|
||||||
|
assert "left_joint_1.pos" in action
|
||||||
|
assert "right_joint_1.pos" not in action
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test_bimanual_piper_follower_action_features_are_available_without_connect(monkeypatch):
|
def test_bimanual_piper_follower_action_features_are_available_without_connect(monkeypatch):
|
||||||
@ -746,8 +629,8 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
|
|||||||
|
|
||||||
teleop = make_teleoperator_from_config(
|
teleop = make_teleoperator_from_config(
|
||||||
BiPiperLeaderConfig(
|
BiPiperLeaderConfig(
|
||||||
left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=False, sync_gripper=True),
|
left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=True, sync_gripper=True),
|
||||||
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
|
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
|
||||||
process_isolation=False,
|
process_isolation=False,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
@ -758,18 +641,13 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
|
|||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
teleop.left_arm.calibration = make_identity_calibration()
|
|
||||||
teleop.right_arm.calibration = make_identity_calibration()
|
|
||||||
robot.left_arm.calibration = make_identity_calibration()
|
|
||||||
robot.right_arm.calibration = make_identity_calibration()
|
|
||||||
|
|
||||||
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
|
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
|
||||||
robot.get_observation = lambda: (_ for _ in ()).throw(
|
robot.get_observation = lambda: (_ for _ in ()).throw(
|
||||||
AssertionError("teleop_loop should not fetch observation for identity processors without display")
|
AssertionError("teleop_loop should not fetch observation for identity processors without display")
|
||||||
)
|
)
|
||||||
|
|
||||||
teleop.connect(calibrate=False)
|
teleop.connect()
|
||||||
robot.connect(calibrate=False)
|
robot.connect()
|
||||||
try:
|
try:
|
||||||
teleop_loop(
|
teleop_loop(
|
||||||
teleop=teleop,
|
teleop=teleop,
|
||||||
@ -797,18 +675,13 @@ def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
|
|||||||
self.arm_cls = arm_cls
|
self.arm_cls = arm_cls
|
||||||
self.arm_config = arm_config
|
self.arm_config = arm_config
|
||||||
self.is_connected = False
|
self.is_connected = False
|
||||||
self.is_calibrated = True
|
|
||||||
|
|
||||||
action_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
action_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
||||||
feedback_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
feedback_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
|
||||||
|
|
||||||
def connect(self, calibrate=True):
|
def connect(self):
|
||||||
del calibrate
|
|
||||||
self.is_connected = True
|
self.is_connected = True
|
||||||
|
|
||||||
def calibrate(self):
|
|
||||||
pass
|
|
||||||
|
|
||||||
def configure(self):
|
def configure(self):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
@ -838,79 +711,3 @@ def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
|
|||||||
|
|
||||||
assert isinstance(teleop.left_arm, DummyProxy)
|
assert isinstance(teleop.left_arm, DummyProxy)
|
||||||
assert isinstance(teleop.right_arm, DummyProxy)
|
assert isinstance(teleop.right_arm, DummyProxy)
|
||||||
|
|
||||||
|
|
||||||
def test_piper_follower_send_only_mode_skips_sdk_reader_threads(monkeypatch):
|
|
||||||
patch_fake_sdk(monkeypatch)
|
|
||||||
|
|
||||||
robot = make_robot_from_config(
|
|
||||||
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
|
|
||||||
)
|
|
||||||
robot.calibration = make_identity_calibration()
|
|
||||||
robot.set_teleop_send_only_mode(True)
|
|
||||||
|
|
||||||
robot.connect(calibrate=False)
|
|
||||||
try:
|
|
||||||
assert robot.arm.connect_calls[-1]["start_thread"] is False
|
|
||||||
with pytest.raises(RuntimeError, match="send-only mode"):
|
|
||||||
robot.get_observation()
|
|
||||||
finally:
|
|
||||||
robot.disconnect()
|
|
||||||
|
|
||||||
|
|
||||||
def test_piper_follower_send_only_mode_skips_camera_connects_and_stays_connected(monkeypatch):
|
|
||||||
patch_fake_sdk(monkeypatch)
|
|
||||||
|
|
||||||
class FakeCamera:
|
|
||||||
def __init__(self):
|
|
||||||
self.is_connected = False
|
|
||||||
self.connect_calls = 0
|
|
||||||
self.disconnect_calls = 0
|
|
||||||
|
|
||||||
def connect(self):
|
|
||||||
self.connect_calls += 1
|
|
||||||
self.is_connected = True
|
|
||||||
|
|
||||||
def disconnect(self):
|
|
||||||
self.disconnect_calls += 1
|
|
||||||
self.is_connected = False
|
|
||||||
|
|
||||||
camera = FakeCamera()
|
|
||||||
monkeypatch.setattr(piper_follower_module, "make_cameras_from_configs", lambda _: {"front": camera})
|
|
||||||
|
|
||||||
robot = make_robot_from_config(
|
|
||||||
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
|
|
||||||
)
|
|
||||||
robot.calibration = make_identity_calibration()
|
|
||||||
robot.set_teleop_send_only_mode(True)
|
|
||||||
|
|
||||||
robot.connect(calibrate=False)
|
|
||||||
try:
|
|
||||||
assert robot.is_connected
|
|
||||||
assert camera.connect_calls == 0
|
|
||||||
assert not camera.is_connected
|
|
||||||
finally:
|
|
||||||
robot.disconnect()
|
|
||||||
|
|
||||||
assert camera.disconnect_calls == 0
|
|
||||||
|
|
||||||
|
|
||||||
def test_bimanual_piper_send_only_mode_propagates_to_both_followers(monkeypatch):
|
|
||||||
patch_fake_sdk(monkeypatch)
|
|
||||||
|
|
||||||
robot = make_robot_from_config(
|
|
||||||
BiPiperFollowerConfig(
|
|
||||||
left_arm_config=PiperFollowerConfigBase(port="can3", sync_gripper=True, require_calibration=False),
|
|
||||||
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True, require_calibration=False),
|
|
||||||
)
|
|
||||||
)
|
|
||||||
robot.left_arm.calibration = make_identity_calibration()
|
|
||||||
robot.right_arm.calibration = make_identity_calibration()
|
|
||||||
robot.set_teleop_send_only_mode(True)
|
|
||||||
|
|
||||||
robot.connect(calibrate=False)
|
|
||||||
try:
|
|
||||||
assert robot.left_arm.arm.connect_calls[-1]["start_thread"] is False
|
|
||||||
assert robot.right_arm.arm.connect_calls[-1]["start_thread"] is False
|
|
||||||
finally:
|
|
||||||
robot.disconnect()
|
|
||||||
|
|||||||
@ -0,0 +1,34 @@
|
|||||||
|
from types import SimpleNamespace
|
||||||
|
|
||||||
|
import lerobot.scripts.lerobot_setup_can as setup_can_module
|
||||||
|
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||||
|
|
||||||
|
|
||||||
|
def test_resolve_piper_can_interface(monkeypatch, tmp_path):
|
||||||
|
interface_path = tmp_path / "can2"
|
||||||
|
interface_path.mkdir()
|
||||||
|
(interface_path / "type").write_text("280\n")
|
||||||
|
monkeypatch.setattr(piper_sdk_utils, "_SYS_CLASS_NET", tmp_path)
|
||||||
|
monkeypatch.setattr(
|
||||||
|
piper_sdk_utils.subprocess,
|
||||||
|
"run",
|
||||||
|
lambda *args, **kwargs: SimpleNamespace(stdout="ID_SERIAL_SHORT=SERIAL_A\n"),
|
||||||
|
)
|
||||||
|
|
||||||
|
assert piper_sdk_utils.resolve_piper_can_interface("SERIAL_A") == "can2"
|
||||||
|
|
||||||
|
|
||||||
|
def test_setup_can_uses_classic_can_for_usb_serials(monkeypatch):
|
||||||
|
monkeypatch.setattr(setup_can_module, "resolve_piper_can_interface", lambda serial: "can2")
|
||||||
|
attempts = []
|
||||||
|
|
||||||
|
def fake_setup(interface, bitrate, data_bitrate, use_fd):
|
||||||
|
attempts.append((interface, bitrate, use_fd))
|
||||||
|
return False, "failed"
|
||||||
|
|
||||||
|
monkeypatch.setattr(setup_can_module, "setup_interface", fake_setup)
|
||||||
|
|
||||||
|
cfg = setup_can_module.CANSetupConfig(mode="setup", usb_can_serials="SERIAL_A")
|
||||||
|
setup_can_module.setup_interface_with_fallback(cfg.get_interfaces()[0], cfg)
|
||||||
|
|
||||||
|
assert attempts == [("can2", 1000000, False)]
|
||||||
在新工单中引用
屏蔽一个用户