wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

refactor(robots): lekiwi v0.4

This commit is contained in:
Steven Palma 2025-03-14 14:30:55 +01:00
parent 81effca4ed
commit b1aed5a5d0
No known key found for this signature in database
8 changed files with 154 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
lerobot/common/errors.py
View File

@ -16,4 +16,11 @@ class DeviceAlreadyConnectedError(ConnectionError):
self.message = message self.message = message
super().__init__(self.message) super().__init__(self.message)
# TODO(Steven): Consider adding an InvalidActionError class InvalidActionError(ConnectionError):
"""Exception raised when an action is already invalid."""
def __init__(
self,
message="The action is invalid. Check the value follows what it is expected from the action space.",
):
self.message = message
super().__init__(self.message)

8
lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py
View File

@ -1,18 +1,10 @@
from dataclasses import dataclass, field from dataclasses import dataclass, field
from lerobot.common.cameras.configs import CameraConfig
from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig
from lerobot.common.robots.config import RobotConfig from lerobot.common.robots.config import RobotConfig
@RobotConfig.register_subclass("daemon_lekiwi") @RobotConfig.register_subclass("daemon_lekiwi")
@dataclass @dataclass
class DaemonLeKiwiRobotConfig(RobotConfig): class DaemonLeKiwiRobotConfig(RobotConfig):
# `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
# Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
# the number of motors in your follower arms.
max_relative_target: int | None = None
# Network Configuration # Network Configuration
remote_ip: str = "192.168.0.193" remote_ip: str = "192.168.0.193"

7
lerobot/common/robots/lekiwi/configuration_lekiwi.py
View File

@ -28,6 +28,8 @@ class LeKiwiRobotConfig(RobotConfig):
port_motor_bus = "/dev/ttyACM0" port_motor_bus = "/dev/ttyACM0"
# TODO(Steven): consider split this into arm and base # TODO(Steven): consider split this into arm and base
# TODO(Steven): Consider also removing this entirely as we can say that
# LeKiwiRobot will always have (and needs) such
shoulder_pan: tuple = (1, "sts3215") shoulder_pan: tuple = (1, "sts3215")
shoulder_lift: tuple = (2, "sts3215") shoulder_lift: tuple = (2, "sts3215")
elbow_flex: tuple=(3, "sts3215") elbow_flex: tuple=(3, "sts3215")
@ -37,3 +39,8 @@ class LeKiwiRobotConfig(RobotConfig):
left_wheel: tuple= (7, "sts3215") left_wheel: tuple= (7, "sts3215")
back_wheel: tuple = (8, "sts3215") back_wheel: tuple = (8, "sts3215")
right_wheel: tuple = (9, "sts3215") right_wheel: tuple = (9, "sts3215")
# `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
# Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
# the number of motors in your follower arms.
max_relative_target: int | None = None

118
lerobot/common/robots/lekiwi/daemon_lekiwi.py
View File

@ -16,20 +16,14 @@
import json import json
import logging import logging
import time
import threading
import numpy as np import numpy as np
import time
# import torch
import base64 import base64
import cv2 import cv2
import torch import torch
from lerobot.common.cameras.utils import make_cameras_from_configs
from lerobot.common.constants import OBS_IMAGES, OBS_STATE from lerobot.common.constants import OBS_IMAGES, OBS_STATE
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError
from ..robot import Robot from ..robot import Robot, RobotMode
from ..utils import ensure_safe_goal_position
from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
import zmq import zmq
@ -38,6 +32,12 @@ import zmq
# TODO(Steven): This doesn't need to take care of the # TODO(Steven): This doesn't need to take care of the
# mapping from teleop to motor commands, but given that # mapping from teleop to motor commands, but given that
# we already have a middle-man (this class) we add it here # we already have a middle-man (this class) we add it here
# Other options include:
# 1. Adding it to the Telop implementation for lekiwi
# (meaning each robot will need a teleop imple) or
# 2. Adding it into the robot implementation
# (meaning the policy might be needed to be train
# over the teleop action space)
class DaemonLeKiwiRobot(Robot): class DaemonLeKiwiRobot(Robot):
config_class = DaemonLeKiwiRobotConfig config_class = DaemonLeKiwiRobotConfig
@ -49,8 +49,6 @@ class DaemonLeKiwiRobot(Robot):
self.id = config.id self.id = config.id
self.robot_type = config.type self.robot_type = config.type
self.max_relative_target = config.max_relative_target
self.remote_ip = config.remote_ip self.remote_ip = config.remote_ip
self.port_zmq_cmd = config.port_zmq_cmd self.port_zmq_cmd = config.port_zmq_cmd
self.port_zmq_observations = config.port_zmq_observations self.port_zmq_observations = config.port_zmq_observations
@ -65,6 +63,7 @@ class DaemonLeKiwiRobot(Robot):
self.last_present_speed = {} self.last_present_speed = {}
self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32) self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
# Define three speed levels and a current index # Define three speed levels and a current index
self.speed_levels = [ self.speed_levels = [
{"xy": 0.1, "theta": 30}, # slow {"xy": 0.1, "theta": 30}, # slow
@ -73,23 +72,14 @@ class DaemonLeKiwiRobot(Robot):
] ]
self.speed_index = 0 # Start at slow self.speed_index = 0 # Start at slow
# Keyboard state for base teleoperation.
# self.running = True
# self.pressed_keys = {
# "forward": False,
# "backward": False,
# "left": False,
# "right": False,
# "rotate_left": False,
# "rotate_right": False,
# }
self.is_connected = False self.is_connected = False
self.logs = {} self.logs = {}
@property @property
def state_feature(self) -> dict: def state_feature(self) -> dict:
# TODO(Steven): Get this from the data fetched? # TODO(Steven): Get this from the data fetched?
# TODO(Steven): Motor names are unknown for the Daemon
# Or assume its size/metadata?
# return { # return {
# "dtype": "float32", # "dtype": "float32",
# "shape": (len(self.actuators),), # "shape": (len(self.actuators),),
@ -103,7 +93,9 @@ class DaemonLeKiwiRobot(Robot):
@property @property
def camera_features(self) -> dict[str, dict]: def camera_features(self) -> dict[str, dict]:
# TODO(Steven): Fetch this info or set it static? # TODO(Steven): Get this from the data fetched?
# TODO(Steven): Motor names are unknown for the Daemon
# Or assume its size/metadata?
# cam_ft = {} # cam_ft = {}
# for cam_key, cam in self.cameras.items(): # for cam_key, cam in self.cameras.items():
# cam_ft[cam_key] = { # cam_ft[cam_key] = {
@ -134,8 +126,11 @@ class DaemonLeKiwiRobot(Robot):
self.is_connected = True self.is_connected = True
def calibrate(self) -> None: def calibrate(self) -> None:
# TODO(Steven): Nothing to calibrate # TODO(Steven): Nothing to calibrate.
pass # Consider triggering calibrate() on the remote mobile robot?
# Althought this would require a more complex comms schema
logging.warning("DaemonLeKiwiRobot has nothing to calibrate.")
return
# Consider moving these static functions out of the class # Consider moving these static functions out of the class
# Copied from robot_lekiwi MobileManipulator class # Copied from robot_lekiwi MobileManipulator class
@ -267,12 +262,15 @@ class DaemonLeKiwiRobot(Robot):
return (x_cmd, y_cmd, theta_cmd) return (x_cmd, y_cmd, theta_cmd)
def get_data(self): def get_data(self):
# Copied from robot_lekiwi.py
"""Polls the video socket for up to 15 ms. If data arrives, decode only """Polls the video socket for up to 15 ms. If data arrives, decode only
the *latest* message, returning frames, speed, and arm state. If the *latest* message, returning frames, speed, and arm state. If
nothing arrives for any field, use the last known values.""" nothing arrives for any field, use the last known values."""
frames = {} frames = {}
present_speed = {} present_speed = {}
# TODO(Steven): Size is being assumed, is this safe?
remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32) remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32)
# Poll up to 15 ms # Poll up to 15 ms
@ -281,10 +279,13 @@ class DaemonLeKiwiRobot(Robot):
socks = dict(poller.poll(15)) socks = dict(poller.poll(15))
if self.zmq_observation_socket not in socks or socks[self.zmq_observation_socket] != zmq.POLLIN: if self.zmq_observation_socket not in socks or socks[self.zmq_observation_socket] != zmq.POLLIN:
# No new data arrived → reuse ALL old data # No new data arrived → reuse ALL old data
# TODO(Steven): This might return empty variables at init
return (self.last_frames, self.last_present_speed, self.last_remote_arm_state) return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
# Drain all messages, keep only the last # Drain all messages, keep only the last
last_msg = None last_msg = None
# TODO(Steven): There's probably a way to do this without while True
# TODO(Steven): Even consider changing to PUB/SUB
while True: while True:
try: try:
obs_string = self.zmq_observation_socket.recv_string(zmq.NOBLOCK) obs_string = self.zmq_observation_socket.recv_string(zmq.NOBLOCK)
@ -300,13 +301,11 @@ class DaemonLeKiwiRobot(Robot):
try: try:
observation = json.loads(last_msg) observation = json.loads(last_msg)
#TODO(Steven): Check this state_observation = {k: v for k, v in observation.items() if k.startswith(OBS_STATE)}
images_dict = observation.get("images", {}) image_observation = {k: v for k, v in observation.items() if k.startswith(OBS_IMAGES)}
new_speed = observation.get("present_speed", {})
new_arm_state = observation.get("follower_arm_state", None)
# Convert images # Convert images
for cam_name, image_b64 in images_dict.items(): for cam_name, image_b64 in image_observation.items():
if image_b64: if image_b64:
jpg_data = base64.b64decode(image_b64) jpg_data = base64.b64decode(image_b64)
np_arr = np.frombuffer(jpg_data, dtype=np.uint8) np_arr = np.frombuffer(jpg_data, dtype=np.uint8)
@ -315,19 +314,17 @@ class DaemonLeKiwiRobot(Robot):
frames[cam_name] = frame_candidate frames[cam_name] = frame_candidate
# If remote_arm_state is None and frames is None there is no message then use the previous message # If remote_arm_state is None and frames is None there is no message then use the previous message
if new_arm_state is not None and frames is not None: if state_observation is not None and frames is not None:
self.last_frames = frames self.last_frames = frames
remote_arm_state_tensor = torch.tensor(new_arm_state, dtype=torch.float32) remote_arm_state_tensor = torch.tensor(state_observation[:6], dtype=torch.float32)
self.last_remote_arm_state = remote_arm_state_tensor self.last_remote_arm_state = remote_arm_state_tensor
present_speed = new_speed present_speed = state_observation[6:]
self.last_present_speed = new_speed self.last_present_speed = present_speed
else: else:
frames = self.last_frames frames = self.last_frames
remote_arm_state_tensor = self.last_remote_arm_state remote_arm_state_tensor = self.last_remote_arm_state
present_speed = self.last_present_speed present_speed = self.last_present_speed
except Exception as e: except Exception as e:
@ -351,7 +348,6 @@ class DaemonLeKiwiRobot(Robot):
obs_dict = {} obs_dict = {}
# TODO(Steven): Check this
frames, present_speed, remote_arm_state_tensor = self.get_data() frames, present_speed, remote_arm_state_tensor = self.get_data()
body_state = self.wheel_raw_to_body(present_speed) body_state = self.wheel_raw_to_body(present_speed)
body_state_mm = (body_state[0] * 1000.0, body_state[1] * 1000.0, body_state[2]) # Convert x,y to mm/s body_state_mm = (body_state[0] * 1000.0, body_state[1] * 1000.0, body_state[2]) # Convert x,y to mm/s
@ -361,18 +357,18 @@ class DaemonLeKiwiRobot(Robot):
obs_dict = {OBS_STATE: combined_state_tensor} obs_dict = {OBS_STATE: combined_state_tensor}
# Loop over each configured camera # Loop over each configured camera
for cam_name, cam in self.cameras.items(): for cam_name, frame in frames.items():
frame = frames.get(cam_name, None)
if frame is None: if frame is None:
# Create a black image using the camera's configured width, height, and channels # TODO(Steven): Daemon doesn't know camera dimensions
frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) logging.warning("Frame is None")
obs_dict[f"{OBS_IMAGES}.{cam_name}"] = torch.from_numpy(frame) #frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8)
obs_dict[cam_name] = torch.from_numpy(frame)
return obs_dict return obs_dict
def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray):
# Speed control # Speed control
# TODO(Steven): Handle the right action
if self.teleop_keys["speed_up"] in pressed_keys: if self.teleop_keys["speed_up"] in pressed_keys:
self.speed_index = min(self.speed_index + 1, 2) self.speed_index = min(self.speed_index + 1, 2)
if self.teleop_keys["speed_down"] in pressed_keys: if self.teleop_keys["speed_down"] in pressed_keys:
@ -381,12 +377,10 @@ class DaemonLeKiwiRobot(Robot):
xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4 xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4
theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90 theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90
# (The rest of your code for generating wheel commands remains unchanged)
x_cmd = 0.0 # m/s forward/backward x_cmd = 0.0 # m/s forward/backward
y_cmd = 0.0 # m/s lateral y_cmd = 0.0 # m/s lateral
theta_cmd = 0.0 # deg/s rotation theta_cmd = 0.0 # deg/s rotation
# TODO(Steven): Handle action properly
if self.teleop_keys["forward"] in pressed_keys: if self.teleop_keys["forward"] in pressed_keys:
x_cmd += xy_speed x_cmd += xy_speed
if self.teleop_keys["backward"] in pressed_keys: if self.teleop_keys["backward"] in pressed_keys:
@ -402,13 +396,21 @@ class DaemonLeKiwiRobot(Robot):
return self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) return self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd)
# TODO(Steven): This assumes this call is always called from a keyboard teleop command
# TODO(Steven): Doing this mapping in here adds latecy between send_action and movement from the user perspective.
# t0: get teleop_cmd
# t1: send_action(teleop_cmd)
# t2: mapping teleop_cmd -> motor_cmd
# t3: execute motor_md
# This mapping for other robots/teleop devices might be slower. Doing this in the teleop will make this explicit
# t0': get teleop_cmd
# t1': mapping teleop_cmd -> motor_cmd
# t2': send_action(motor_cmd)
# t3': execute motor_cmd
# t3'-t2' << t3-t1
def send_action(self, action: np.ndarray) -> np.ndarray: def send_action(self, action: np.ndarray) -> np.ndarray:
"""Command lekiwi to move to a target joint configuration. """Command lekiwi to move to a target joint configuration.
The relative action magnitude may be clipped depending on the configuration parameter
`max_relative_target`. In this case, the action sent differs from original action.
Thus, this function always returns the action actually sent.
Args: Args:
action (np.ndarray): array containing the goal positions for the motors. action (np.ndarray): array containing the goal positions for the motors.
@ -422,24 +424,24 @@ class DaemonLeKiwiRobot(Robot):
raise DeviceNotConnectedError( raise DeviceNotConnectedError(
"ManipulatorRobot is not connected. You need to run `robot.connect()`." "ManipulatorRobot is not connected. You need to run `robot.connect()`."
) )
if self.mode is TELEOP:
# do conversion keys to motor
else:
# convert policy output
# TODO(Steven): This won't work if this is called by a policy with body vels outputs
goal_pos: np.array = np.empty(9) goal_pos: np.array = np.empty(9)
if action.size <6:
# TODO(Steven): Handle this properly if self.robot_mode is RobotMode.AUTO:
# TODO(Steven): Not yet implemented. The policy outputs might need a different conversion
raise Exception raise Exception
# TODO(Steven): This assumes teleop mode is always used with keyboard
if self.robot_mode is RobotMode.TELEOP:
if action.size <6:
logging.error("Action should include at least the 6 states of the leader arm")
raise InvalidActionError
# TODO(Steven): Assumes size and order is respected # TODO(Steven): Assumes size and order is respected
# TODO(Steven): This assumes this call is always called from a keyboard teleop command
wheel_actions = [v for _,v in self.from_keyboard_to_wheel_action(action[6:])] wheel_actions = [v for _,v in self.from_keyboard_to_wheel_action(action[6:])]
goal_pos[:6]=action[:6] goal_pos[:6]=action[:6]
goal_pos[6:]=wheel_actions goal_pos[6:]=wheel_actions
self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) #action is in motor space
self.zmq_cmd_socket.send_string(json.dumps(goal_pos))
return goal_pos return goal_pos

25
lerobot/common/robots/lekiwi/daemon_lekiwi_app.py
View File

@ -1,7 +1,7 @@
from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig
from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
from .daemon_lekiwi import DaemonLeKiwiRobot from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode
import time import time
import logging import logging
@ -24,31 +24,28 @@ def main():
logging.info("Connecting remote LeKiwiRobot") logging.info("Connecting remote LeKiwiRobot")
robot.connect() # Establishes ZMQ sockets with the remote mobile robot robot.connect() # Establishes ZMQ sockets with the remote mobile robot
robot.robot_mode = RobotMode.TELEOP
logging.info("Starting LeKiwiRobot teleoperation") logging.info("Starting LeKiwiRobot teleoperation")
start = time.perf_counter() start = time.perf_counter()
duration = 0 duration = 0
while duration < 20: while duration < 20:
arm_action = leader_arm.get_action() # 6 motors arm_action = leader_arm.get_action()
base_action = keyboard.get_action() # n keys pressed base_action = keyboard.get_action()
action = { action = {
**arm_action, **arm_action,
**base_action **base_action
} }
robot.set_mode(TELEOP) _action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ
action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ _observation = robot.get_observation() # Receives over ZMQ, translate to body-frame vel
obs = robot.get_observation() # Receives over ZMQ, translate to body-frame vel
dataset.save(action_sent, obs) # dataset.save(action_sent, obs)
# TODO(Steven) # TODO(Steven): Deal with policy action space
robot.set_mode(AUTO) # robot.set_mode(RobotMode.AUTO)
policy_action = policy.get_action() # This might be in body frame or in key space # policy_action = policy.get_action() # This might be in body frame, key space or smt else
robot.send_action(policy_action) # This has no way to know # robot.send_action(policy_action)
teleop_step() # teleop
send_action() #policy
duration = time.perf_counter() - start duration = time.perf_counter() - start

100
lerobot/common/robots/lekiwi/lekiwi_robot.py
View File

@ -20,7 +20,6 @@ import time
import threading import threading
import numpy as np import numpy as np
import time import time
# import torch
import base64 import base64
import cv2 import cv2
@ -59,22 +58,27 @@ class LeKiwiRobot(Robot):
# TODO(Steven): Consider in the future using S100 robot class # TODO(Steven): Consider in the future using S100 robot class
# TODO(Steven): Another option is to use the motorbus factory, but in this case we assume that # TODO(Steven): Another option is to use the motorbus factory, but in this case we assume that
# what we consider 'lekiwi robot' always uses the FeetechMotorsBus # what we consider 'lekiwi robot' always uses the FeetechMotorsBus
# TODO(Steven): We will need to have a key for arm and base for calibration # TODO(Steven): Order and dimension are generaly assumed to be 6 first for arm, 3 last for base
self.actuators = FeetechMotorsBus( self.actuators_bus = FeetechMotorsBus(
port=self.config.port_motor_bus, port=self.config.port_motor_bus,
motors={ motors={
"shoulder_pan": config.shoulder_pan, "arm_shoulder_pan": config.shoulder_pan,
"shoulder_lift": config.shoulder_lift, "arm_shoulder_lift": config.shoulder_lift,
"elbow_flex": config.elbow_flex, "arm_elbow_flex": config.elbow_flex,
"wrist_flex": config.wrist_flex, "arm_wrist_flex": config.wrist_flex,
"wrist_roll": config.wrist_roll, "arm_wrist_roll": config.wrist_roll,
"gripper": config.gripper, "arm_gripper": config.gripper,
"left_wheel": config.left_wheel, "base_left_wheel": config.left_wheel,
"right_wheel": config.right_wheel, "base_right_wheel": config.right_wheel,
"back_wheel": config.back_wheel, "base_back_wheel": config.back_wheel,
}, },
) )
self.arm_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("arm")]
self.base_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("base")]
self.max_relative_target = config.max_relative_target
#TODO(Steven): Consider removing cameras from configs #TODO(Steven): Consider removing cameras from configs
self.cameras = make_cameras_from_configs(config.cameras) self.cameras = make_cameras_from_configs(config.cameras)
@ -94,8 +98,8 @@ class LeKiwiRobot(Robot):
def state_feature(self) -> dict: def state_feature(self) -> dict:
return { return {
"dtype": "float32", "dtype": "float32",
"shape": (len(self.actuators),), "shape": (len(self.actuators_bus),),
"names": {"motors": list(self.actuators.motors)}, "names": {"motors": list(self.actuators_bus.motors)},
} }
@property @property
@ -126,32 +130,39 @@ class LeKiwiRobot(Robot):
return context, cmd_socket, observation_socket return context, cmd_socket, observation_socket
def setup_actuators(self): def setup_actuators(self):
# Set-up arm actuators (position mode)
# We assume that at connection time, arm is in a rest position, # We assume that at connection time, arm is in a rest position,
# and torque can be safely disabled to run calibration. # and torque can be safely disabled to run calibration.
self.actuators.write("Torque_Enable", TorqueMode.DISABLED.value) self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value,self.arm_actuators)
self.calibrate() self.calibrate() # TODO(Steven): This should be only for the arm
# Mode=0 for Position Control # Mode=0 for Position Control
# TODO(Steven): Base robots should actually be in vel mode self.actuators_bus.write("Mode", 0,self.arm_actuators)
self.actuators.write("Mode", 0)
# Set P_Coefficient to lower value to avoid shakiness (Default is 32) # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
self.actuators.write("P_Coefficient", 16) self.actuators_bus.write("P_Coefficient", 16,self.arm_actuators)
# Set I_Coefficient and D_Coefficient to default value 0 and 32 # Set I_Coefficient and D_Coefficient to default value 0 and 32
self.actuators.write("I_Coefficient", 0) self.actuators_bus.write("I_Coefficient", 0, self.arm_actuators)
self.actuators.write("D_Coefficient", 32) self.actuators_bus.write("D_Coefficient", 32,self.arm_actuators)
# Close the write lock so that Maximum_Acceleration gets written to EPROM address, # Close the write lock so that Maximum_Acceleration gets written to EPROM address,
# which is mandatory for Maximum_Acceleration to take effect after rebooting. # which is mandatory for Maximum_Acceleration to take effect after rebooting.
self.actuators.write("Lock", 0) self.actuators_bus.write("Lock", 0,self.arm_actuators)
# Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
# the motors. Note: this configuration is not in the official STS3215 Memory Table # the motors. Note: this configuration is not in the official STS3215 Memory Table
self.actuators.write("Maximum_Acceleration", 254) self.actuators_bus.write("Maximum_Acceleration", 254,self.arm_actuators)
self.actuators.write("Acceleration", 254) self.actuators_bus.write("Acceleration", 254, self.arm_actuators)
logging.info("Activating torque.") logging.info("Activating torque.")
self.actuators.write("Torque_Enable", TorqueMode.ENABLED.value) self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value,self.arm_actuators)
# Check arm can be read # Check arm can be read
self.actuators.read("Present_Position") self.actuators_bus.read("Present_Position",self.arm_actuators)
# Set-up base actuators (velocity mode)
self.actuators_bus.write("Lock",0,self.base_actuators)
self.actuators_bus.write("Mode",[1,1,1],self.base_actuators)
self.actuators_bus.write("Lock",1,self.base_actuators)
def connect(self) -> None: def connect(self) -> None:
if self.is_connected: if self.is_connected:
@ -160,7 +171,7 @@ class LeKiwiRobot(Robot):
) )
logging.info("Connecting actuators.") logging.info("Connecting actuators.")
self.actuators.connect() self.actuators_bus.connect()
self.setup_actuators() self.setup_actuators()
logging.info("Connecting cameras.") logging.info("Connecting cameras.")
@ -168,14 +179,10 @@ class LeKiwiRobot(Robot):
cam.connect() cam.connect()
logging.info("Connecting ZMQ sockets.") logging.info("Connecting ZMQ sockets.")
self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets(self.config) self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets()
self.is_connected = True self.is_connected = True
# TODO(Steven): Consider using this
# def get_motor_names(self, arms: dict[str, MotorsBus]) -> list:
# return [f"{arm}_{motor}" for arm, bus in arms.items() for motor in bus.motors]
def calibrate(self) -> None: def calibrate(self) -> None:
# Copied from S100 robot # Copied from S100 robot
"""After calibration all motors function in human interpretable ranges. """After calibration all motors function in human interpretable ranges.
@ -189,14 +196,14 @@ class LeKiwiRobot(Robot):
calibration = json.load(f) calibration = json.load(f)
else: else:
logging.info(f"Missing calibration file '{actuators_calib_path}'") logging.info(f"Missing calibration file '{actuators_calib_path}'")
calibration = run_arm_manual_calibration(self.actuators, self.robot_type, self.name, "follower") calibration = run_arm_manual_calibration(self.actuators_bus, self.robot_type, self.name, "follower")
logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'")
actuators_calib_path.parent.mkdir(parents=True, exist_ok=True) actuators_calib_path.parent.mkdir(parents=True, exist_ok=True)
with open(actuators_calib_path, "w") as f: with open(actuators_calib_path, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.actuators.set_calibration(calibration) self.actuators_bus.set_calibration(calibration)
def get_observation(self) -> dict[str, np.ndarray]: def get_observation(self) -> dict[str, np.ndarray]:
"""The returned observations do not have a batch dimension.""" """The returned observations do not have a batch dimension."""
@ -207,10 +214,9 @@ class LeKiwiRobot(Robot):
obs_dict = {} obs_dict = {}
# Read actuators position # Read actuators position for arm and vel for base
# TODO(Steven): Base motors should return a vel instead of a pos
before_read_t = time.perf_counter() before_read_t = time.perf_counter()
obs_dict[OBS_STATE] = self.actuators.read("Present_Position") obs_dict[OBS_STATE] = self.actuators_bus.read("Present_Position",self.arm_actuators) + self.actuators_bus.read("Present_Speed", self.base_actuators)
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
# Capture images from cameras # Capture images from cameras
@ -249,17 +255,18 @@ class LeKiwiRobot(Robot):
"LeKiwiRobot is not connected. You need to run `robot.connect()`." "LeKiwiRobot is not connected. You need to run `robot.connect()`."
) )
# Input action is in motor space
goal_pos=action goal_pos=action
# Cap goal position when too far away from present position. # Cap goal position when too far away from present position.
# /!\ Slower fps expected due to reading from the follower. # /!\ Slower fps expected due to reading from the follower.
if self.config.max_relative_target is not None: if self.config.max_relative_target is not None:
present_pos = self.actuators.read("Present_Position") present_pos = self.actuators_bus.read("Present_Position",self.arm_actuators)
goal_pos = ensure_safe_goal_position(goal_pos, present_pos, self.config.max_relative_target) goal_pos[:6] = ensure_safe_goal_position(goal_pos[:6], present_pos, self.config.max_relative_target)
# Send goal position to the actuators # Send goal position to the actuators
# TODO(Steven): Base motors should set a vel instead # TODO(Steven): This happens synchronously
self.actuators.write("Goal_Position", goal_pos.astype(np.int32)) self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32),self.arm_actuators)
self.actuators_bus.write("Goal_Speed",goal_pos[6:].astype(np.int32),self.base_actuators)
return goal_pos return goal_pos
@ -271,8 +278,10 @@ class LeKiwiRobot(Robot):
# TODO(Steven): Consider adding a delay to not starve the CPU # TODO(Steven): Consider adding a delay to not starve the CPU
def stop(self): def stop(self):
# TODO(Steven): Base motors speed should be set to 0 # TODO(Steven): Assumes there's only 3 motors for base
pass logging.info("Stopping base")
self.actuators_bus.write("Goal_Speed",[0,0,0],self.base_actuators)
logging.info("Base motors stopped")
def run(self): def run(self):
# Copied logic from run_lekiwi in lekiwi_remote.py # Copied logic from run_lekiwi in lekiwi_remote.py
@ -297,7 +306,6 @@ class LeKiwiRobot(Robot):
try: try:
msg = self.cmd_socket.recv_string(zmq.NOBLOCK) msg = self.cmd_socket.recv_string(zmq.NOBLOCK)
data = json.loads(msg) data = json.loads(msg)
# TODO(Steven): Process data correctly
self.send_action(data) self.send_action(data)
last_cmd_time = time.time() last_cmd_time = time.time()
# except zmq.Again: # except zmq.Again:
@ -338,7 +346,7 @@ class LeKiwiRobot(Robot):
) )
self.stop() self.stop()
self.actuators.disconnect() self.actuators_bus.disconnect()
for cam in self.cameras.values(): for cam in self.cameras.values():
cam.disconnect() cam.disconnect()
self.observation_socket.close() self.observation_socket.close()

7
lerobot/common/robots/robot.py
View File

@ -9,6 +9,12 @@ from lerobot.common.motors import MotorCalibration
from .config import RobotConfig from .config import RobotConfig
import enum
class RobotMode(enum.Enum):
TELEOP = 0
AUTO = 1
# TODO(aliberts): action/obs typing such as Generic[ObsType, ActType] similar to gym.Env ? # TODO(aliberts): action/obs typing such as Generic[ObsType, ActType] similar to gym.Env ?
# https://github.com/Farama-Foundation/Gymnasium/blob/3287c869f9a48d99454306b0d4b4ec537f0f35e3/gymnasium/core.py#L23 # https://github.com/Farama-Foundation/Gymnasium/blob/3287c869f9a48d99454306b0d4b4ec537f0f35e3/gymnasium/core.py#L23
@ -21,6 +27,7 @@ class Robot(abc.ABC):
def __init__(self, config: RobotConfig): def __init__(self, config: RobotConfig):
self.robot_type = self.name self.robot_type = self.name
self.robot_mode: RobotMode | None = None
self.id = config.id self.id = config.id
self.calibration_dir = ( self.calibration_dir = (
config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name

2
lerobot/common/teleoperators/keyboard/configuration_keyboard.py
View File

@ -22,5 +22,5 @@ from ..config import TeleoperatorConfig
@TeleoperatorConfig.register_subclass("keyboard") @TeleoperatorConfig.register_subclass("keyboard")
@dataclass @dataclass
class KeyboardTeleopConfig(TeleoperatorConfig): class KeyboardTeleopConfig(TeleoperatorConfig):
# TODO(Steven): Maybe set in here the keys that we want to capture # TODO(Steven): Maybe set in here the keys that we want to capture/listen
mock: bool = False mock: bool = False