Piper robot integration #645

Ke-Wang1017 · 2025-01-17T11:03:11Z

What this does

This PR integrates PIPER robot (https://global.agilex.ai/products/piper) into lerobot, so that people can teleoperate, collect demos, visualize and train policy with lerobot functions for PIPER.

Piper SDK is needed: https://github.com/agilexrobotics/piper_sdk

How it was tested

First activate the CAN bus: bash can_activate.sh can0 1000000
Teleoperation:
python lerobot/scripts/control_robot.py teleoperate --robot-path lerobot/configs/robot/piper.yaml
Recording:
python lerobot/scripts/control_robot.py record --robot-path lerobot/configs/robot/piper.yam --fps 30 --repo-id $USER/piper_test --single-task True --num-episodes 50 --run-compute-stats 0
Data Visualization:
python lerobot/scripts/visualize_dataset_html.py --repo-id $USER/piper_test

Examples:

Added piper.yaml in configs/robot/.
Added piper.py in common/robot_devices/robots
Added joystick_interface.py in common/robot_devices/robots

Things to be done:

Add leader/follower arm config, currently one arm
Fix joint naming

helper2424 · 2025-01-17T11:19:01Z

@Ke-Wang1017 that's very cool. You are 🚀

I can't test it because I don't have those hands, but can check the code.

I think that need to cleanup a little bit PR, because it has a lot of already removed commits from main. If you need assistance with it - I can help.

Ke-Wang1017 · 2025-01-17T11:53:32Z

@Ke-Wang1017 that's very cool. You are 🚀

I can't test it because I don't have those hands, but can check the code.

I think that need to cleanup a little bit PR, because it has a lot of already removed commits from main. If you need assistance with it - I can help.

Hi @helper2424 thank you so much, yes I just realized that some commits are from hil-serl port. It will be great if you are willing to help :)

- Implemented `images_to_video` function to convert image sequences into video files. - Added `save_demonstration_video` function to save demonstration images as a video. - Integrated video saving functionality into the camera control script, allowing for automatic video creation from captured images. - Introduced `play_video` function to enable playback of saved video files using OpenCV. - Updated imports in relevant files to include new video processing utilities. These enhancements improve the functionality for recording and visualizing robot demonstrations.

…ation - Improved the `images_to_video` and `save_demonstration_video` functions for better video creation from image sequences. - Enhanced the camera control script to automatically save captured images as videos. - Added `play_video` functionality for playback of saved videos using OpenCV. - Cleaned up imports to ensure all necessary video processing utilities are included. These updates streamline the video recording and visualization process for robot demonstrations.

…eleoperation - Updated `JoystickInterface` to include a comment for the BTN_EAST button functionality. - Introduced `JoystickIntervention` class to manage joystick actions and gripper control, improving the interaction with the robot. - Modified `PiperRobot` to utilize `JoystickIntervention` for teleoperation, enhancing the robot's control capabilities. - Added a `Rate` class to manage execution frequency for smoother operation. - Refactored the `send_action` method to handle actions more effectively and updated the robot's connection logic. - Removed the obsolete `control_robot_piper.py` script to streamline the codebase. These changes improve the joystick control system and overall robot teleoperation experience.

- Added signal handling in the Logger class for graceful shutdown of wandb logging. - Updated control_utils.py to include additional robot type checks and modified video writer codec for better compatibility. - Improved joystick interface with error handling during process termination and added comments for clarity. - Introduced a LowPassFilter class in PiperRobot for smoother control input handling. - Updated PiperRobot configuration to include leader arm definitions and adjusted camera settings. - Disabled camera display by default in control_robot.py to prevent freezing issues. These changes improve the robustness and usability of the robot control system, enhancing both logging and operational stability.

- Added a check in the PiperRobot class to convert action inputs from torch.Tensor to list format, ensuring compatibility with existing processing logic. - This change improves the flexibility of the robot's action handling, allowing for seamless integration of tensor-based inputs.

- Added debug print statements in the Intel RealSense camera detection to log serial numbers and names of detected cameras. - Refactored the LowPassFilter class in PiperRobot to accept a single orientation parameter instead of separate rx, ry, and rz values, improving code clarity. - Updated the PiperRobot class to apply the LowPassFilter to the robot's state during action execution, enhancing control stability. - Modified the PiperRobot configuration to replace the wrist camera with an Intel RealSense D415, ensuring better camera integration and settings. These changes improve the robot's camera handling and overall control system stability.

- Replaced the LowPassFilter class with a new EulerAngles class to improve signal rectification for orientation data. - Introduced a rectify_signal function to handle signal adjustments based on previous values, enhancing the accuracy of orientation readings. - Updated PiperRobot to utilize the new rectification method during state updates, ensuring more stable control during operation. These changes enhance the robot's orientation handling and improve overall control stability.

- Modified the `rectify_signal` function to adjust the threshold for signal rectification from 350000 to 180000, enhancing the sensitivity of signal adjustments. - This change aims to improve the accuracy of signal handling in the PiperRobot, contributing to better overall control and stability during operation.

helper2424 · 2025-01-20T15:50:26Z

@Ke-Wang1017 here is a clean version of your PR #648

I have extracted only changes related to Piper integration. You can use this branch in your PR

helper2424

I have added some comments on the part that checked. I will add more later.

helper2424 · 2025-01-20T16:15:23Z

lerobot/common/logger.py

@@ -23,6 +23,8 @@
 import re
 from glob import glob
 from pathlib import Path
+import signal
+from typing import Optional


Probably we don't need it here.

helper2424 · 2025-01-20T16:20:43Z

lerobot/common/robot_devices/cameras/intelrealsense.py

@@ -42,6 +42,7 @@ def find_cameras(raise_when_empty=True, mock=False) -> list[dict]:
    for device in rs.context().query_devices():
        serial_number = int(device.get_info(rs.camera_info(SERIAL_NUMBER_INDEX)))
        name = device.get_info(rs.camera_info.name)
+        print(f"{serial_number=} {name=}")


Looks like some debug. Maybe we can delete it.

lerobot/scripts/control_robot.py

helper2424 · 2025-01-20T18:00:16Z

lerobot/common/robot_devices/robots/piper.py

+import torch
+import numpy as np
+from lerobot.common.robot_devices.robots.joystick_interface import JoystickIntervention, ControllerType
+from piper_sdk import *


It seems that need to add it as dependency to the poetry. So it could be installed via pip or poetry.

Also, it should be hidden in the extra block to avoid having troubles with installation. Not everybody have piper.

Also, the import should be hidden inside the class, by the same reason.

Right, do you have suggestions how to do this properly? Will be great if you can do this part

@Ke-Wang1017 yeah, let me do it.

The Fix is here - Ke-Wang1017#2

…n_fix_imports Piper. Fix imports

helper2424

I have added bunch of comments

helper2424 · 2025-01-24T08:17:08Z

lerobot/common/robot_devices/robots/joystick_interface.py

+        self.controller_config = self.CONTROLLER_CONFIGS[controller_type]
+
+        # Manager to handle shared state between processes
+        self.manager = multiprocessing.Manager()


Nice approach, I haven't used it a lot in production - but looks great.

Yeah it is nice, never had issues using for 3 different computers, although you can use multithreads as well

helper2424 · 2025-01-24T08:22:41Z

lerobot/common/robot_devices/robots/joystick_interface.py

+
+    def get_action(self):
+        """Returns the latest action and button state from the Joystick."""
+        action = self.latest_data["action"]


The latest_data keep just a last state, right?

If I call get_action one time per 10 seconds and during these 10 seconds do a lot of changes then I will receive only last state at the end of 10 seconds, right? Also, it should be noblocking, right?

Yes this is the intention

helper2424 · 2025-01-24T08:24:02Z

lerobot/common/robot_devices/robots/joystick_interface.py

+        try:
+            if hasattr(self, '_process') and self._process is not None:
+                import signal
+                self._process.terminate()


I see that it's initialized as self.process, and not as self._process. It looks like a mistake

You are right, I have fixed that in a new commit

helper2424 · 2025-01-24T08:56:08Z

lerobot/common/robot_devices/robots/joystick_interface.py

+    resolution: dict
+    scale: dict
+
+class JoystickInterface:


I have tried to check these class with my PS5

>>> from lerobot.common.robot_devices.robots.joystick_interface import JoystickInterface >>> intr = JoystickInterface() Traceback (most recent call last): File "<stdin>", line 1, in <module> File "/Users/helper2424/Documents/lerobot/lerobot/common/robot_devices/robots/joystick_interface.py", line 84, in __init__ self.process.start() File "/Users/helper2424/.pyenv/versions/3.10.15/lib/python3.10/multiprocessing/process.py", line 121, in start self._popen = self._Popen(self) File "/Users/helper2424/.pyenv/versions/3.10.15/lib/python3.10/multiprocessing/context.py", line 224, in _Popen return _default_context.get_context().Process._Popen(process_obj) File "/Users/helper2424/.pyenv/versions/3.10.15/lib/python3.10/multiprocessing/context.py", line 288, in _Popen return Popen(process_obj) File "/Users/helper2424/.pyenv/versions/3.10.15/lib/python3.10/multiprocessing/popen_spawn_posix.py", line 32, in __init__ super().__init__(process_obj) File "/Users/helper2424/.pyenv/versions/3.10.15/lib/python3.10/multiprocessing/popen_fork.py", line 19, in __init__ self._launch(process_obj) File "/Users/helper2424/.pyenv/versions/3.10.15/lib/python3.10/multiprocessing/popen_spawn_posix.py", line 47, in _launch reduction.dump(process_obj, fp) File "/Users/helper2424/.pyenv/versions/3.10.15/lib/python3.10/multiprocessing/reduction.py", line 60, in dump ForkingPickler(file, protocol).dump(obj) TypeError: cannot pickle 'weakref.ReferenceType' object >>>

Any idea how to fix? ChatGPT suggest using queue instead of manager for sharing data between processes

Hmm I haven't tried PS5 and only use XBOX, maybe @jackvial can help with this?

helper2424 · 2025-01-24T08:57:08Z

lerobot/common/robot_devices/robots/piper.py

+        self.rate = Rate(200)
+        self.euler_filter = EulerAngles()
+        # init piper robot
+        self.piper = C_PiperInterface("can0")


Can be something else except can0? Maybe it should be a parameter?

That is a good idea, if there is only one arm then can0, if two arms can be different

helper2424 · 2025-01-24T08:59:45Z

lerobot/common/robot_devices/robots/joystick_interface.py

+                self._process = None
+
+
+class JoystickIntervention():


A very minor comment. I would add tests for all new classes. They could cover not all cases, but maybe some basic successful flow.

That's really helpful, thanks @helper2424

…ptation

- Added a new `move_to_home_2()` method for returning to home position after demo - Updated home position coordinates in `move_to_home()` method - Changed top camera index from 2 to 8 in Piper configuration - Modified `control_robot.py` to use `move_to_home_2()` when resetting Piper robot - Simplified import of Piper SDK by using wildcard import

Ke-Wang1017 added 8 commits January 20, 2025 22:43

helper2424 mentioned this pull request Jan 20, 2025

Piper adaptation new #648

Closed

helper2424 reviewed Jan 20, 2025

View reviewed changes

Ke-Wang1017 force-pushed the piper_adaptation branch from 5a45b3a to 4abf2c8 Compare January 22, 2025 09:59

helper2424 and others added 3 commits January 22, 2025 18:16

Add piper poetry deps

e5fdb3e

Fixup import for piper sdk

081aceb

Merge pull request #2 from helper2424/user/helper2424/piper_adaptatio…

fc04c9d

…n_fix_imports Piper. Fix imports

helper2424 reviewed Jan 24, 2025

View reviewed changes

Ke-Wang1017 added 3 commits January 24, 2025 11:41

fix the self._process

482ec0a

Merge remote-tracking branch 'origin/piper_adaptation' into piper_ada…

0b375e7

…ptation

CarolinePascal added enhancement Suggestions for new features or improvements robots Issues concerning robots HW interfaces labels Apr 14, 2025

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Piper robot integration #645

Piper robot integration #645

Piper robot integration #645

Are you sure you want to change the base?

Piper robot integration #645

Conversation

What this does

How it was tested

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment