refactor(robots): update lekiwi for the latest motor bus api

chore(teleop): Add missing abstract methods to keyboard implementation

refactor(robots): update lekiwi client and host code for the new api

chore(config): update host lekiwi ip in client config

chore(examples): move application scripts to the examples directory

fix(motors): missing type check condition in set_half_turn_homings

fix(robots): fix assumption in calibrate() for robots with more than just an arm

fix(robot): change Mode to Operating_Mode in configure write for lekiwi

fix(robots): make sure message is display in calibrate() method lekiwi

fix(robots): no need for .tolist() in lekiwi host app

fix(teleop): fix is_connected in teleoperator keyboard

fix(teleop): always display calibration message in so100

fix(robots): fix send_action in lekiwi_client

debug(examples): configuration for lekiwi client app

fix(robots): fix send_action in lekiwi client part 2

refactor(robots): use dicts in lekiwi for get_obs and send_action

dbg(robots): check sent action wheels lekiwi

debug(robots): fix overflow base commands

debug(robots): fix how we deal with negative values lekiwi

debug(robots): lekiwi sign degrees fix

fix(robots): right motors id in lekiwi host

chore(doc): update todos

chore(doc): added todos

Author: imstevenpmwork

lerobot/common/robots/lekiwi/lekiwi_client_app.py renamed to examples/robots/lekiwi_client_app.py

@@ -14,16 +14,11 @@
 
 import logging
 
-import numpy as np
-
-from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
-from lerobot.common.robots.config import RobotMode
+from lerobot.common.robots.lekiwi.config_lekiwi import LeKiwiClientConfig, RobotMode
+from lerobot.common.robots.lekiwi.lekiwi_client import LeKiwiClient
 from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
 from lerobot.common.teleoperators.so100 import SO100Leader, SO100LeaderConfig
 
-from .config_lekiwi import LeKiwiClientConfig
-from .lekiwi_client import LeKiwiClient
-
 DUMMY_FEATURES = {
     "observation.state": {
         "dtype": "float64",
@@ -82,26 +77,24 @@
 
 def main():
     logging.info("Configuring Teleop Devices")
-    leader_arm_config = SO100LeaderConfig(port="/dev/tty.usbmodem58760429271")
+    leader_arm_config = SO100LeaderConfig(port="/dev/tty.usbmodem58760434171")
     leader_arm = SO100Leader(leader_arm_config)
 
     keyboard_config = KeyboardTeleopConfig()
     keyboard = KeyboardTeleop(keyboard_config)
 
     logging.info("Configuring LeKiwi Client")
-    robot_config = LeKiwiClientConfig(
-        id="daemonlekiwi", calibration_dir=".cache/calibration/lekiwi", robot_mode=RobotMode.TELEOP
-    )
+    robot_config = LeKiwiClientConfig(id="lekiwi", robot_mode=RobotMode.TELEOP)
     robot = LeKiwiClient(robot_config)
 
     logging.info("Creating LeRobot Dataset")
 
-    # TODO(Steven): Check this creation
-    dataset = LeRobotDataset.create(
-        repo_id="user/lekiwi",
-        fps=10,
-        features=DUMMY_FEATURES,
-    )
+    # # TODO(Steven): Check this creation
+    # dataset = LeRobotDataset.create(
+    #     repo_id="user/lekiwi2",
+    #     fps=10,
+    #     features=DUMMY_FEATURES,
+    # )
 
     logging.info("Connecting Teleop Devices")
     leader_arm.connect()
@@ -110,30 +103,32 @@ def main():
     logging.info("Connecting remote LeKiwi")
     robot.connect()
 
+    if not robot.is_connected or not leader_arm.is_connected or not keyboard.is_connected:
+        logging.error("Failed to connect to all devices")
+        return
+
     logging.info("Starting LeKiwi teleoperation")
     i = 0
     while i < 1000:
         arm_action = leader_arm.get_action()
         base_action = keyboard.get_action()
-        action = np.append(arm_action, base_action) if base_action.size > 0 else arm_action
+        action = {**arm_action, **base_action} if len(base_action) > 0 else arm_action
 
         # TODO(Steven): Deal with policy action space
         # robot.set_mode(RobotMode.AUTO)
         # policy_action = policy.get_action() # This might be in body frame, key space or smt else
         # robot.send_action(policy_action)
-
         action_sent = robot.send_action(action)
         observation = robot.get_observation()
 
-        frame = {"action": action_sent}
-        frame.update(observation)
+        frame = {**action_sent, **observation}
         frame.update({"task": "Dummy Task Dataset"})
 
         logging.info("Saved a frame into the dataset")
-        dataset.add_frame(frame)
+        # dataset.add_frame(frame)
         i += 1
 
-    dataset.save_episode()
+    # dataset.save_episode()
     # dataset.push_to_hub()
 
     logging.info("Disconnecting Teleop Devices and LeKiwi Client")

lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py renamed to examples/teleoperators/teleop_keyboard_app.py

@@ -48,5 +48,5 @@
 HF_LEROBOT_HOME = Path(os.getenv("HF_LEROBOT_HOME", default_cache_path)).expanduser()
 
 # calibration dir
-default_calibration_path = HF_LEROBOT_HOME / ".calibration"
+default_calibration_path = HF_LEROBOT_HOME / "calibration"
 HF_LEROBOT_CALIBRATION = Path(os.getenv("HF_LEROBOT_CALIBRATION", default_calibration_path)).expanduser()

lerobot/common/constants.py

@@ -563,7 +563,7 @@ def set_half_turn_homings(self, motors: NameOrID | list[NameOrID] | None = None)
             motors = self.names
         elif isinstance(motors, (str, int)):
             motors = [motors]
-        else:
+        elif not isinstance(motors, list):
             raise TypeError(motors)
 
         self.reset_calibration(motors)
@@ -626,6 +626,7 @@ def _normalize(self, data_name: str, ids_values: dict[int, int]) -> dict[int, fl
             min_ = self.calibration[name].range_min
             max_ = self.calibration[name].range_max
             bounded_val = min(max_, max(min_, val))
+            # TODO(Steven): normalization can go boom if max_ == min_, we should add a check probably in record_ranges_of_motions (which probably indicates the user forgot to move a motor)
             if self.motors[name].norm_mode is MotorNormMode.RANGE_M100_100:
                 normalized_values[id_] = (((bounded_val - min_) / (max_ - min_)) * 200) - 100
             elif self.motors[name].norm_mode is MotorNormMode.RANGE_0_100:

lerobot/common/motors/motors_bus.py

@@ -1,23 +1,16 @@
 import abc
-import enum
 from dataclasses import dataclass
 from pathlib import Path
 
 import draccus
 
 
-class RobotMode(enum.Enum):
-    TELEOP = 0
-    AUTO = 1
-
-
 @dataclass(kw_only=True)
 class RobotConfig(draccus.ChoiceRegistry, abc.ABC):
     # Allows to distinguish between different robots of the same type
     id: str | None = None
     # Directory to store calibration file
     calibration_dir: Path | None = None
-    robot_mode: RobotMode | None = None
 
     @property
     def type(self) -> str:

lerobot/common/robots/config.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import enum
 from dataclasses import dataclass, field
 
 from lerobot.common.cameras.configs import CameraConfig
@@ -20,6 +21,12 @@
 from ..config import RobotConfig
 
 
+class RobotMode(enum.Enum):
+    TELEOP = 0
+    AUTO = 1
+
+
+# TODO(Steven): Consider sending config at initial step over a socket
 @RobotConfig.register_subclass("lekiwi")
 @dataclass
 class LeKiwiConfig(RobotConfig):
@@ -48,7 +55,7 @@ class LeKiwiConfig(RobotConfig):
 @dataclass
 class LeKiwiClientConfig(RobotConfig):
     # Network Configuration
-    remote_ip: str = "172.18.133.90"
+    remote_ip: str = "172.18.129.208"
     port_zmq_cmd: int = 5555
     port_zmq_observations: int = 5556
 
@@ -68,3 +75,5 @@ class LeKiwiClientConfig(RobotConfig):
             "quit": "q",
         }
     )
+
+    robot_mode: RobotMode | None = None

lerobot/common/robots/lekiwi/config_lekiwi.py

@@ -14,13 +14,10 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import base64
 import logging
 import time
 from typing import Any
 
-import cv2
-
 from lerobot.common.cameras.utils import make_cameras_from_configs
 from lerobot.common.constants import OBS_IMAGES, OBS_STATE
 from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
@@ -64,8 +61,8 @@ def __init__(self, config: LeKiwiConfig):
                 "arm_gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
                 # base
                 "base_left_wheel": Motor(7, "sts3215", MotorNormMode.RANGE_M100_100),
-                "base_back_wheel": Motor(8, "sts3215", MotorNormMode.RANGE_M100_100),
-                "base_right_wheel": Motor(9, "sts3215", MotorNormMode.RANGE_M100_100),
+                "base_right_wheel": Motor(8, "sts3215", MotorNormMode.RANGE_M100_100),
+                "base_back_wheel": Motor(9, "sts3215", MotorNormMode.RANGE_M100_100),
             },
             calibration=self.calibration,
         )
@@ -119,23 +116,33 @@ def connect(self) -> None:
     def is_calibrated(self) -> bool:
         return self.bus.is_calibrated
 
+    # TODO(Steven): I think we should extend this to give the user the option of re-calibrate
+    # calibrate(recalibrate: bool = False) -> None:
+    # If true, then we overwrite the previous calibration file with new values
     def calibrate(self) -> None:
         logger.info(f"\nRunning calibration of {self}")
+
+        motors = self.arm_motors + self.base_motors
+
         self.bus.disable_torque(self.arm_motors)
         for name in self.arm_motors:
             self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value)
 
         input("Move robot to the middle of its range of motion and press ENTER....")
-        homing_offsets = self.bus.set_half_turn_homings(self.arm_motors)
+        homing_offsets = self.bus.set_half_turn_homings(motors)
 
-        full_turn_motor = "arm_wrist_roll"
-        unknown_range_motors = [name for name in self.arm_motors if name != full_turn_motor]
-        logger.info(
+        # TODO(Steven): Might be worth to do this also in other robots but it should be added in the docs
+        full_turn_motor = [
+            motor for motor in motors if any(keyword in motor for keyword in ["wheel", "gripper"])
+        ]
+        unknown_range_motors = [motor for motor in motors if motor not in full_turn_motor]
+
+        print(
             f"Move all arm joints except '{full_turn_motor}' sequentially through their "
             "entire ranges of motion.\nRecording positions. Press ENTER to stop..."
         )
         range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors)
-        for name in [*self.base_motors, full_turn_motor]:
+        for name in full_turn_motor:
             range_mins[name] = 0
             range_maxes[name] = 4095
 
@@ -159,7 +166,7 @@ def configure(self):
         # and torque can be safely disabled to run calibration.
         self.bus.disable_torque(self.arm_motors)
         for name in self.arm_motors:
-            self.bus.write("Mode", name, OperatingMode.POSITION.value)
+            self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value)
             # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
             self.bus.write("P_Coefficient", name, 16)
             # Set I_Coefficient and D_Coefficient to default value 0 and 32
@@ -171,15 +178,15 @@ def configure(self):
             self.bus.write("Acceleration", name, 254)
 
         for name in self.base_motors:
-            self.bus.write("Mode", name, OperatingMode.VELOCITY.value)
+            self.bus.write("Operating_Mode", name, OperatingMode.VELOCITY.value)
 
-        self.bus.enable_torque()
+        self.bus.enable_torque()  # TODO(Steven): Operation has failed with: ConnectionError: Failed to write 'Lock' on id_=6 with '1' after 1 tries. [TxRxResult] Incorrect status packet!
 
     def get_observation(self) -> dict[str, Any]:
         if not self.is_connected:
             raise DeviceNotConnectedError(f"{self} is not connected.")
 
-        obs_dict = {}
+        obs_dict = {OBS_IMAGES: {}}
 
         # Read actuators position for arm and vel for base
         start = time.perf_counter()
@@ -192,12 +199,7 @@ def get_observation(self) -> dict[str, Any]:
         # Capture images from cameras
         for cam_key, cam in self.cameras.items():
             start = time.perf_counter()
-            frame = cam.async_read()
-            ret, buffer = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), 90])
-            if ret:
-                obs_dict[f"{OBS_IMAGES}.{cam_key}"] = base64.b64encode(buffer).decode("utf-8")
-            else:
-                obs_dict[f"{OBS_IMAGES}.{cam_key}"] = ""
+            obs_dict[OBS_IMAGES][cam_key] = cam.async_read()
             dt_ms = (time.perf_counter() - start) * 1e3
             logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms")
 
@@ -229,15 +231,19 @@ def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
             present_pos = self.bus.sync_read("Present_Position", self.arm_motors)
             goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in arm_goal_pos.items()}
             arm_safe_goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target)
+            arm_goal_pos = arm_safe_goal_pos
+
+        # TODO(Steven):  Message fetching failed: Magnitude 34072 exceeds 32767 (max for sign_bit_index=15)
+        # TODO(Steven): IMO, this should be a check in the motor bus
 
         # Send goal position to the actuators
-        self.bus.sync_write("Goal_Position", arm_safe_goal_pos)
+        self.bus.sync_write("Goal_Position", arm_goal_pos)
         self.bus.sync_write("Goal_Speed", base_goal_vel)
 
-        return {**arm_safe_goal_pos, **base_goal_vel}
+        return {**arm_goal_pos, **base_goal_vel}
 
     def stop_base(self):
-        self.bus.sync_write("Goal_Speed", {name: 0 for name in self.base_motors}, num_retry=5)
+        self.bus.sync_write("Goal_Speed", dict.fromkeys(self.base_motors, 0), num_retry=5)
         logger.info("Base motors stopped")
 
     def disconnect(self):