Add support for Stretch (hello-robot) #409

aliberts · 2024-09-04T13:19:55Z

What this does

Adds native support for Stretch 3

Adds StretchRobot and associated config for a standard Stretch 3
Adds support for rotating images from camera streams (some cameras on Stretch are mounted sideways)
Adds examples/8_use_stretch.md

TODO in this PR:

Teleop / record is ~~currently not~~ working when streaming Stretch's GUI through Moonlight
Try with tethered setup as well
Add eval mode
Add documentation

TODO in future PRs:

Add tests (add stretch in init.py)
Maybe save "real" robot's action instead of gamepad state as action?
Feature: Add the option to teleoperate during environment reset
Refactor: Move robot-specific logic of control_robot in their respective classes
Refactor: Naming the motors and controller's state in the recorded datasets and have them display nicely in independent columns on the visualization
Plumbing:
- Add observation.images shapes in datasets metadata
- Make sure those shapes are retrieved from there to properly size the policies' visual backbones and not from the robots config (the reason being that the width/height there can be wrong because of rotations)

How it was tested

I tested calibration, teleoperation and recoding in the 3 setups (tethered, untethered and ssh). I also tested replaying an episode with the tethered setup.

How to checkout & try? (for the reviewer)

Calibration (homing):

python lerobot/scripts/control_robot.py calibrate \
    --robot-path lerobot/configs/robot/stretch.yaml

Teleoperation:

python lerobot/scripts/control_robot.py teleoperate \
    --robot-path lerobot/configs/robot/stretch.yaml

Dataset recording:

python lerobot/scripts/control_robot.py record \
    --robot-path lerobot/configs/robot/stretch.yaml \
    --fps 30 \
    --root data \
    --repo-id ${HF_USER}/stretch_test \
    --tags stretch tutorial \
    --warmup-time-s 3 \
    --episode-time-s 40 \
    --reset-time-s 10 \
    --num-episodes 2 \
    --push-to-hub 0

Replay:

python lerobot/scripts/control_robot.py replay \
    --robot-path lerobot/configs/robot/stretch.yaml \
    --fps 30 \
    --root data \
    --repo-id ${HF_USER}/stretch_test \
    --episode 0

Cadene

First pass. Still reviewing

lerobot/common/datasets/compute_stats.py

lerobot/common/robot_devices/utils.py

Cadene · 2024-09-11T11:52:06Z

lerobot/common/robot_devices/cameras/intelrealsense.py

-def find_camera_indices(raise_when_empty=True) -> list[int]:
+def find_cameras_info(raise_when_empty=True) -> dict[int, str]:
    """
-    Find the serial numbers of the Intel RealSense cameras
+    Find the names and the serial numbers of the Intel RealSense cameras
    connected to the computer.
    """
-    camera_ids = []
+    cameras_info = {}
    for device in rs.context().query_devices():
        serial_number = int(device.get_info(rs.camera_info(SERIAL_NUMBER_INDEX)))
-        camera_ids.append(serial_number)
+        name = device.get_info(rs.camera_info.name)
+        cameras_info[serial_number] = name

-    if raise_when_empty and len(camera_ids) == 0:
+    if raise_when_empty and len(cameras_info) == 0:
        raise OSError(
            "Not a single camera was detected. Try re-plugging, or re-installing `librealsense` and its python wrapper `pyrealsense2`, or updating the firmware."
        )

-    return camera_ids
+    return cameras_info


I am not fan of modifying this function, to stay consistent with the one in opencv.py
Instead, could we have another function that given camera_indices / serial number retrieves the camera name?

The issue is that we would still need to iterate through rs.context().query_devices() in that function and compare device.get_info(rs.camera_info(SERIAL_NUMBER_INDEX)) against the given serial_number before accessing the name. We can do it but:

Would makes things harder to read and understand why it's doing those convolutions

Would be slower as you'd iterate twice on the devices

What can we do to unify opencv and intelrealsense on this function?

I would imagine find_cameras_info(raise_when_empty=True) -> list[dict]: for both, with camera_infos[0]["index"] being the camera index port for opencv or intelrealsense serial, and camera_infos[0]["name"] not existing for opencv.

Nit: camera_infos instead of cameras_info for consistency with camera_ids and how we usually add a s at the end for our variable names of type list

What can we do to unify opencv and intelrealsense on this function?

As we discussed, I think we should eventually converge to a single Camera class with different backends (realsense, opencv...). This would probably simplify the design and avoid having to monkeypatch those classes in the tests. Then we could think about the design of this function (which would be a single one). Wdyt?

Nit: camera_infos instead of cameras_info for consistency with camera_ids and how we usually add a s at the end for our variable names of type list

I wondered about the exact same thing but the plural of "info" or "information" in English is without a s.

I agree, but I think we should homogenise opencv and intelrealsense at this level before merging. What do you think of my suggestion?

I know but we dont write in english, we write in python (which is an english dialect) so the english grammar rules dont apply the same way ^^

lerobot/common/robot_devices/cameras/intelrealsense.py

lerobot/common/robot_devices/robots/factory.py

Co-authored-by: Remi <remi.cadene@huggingface.co>

…_03_add_stretch

hello-binit

Hey @aliberts, looks great! I've only done a read-through so far. I can run it on actual hardware later this week. I don't want to delay your PR, so don't feel obliged to wait.

examples/8_use_stretch.md

hello-binit · 2024-09-25T15:54:17Z

examples/8_use_stretch.md

+```
+This is essentially the same as running `stretch_gamepad_teleop.py`
+
+Store your Hugging Face repository name in a variable to run these commands:


Should there be a huggingface-cli login step?

Yes you're correct. Technically, it's already pointed out in the 7_get_started_with_real_robot.md as some other details and I didn't want to add to avoid too much redundancy. I've added a word on that (7c87025)
Eventually we might rearrange these markdown as the number of robots grow (cc @Cadene)

hello-binit · 2024-09-25T15:57:35Z

lerobot/common/robot_devices/cameras/intelrealsense.py

-    if camera_ids is None:
-        camera_ids = find_camera_indices()
+    if len(camera_ids) == 0:
+        camera_ids = find_cameras_info()

    print("Connecting cameras")
    cameras = []
    for cam_idx in camera_ids:


IMO, the variable should be renamed to cameras_info

I agree, this was because it was copied from a very similar function for opencv cameras (see this discussion).
I'll wait for that discussion to resolve before changing it.

hello-binit · 2024-09-25T16:01:43Z

lerobot/common/robot_devices/cameras/intelrealsense.py

@@ -237,6 +248,36 @@ def __init__(
        self.depth_map = None
        self.logs = {}

+        # TODO(alibets): Do we keep original width/height or do we define them after rotation?


Definitely nice to keep width/height consistent with the rotated image

hello-binit · 2024-09-25T16:35:10Z

lerobot/common/robot_devices/robots/stretch.py

+        self.is_connected = False
+        self.teleop = None
+        self.logs = {}
+        # TODO(aliberts): remove original low-level logging from stretch


You can add

https://github.com/hello-robot/stretch_body/blob/5ef6564dcdd93fb8120e44ad4d34017617210142/tools/bin/stretch_configure_tool.py#L2-L4

at the top of the file to raise the logging level. I might be able to also add a boolean to outright disable Stretch Body logging if it still interferes with your loggers. Let me know.

I've tried that in the past and it didn't work, but somehow since then the logs from Stretch have disappeared and I'm not exactly sure why 😅
I've added it e0fa5e4

That's interesting 😂. I'll let you know if I see them when I run it.

hello-binit · 2024-09-25T16:43:03Z

lerobot/common/robot_devices/robots/stretch.py

+        if self.state_keys is None:
+            self.state_keys = list(state)
+
+        if not record_data:
+            return
+
+        state = torch.as_tensor(list(state.values()))
+        action = torch.as_tensor(list(action.values()))
+
+        # Capture images from cameras
+        images = {}
+        for name in self.cameras:
+            before_camread_t = time.perf_counter()
+            images[name] = self.cameras[name].async_read()
+            images[name] = torch.from_numpy(images[name])
+            self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs["delta_timestamp_s"]
+            self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
+
+        # Populate output dictionnaries
+        obs_dict, action_dict = {}, {}
+        obs_dict["observation.state"] = state
+        action_dict["action"] = action
+        for name in self.cameras:
+            obs_dict[f"observation.images.{name}"] = images[name]


Lot of duplicate code. Could call self.get_observation() instead.

@Cadene I know we can't do that for send_action but after looking at it, I don't see why we can't simply call self.get_observation() in teleop_step. I've seen your comment in manipulator.py that get_observation doesn't return a batch dim but so does teleop_step.
Wdyt, am I missing something here?

You mean self.capture_observation() ?

I am fine with this duplicated code for now. We have the exact same pattern in manipulator.py. We will refactor at some point.

We can add a TODO(rcadene, aliberts): refactor with self.capture_observation()

hello-binit · 2024-09-25T16:44:51Z

lerobot/common/robot_devices/robots/stretch.py

+
+    def print_logs(self) -> None:
+        ...
+        # TODO(aliberts): move robot-specific logs logic here


for log in self.logs: self.logger.info(f"{log}: {self.logs[log]}")

i think

lerobot/common/robot_devices/utils.py

hello-binit · 2024-09-25T16:52:29Z

lerobot/configs/robot/stretch.yaml

+robot_type: stretch3
+
+cameras:
+  # TODO(aliberts): hardware currently not working, uncomment and test this when fixed


Did this get resolved? This is specific to your robot, right?

Replacement board is on the way ;)

Glad to hear it!

examples/8_use_stretch.md

Cadene

LGTM, almost ready to merge

Cadene · 2024-09-25T16:40:00Z

lerobot/common/robot_devices/robots/manipulator.py

+    def print_logs(self):
+        ...
+        # TODO(aliberts): move robot-specific logs logic here


should we use pass instead of ...?

Done eb48452

Cadene · 2024-09-25T16:41:52Z

lerobot/common/robot_devices/robots/stretch.py

+    def print_logs(self) -> None:
+        ...
+        # TODO(aliberts): move robot-specific logs logic here


Same pass?

Done eb48452

lerobot/common/robot_devices/robots/utils.py

pyproject.toml

.github/workflows/test.yml

Cadene · 2024-09-26T09:03:19Z

lerobot/common/robot_devices/cameras/intelrealsense.py

-def find_camera_indices(raise_when_empty=True) -> list[int]:
+def find_cameras_info(raise_when_empty=True) -> dict[int, str]:
    """
-    Find the serial numbers of the Intel RealSense cameras
+    Find the names and the serial numbers of the Intel RealSense cameras
    connected to the computer.
    """
-    camera_ids = []
+    cameras_info = {}
    for device in rs.context().query_devices():
        serial_number = int(device.get_info(rs.camera_info(SERIAL_NUMBER_INDEX)))
-        camera_ids.append(serial_number)
+        name = device.get_info(rs.camera_info.name)
+        cameras_info[serial_number] = name

-    if raise_when_empty and len(camera_ids) == 0:
+    if raise_when_empty and len(cameras_info) == 0:
        raise OSError(
            "Not a single camera was detected. Try re-plugging, or re-installing `librealsense` and its python wrapper `pyrealsense2`, or updating the firmware."
        )

-    return camera_ids
+    return cameras_info


What can we do to unify opencv and intelrealsense on this function?

I would imagine find_cameras_info(raise_when_empty=True) -> list[dict]: for both, with camera_infos[0]["index"] being the camera index port for opencv or intelrealsense serial, and camera_infos[0]["name"] not existing for opencv.

Cadene · 2024-09-26T09:04:46Z

lerobot/common/robot_devices/cameras/intelrealsense.py

-def find_camera_indices(raise_when_empty=True) -> list[int]:
+def find_cameras_info(raise_when_empty=True) -> dict[int, str]:
    """
-    Find the serial numbers of the Intel RealSense cameras
+    Find the names and the serial numbers of the Intel RealSense cameras
    connected to the computer.
    """
-    camera_ids = []
+    cameras_info = {}
    for device in rs.context().query_devices():
        serial_number = int(device.get_info(rs.camera_info(SERIAL_NUMBER_INDEX)))
-        camera_ids.append(serial_number)
+        name = device.get_info(rs.camera_info.name)
+        cameras_info[serial_number] = name

-    if raise_when_empty and len(camera_ids) == 0:
+    if raise_when_empty and len(cameras_info) == 0:
        raise OSError(
            "Not a single camera was detected. Try re-plugging, or re-installing `librealsense` and its python wrapper `pyrealsense2`, or updating the firmware."
        )

-    return camera_ids
+    return cameras_info


Nit: camera_infos instead of cameras_info for consistency with camera_ids and how we usually add a s at the end for our variable names of type list

Co-authored-by: Remi <remi.cadene@huggingface.co>

…_03_add_stretch

Co-authored-by: Remi <remi.cadene@huggingface.co> Co-authored-by: Remi Cadene <re.cadene@gmail.com>

aliberts added the 🌍 Real world Real-world robotics & controls label Sep 4, 2024

aliberts self-assigned this Sep 4, 2024

aliberts force-pushed the user/aliberts/2024_09_03_add_stretch branch 2 times, most recently from 2381002 to 4e531f0 Compare September 6, 2024 10:53

aliberts requested a review from Cadene September 10, 2024 17:11

aliberts marked this pull request as ready for review September 10, 2024 17:12

Cadene reviewed Sep 11, 2024

View reviewed changes

aliberts force-pushed the user/aliberts/2024_09_03_add_stretch branch from 50aa25c to dee3b0a Compare September 12, 2024 09:05

aliberts and others added 22 commits September 12, 2024 14:34

Add stretch_body

0e4b9e2

Fix poetry relax

7992d2e

Fix install

bea022f

Add stretch boilerplate

90e7895

Fix install

bf5b6e1

Add teleop

e5a4fa3

Fix '--camera-ids' empty, add init_from_name to IntelRealSenseCamera

2e36039

Add camera rotation

9a6150c

Add StretchRobotConfig, cameras obs

38f6be8

Add safe_disconnect

f2269c1

Add todo

8b28a3d

Reduce workers

e9682af

Fixes

4219527

Fix typo

aaf7fd4

Invert has_method args

2bf4dcb

Add nav cam, add rotation to opencv cams

f5651a5

Update lerobot/common/datasets/compute_stats.py

e755d10

Co-authored-by: Remi <remi.cadene@huggingface.co>

Fix install

a98556b

Fix CI install

6bc1c6b

Add copyrights

bd4fdc8

Remove default robot_type

12a2f2c

Remove StretchRobot import

16dd73f

aliberts force-pushed the user/aliberts/2024_09_03_add_stretch branch from dee3b0a to 88526d0 Compare September 12, 2024 12:34

aliberts added 8 commits September 12, 2024 16:28

Add send_action, type-hints

9e2d98c

Add todo

1053845

Merge remote-tracking branch 'origin/main' into user/aliberts/2024_09…

652eb01

…_03_add_stretch

Fixes

3fbd680

Update stretch-body

42d9556

Add pynput

db09ebf

Add connection errors

ddd8059

Add doc

d1fc630

aliberts requested a review from Cadene September 25, 2024 14:15

hello-binit reviewed Sep 25, 2024

View reviewed changes

aliberts added 2 commits September 26, 2024 10:23

Add logging step

7c87025

Change stretch log levels

e0fa5e4

Cadene reviewed Sep 26, 2024

View reviewed changes

aliberts and others added 10 commits September 26, 2024 11:48

Add system check

d5fbfd4

Update pyproject.toml

78e8435

Co-authored-by: Remi <remi.cadene@huggingface.co>

Add todo

37572eb

Co-authored-by: Remi <remi.cadene@huggingface.co>

Add suggestion

eb48452

Cleanup camera utils

722092a

Add nav cam

56d3845

Add more info in markdown

306a1ac

Add comment

c24b137

fps=20, homogenization intelrealsense opencv

2c0feed

Merge remote-tracking branch 'origin/main' into user/aliberts/2024_09…

d28f556

…_03_add_stretch

Cadene approved these changes Oct 4, 2024

View reviewed changes

Cadene added 2 commits October 4, 2024 18:16

fix unit tests

d210d2e

fix unit tests

d716511

Cadene merged commit 1a343c3 into main Oct 4, 2024
7 checks passed

Cadene deleted the user/aliberts/2024_09_03_add_stretch branch October 4, 2024 16:56

amandip7 pushed a commit to amandip7/lerobot that referenced this pull request Oct 10, 2024

Add support for Stretch (hello-robot) (huggingface#409)

8d9098f

Co-authored-by: Remi <remi.cadene@huggingface.co> Co-authored-by: Remi Cadene <re.cadene@gmail.com>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add support for Stretch (hello-robot) #409

Add support for Stretch (hello-robot) #409

aliberts commented Sep 4, 2024 •

edited by Cadene

Loading

Cadene left a comment

Cadene Sep 11, 2024

aliberts Sep 12, 2024

Cadene Sep 26, 2024

Cadene Sep 26, 2024

aliberts Sep 26, 2024 •

edited

Loading

Cadene Sep 26, 2024

hello-binit left a comment

hello-binit Sep 25, 2024

aliberts Sep 26, 2024 •

edited

Loading

hello-binit Sep 25, 2024

aliberts Sep 26, 2024

hello-binit Sep 25, 2024

hello-binit Sep 25, 2024

aliberts Sep 26, 2024

hello-binit Sep 26, 2024

hello-binit Sep 25, 2024

aliberts Sep 26, 2024

Cadene Sep 26, 2024

hello-binit Sep 25, 2024

hello-binit Sep 25, 2024

aliberts Sep 26, 2024

hello-binit Sep 26, 2024

Cadene left a comment

Cadene Sep 25, 2024

aliberts Sep 26, 2024

Cadene Sep 25, 2024

aliberts Sep 26, 2024

Cadene Sep 26, 2024

Cadene Sep 26, 2024

Add support for Stretch (hello-robot) #409

Add support for Stretch (hello-robot) #409

Conversation

aliberts commented Sep 4, 2024 • edited by Cadene Loading

What this does

How it was tested

How to checkout & try? (for the reviewer)

Cadene left a 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

aliberts Sep 26, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hello-binit left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aliberts Sep 26, 2024 • edited Loading

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

Cadene left a 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

aliberts commented Sep 4, 2024 •

edited by Cadene

Loading

aliberts Sep 26, 2024 •

edited

Loading

aliberts Sep 26, 2024 •

edited

Loading