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.5

This commit is contained in:
Steven Palma 2025-03-14 14:38:06 +01:00
parent b1aed5a5d0
commit cb27f14f7b
No known key found for this signature in database
11 changed files with 121 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
lerobot/common/errors.py
View File

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

0
lerobot/common/robots/lekiwi/old_lekiwi_remote.py → lerobot/common/robots/lekiwi/_old_lekiwi_remote.py
View File

0
lerobot/common/robots/lekiwi/old_robot_lekiwi.py → lerobot/common/robots/lekiwi/_old_robot_lekiwi.py
View File

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

@ -2,10 +2,10 @@ from dataclasses import dataclass, field
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):
# Network Configuration # Network Configuration
remote_ip: str = "192.168.0.193" remote_ip: str = "192.168.0.193"
port_zmq_cmd: int = 5555 port_zmq_cmd: int = 5555
@ -26,4 +26,4 @@ class DaemonLeKiwiRobotConfig(RobotConfig):
# quit teleop # quit teleop
"quit": "q", "quit": "q",
} }
) )

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

@ -8,7 +8,6 @@ from lerobot.common.robots.config import RobotConfig
@RobotConfig.register_subclass("lekiwi") @RobotConfig.register_subclass("lekiwi")
@dataclass @dataclass
class LeKiwiRobotConfig(RobotConfig): class LeKiwiRobotConfig(RobotConfig):
port_zmq_cmd: int = 5555 port_zmq_cmd: int = 5555
port_zmq_observations: int = 5556 port_zmq_observations: int = 5556
@ -24,7 +23,7 @@ class LeKiwiRobotConfig(RobotConfig):
) )
calibration_dir: str = ".cache/calibration/lekiwi" calibration_dir: str = ".cache/calibration/lekiwi"
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
@ -32,12 +31,12 @@ class LeKiwiRobotConfig(RobotConfig):
# LeKiwiRobot will always have (and needs) such # 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")
wrist_flex: tuple=(4, "sts3215") wrist_flex: tuple = (4, "sts3215")
wrist_roll: tuple=(5, "sts3215") wrist_roll: tuple = (5, "sts3215")
gripper: tuple =(6, "sts3215") gripper: tuple = (6, "sts3215")
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. # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.

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

@ -14,32 +14,34 @@
# 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 base64
import json import json
import logging import logging
import numpy as np
import base64
import cv2 import cv2
import numpy as np
import torch import torch
import zmq
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, InvalidActionError from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError
from ..robot import Robot, RobotMode from ..robot import Robot, RobotMode
from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
import zmq
# TODO(Steven): This doesn't need to inherit from Robot # TODO(Steven): This doesn't need to inherit from Robot
# But we do it for now to offer a familiar API # But we do it for now to offer a familiar API
# 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: # Other options include:
# 1. Adding it to the Telop implementation for lekiwi # 1. Adding it to the Telop implementation for lekiwi
# (meaning each robot will need a teleop imple) or # (meaning each robot will need a teleop imple) or
# 2. Adding it into the robot implementation # 2. Adding it into the robot implementation
# (meaning the policy might be needed to be train # (meaning the policy might be needed to be train
# over the teleop action space) # over the teleop action space)
class DaemonLeKiwiRobot(Robot): class DaemonLeKiwiRobot(Robot):
config_class = DaemonLeKiwiRobotConfig config_class = DaemonLeKiwiRobotConfig
name = "daemonlekiwi" name = "daemonlekiwi"
@ -63,7 +65,6 @@ 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
@ -105,7 +106,7 @@ class DaemonLeKiwiRobot(Robot):
# } # }
# return cam_ft # return cam_ft
pass pass
def connect(self) -> None: def connect(self) -> None:
if self.is_connected: if self.is_connected:
raise DeviceAlreadyConnectedError( raise DeviceAlreadyConnectedError(
@ -121,17 +122,17 @@ class DaemonLeKiwiRobot(Robot):
self.zmq_observation_socket = self.zmq_context.socket(zmq.PULL) self.zmq_observation_socket = self.zmq_context.socket(zmq.PULL)
zmq_observations_locator = f"tcp://{self.remote_ip}:{self.port_zmq_observations}" zmq_observations_locator = f"tcp://{self.remote_ip}:{self.port_zmq_observations}"
self.zmq_observation_socket.connect(zmq_observations_locator) self.zmq_observation_socket.connect(zmq_observations_locator)
self.zmq_observation_socket.setsockopt(zmq.CONFLATE,1) self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1)
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.
# Consider triggering calibrate() on the remote mobile robot? # Consider triggering calibrate() on the remote mobile robot?
# Althought this would require a more complex comms schema # Although this would require a more complex comms schema
logging.warning("DaemonLeKiwiRobot has nothing to calibrate.") logging.warning("DaemonLeKiwiRobot has nothing to calibrate.")
return 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
@staticmethod @staticmethod
@ -145,7 +146,7 @@ class DaemonLeKiwiRobot(Robot):
return speed_int | 0x8000 return speed_int | 0x8000
else: else:
return speed_int & 0x7FFF return speed_int & 0x7FFF
# Copied from robot_lekiwi MobileManipulator class # Copied from robot_lekiwi MobileManipulator class
@staticmethod @staticmethod
def raw_to_degps(raw_speed: int) -> float: def raw_to_degps(raw_speed: int) -> float:
@ -155,7 +156,7 @@ class DaemonLeKiwiRobot(Robot):
if raw_speed & 0x8000: if raw_speed & 0x8000:
degps = -degps degps = -degps
return degps return degps
# Copied from robot_lekiwi MobileManipulator class # Copied from robot_lekiwi MobileManipulator class
def body_to_wheel_raw( def body_to_wheel_raw(
self, self,
@ -217,7 +218,7 @@ class DaemonLeKiwiRobot(Robot):
wheel_raw = [DaemonLeKiwiRobot.degps_to_raw(deg) for deg in wheel_degps] wheel_raw = [DaemonLeKiwiRobot.degps_to_raw(deg) for deg in wheel_degps]
return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]} return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]}
# Copied from robot_lekiwi MobileManipulator class # Copied from robot_lekiwi MobileManipulator class
def wheel_raw_to_body( def wheel_raw_to_body(
self, wheel_raw: dict, wheel_radius: float = 0.05, base_radius: float = 0.125 self, wheel_raw: dict, wheel_radius: float = 0.05, base_radius: float = 0.125
@ -260,7 +261,7 @@ class DaemonLeKiwiRobot(Robot):
x_cmd, y_cmd, theta_rad = velocity_vector x_cmd, y_cmd, theta_rad = velocity_vector
theta_cmd = theta_rad * (180.0 / np.pi) theta_cmd = theta_rad * (180.0 / np.pi)
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 # 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
@ -332,7 +333,7 @@ class DaemonLeKiwiRobot(Robot):
# If decode fails, fall back to old data # If decode fails, fall back to old data
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)
return frames, present_speed, remote_arm_state_tensor return frames, present_speed, remote_arm_state_tensor
# TODO(Steven): The returned space is different from the get_observation of LeKiwiRobot # TODO(Steven): The returned space is different from the get_observation of LeKiwiRobot
# This returns body-frames velocities instead of wheel pos/speeds # This returns body-frames velocities instead of wheel pos/speeds
def get_observation(self) -> dict[str, np.ndarray]: def get_observation(self) -> dict[str, np.ndarray]:
@ -353,7 +354,7 @@ class DaemonLeKiwiRobot(Robot):
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
wheel_state_tensor = torch.tensor(body_state_mm, dtype=torch.float32) wheel_state_tensor = torch.tensor(body_state_mm, dtype=torch.float32)
combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0) combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0)
obs_dict = {OBS_STATE: combined_state_tensor} obs_dict = {OBS_STATE: combined_state_tensor}
# Loop over each configured camera # Loop over each configured camera
@ -361,13 +362,12 @@ class DaemonLeKiwiRobot(Robot):
if frame is None: if frame is None:
# TODO(Steven): Daemon doesn't know camera dimensions # TODO(Steven): Daemon doesn't know camera dimensions
logging.warning("Frame is None") logging.warning("Frame is None")
#frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) # frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8)
obs_dict[cam_name] = torch.from_numpy(frame) 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
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)
@ -376,7 +376,7 @@ class DaemonLeKiwiRobot(Robot):
speed_setting = self.speed_levels[self.speed_index] speed_setting = self.speed_levels[self.speed_index]
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
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
@ -424,27 +424,27 @@ 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()`."
) )
goal_pos: np.array = np.empty(9) goal_pos: np.array = np.empty(9)
if self.robot_mode is RobotMode.AUTO: if self.robot_mode is RobotMode.AUTO:
# TODO(Steven): Not yet implemented. The policy outputs might need a different conversion # 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 # TODO(Steven): This assumes teleop mode is always used with keyboard
if self.robot_mode is RobotMode.TELEOP: if self.robot_mode is RobotMode.TELEOP:
if action.size <6: if action.size < 6:
logging.error("Action should include at least the 6 states of the leader arm") logging.error("Action should include at least the 6 states of the leader arm")
raise InvalidActionError raise InvalidActionError
# TODO(Steven): Assumes size and order is respected # TODO(Steven): Assumes size and order is respected
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)) # action is in motor space
return goal_pos return goal_pos
def print_logs(self): def print_logs(self):
# TODO(Steven): Refactor logger # TODO(Steven): Refactor logger
pass pass
@ -459,7 +459,7 @@ class DaemonLeKiwiRobot(Robot):
self.zmq_cmd_socket.close() self.zmq_cmd_socket.close()
self.zmq_context.term() self.zmq_context.term()
self.is_connected = False self.is_connected = False
def __del__(self): def __del__(self):
if getattr(self, "is_connected", False): if getattr(self, "is_connected", False):
self.disconnect() self.disconnect()

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

@ -1,12 +1,15 @@
from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig import logging
import time
import numpy as np
from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig
from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode
import time
import logging
def main(): def main():
logging.info("Configuring Teleop Devices") logging.info("Configuring Teleop Devices")
leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511") leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511")
leader_arm = SO100Teleop(leader_arm_config) leader_arm = SO100Teleop(leader_arm_config)
@ -21,24 +24,20 @@ def main():
logging.info("Configuring LeKiwiRobot Daemon") logging.info("Configuring LeKiwiRobot Daemon")
robot_config = DaemonLeKiwiRobotConfig() robot_config = DaemonLeKiwiRobotConfig()
robot = DaemonLeKiwiRobot(robot_config) robot = DaemonLeKiwiRobot(robot_config)
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 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() arm_action = leader_arm.get_action()
base_action = keyboard.get_action() base_action = keyboard.get_action()
action = { action = np.concatenate((arm_action, base_action))
**arm_action, _action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ
**base_action _observation = robot.get_observation() # Receives over ZMQ, translate to body-frame vel
}
_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
# dataset.save(action_sent, obs) # dataset.save(action_sent, obs)
@ -50,11 +49,12 @@ def main():
duration = time.perf_counter() - start duration = time.perf_counter() - start
logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon")
robot.disconnect() # Cleans ZMQ comms robot.disconnect() # Cleans ZMQ comms
leader_arm.disconnect() leader_arm.disconnect()
keyboard.disconnect() keyboard.disconnect()
logging.info("Finished LeKiwiRobot cleanly") logging.info("Finished LeKiwiRobot cleanly")
if __name__ == "__main__": if __name__ == "__main__":
main() main()

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

@ -1,9 +1,10 @@
import logging import logging
from .configuration_lekiwi import LeKiwiRobotConfig from .configuration_lekiwi import LeKiwiRobotConfig
from .lekiwi_robot import LeKiwiRobot from .lekiwi_robot import LeKiwiRobot
def main():
def main():
logging.info("Configuring LeKiwiRobot") logging.info("Configuring LeKiwiRobot")
robot_config = LeKiwiRobotConfig() robot_config = LeKiwiRobotConfig()
robot = LeKiwiRobot(robot_config) robot = LeKiwiRobot(robot_config)
@ -14,9 +15,9 @@ def main():
# Remotely teleoperated # Remotely teleoperated
logging.info("Starting LeKiwiRobot teleoperation") logging.info("Starting LeKiwiRobot teleoperation")
robot.run() robot.run()
logging.info("Finished LeKiwiRobot cleanly") logging.info("Finished LeKiwiRobot cleanly")
if __name__ == "__main__": if __name__ == "__main__":
main() main()

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

@ -14,27 +14,29 @@
# 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 base64
import json import json
import logging import logging
import time
import threading import threading
import numpy as np
import time import time
import base64
import cv2 import cv2
import numpy as np
import zmq
from lerobot.common.cameras.utils import make_cameras_from_configs 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
from ..robot import Robot
from ..utils import ensure_safe_goal_position
from .configuration_lekiwi import LeKiwiRobotConfig
from lerobot.common.motors.feetech import ( from lerobot.common.motors.feetech import (
FeetechMotorsBus, FeetechMotorsBus,
TorqueMode, TorqueMode,
run_arm_manual_calibration, run_full_arm_calibration,
) )
import zmq
from ..robot import Robot
from ..utils import ensure_safe_goal_position
from .configuration_lekiwi import LeKiwiRobotConfig
class LeKiwiRobot(Robot): class LeKiwiRobot(Robot):
""" """
@ -58,7 +60,7 @@ 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): Order and dimension are generaly assumed to be 6 first for arm, 3 last for base # TODO(Steven): Order and dimension are generally assumed to be 6 first for arm, 3 last for base
self.actuators_bus = FeetechMotorsBus( self.actuators_bus = FeetechMotorsBus(
port=self.config.port_motor_bus, port=self.config.port_motor_bus,
motors={ motors={
@ -79,7 +81,7 @@ class LeKiwiRobot(Robot):
self.max_relative_target = config.max_relative_target 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)
self.observation_lock = threading.Lock() self.observation_lock = threading.Lock()
@ -92,8 +94,6 @@ class LeKiwiRobot(Robot):
self.is_connected = False self.is_connected = False
self.logs = {} self.logs = {}
@property @property
def state_feature(self) -> dict: def state_feature(self) -> dict:
return { return {
@ -128,41 +128,39 @@ class LeKiwiRobot(Robot):
observation_socket.bind(f"tcp://*:{self.port_zmq_observations}") observation_socket.bind(f"tcp://*:{self.port_zmq_observations}")
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) # 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_bus.write("Torque_Enable", TorqueMode.DISABLED.value,self.arm_actuators) self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators)
self.calibrate() # TODO(Steven): This should be only for the arm self.calibrate() # TODO(Steven): This should be only for the arm
# Mode=0 for Position Control # Mode=0 for Position Control
self.actuators_bus.write("Mode", 0,self.arm_actuators) self.actuators_bus.write("Mode", 0, self.arm_actuators)
# 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_bus.write("P_Coefficient", 16,self.arm_actuators) 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_bus.write("I_Coefficient", 0, self.arm_actuators) self.actuators_bus.write("I_Coefficient", 0, self.arm_actuators)
self.actuators_bus.write("D_Coefficient", 32,self.arm_actuators) 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_bus.write("Lock", 0,self.arm_actuators) 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_bus.write("Maximum_Acceleration", 254,self.arm_actuators) self.actuators_bus.write("Maximum_Acceleration", 254, self.arm_actuators)
self.actuators_bus.write("Acceleration", 254, self.arm_actuators) self.actuators_bus.write("Acceleration", 254, self.arm_actuators)
logging.info("Activating torque.") logging.info("Activating torque.")
self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value,self.arm_actuators) self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value, self.arm_actuators)
# Check arm can be read # Check arm can be read
self.actuators_bus.read("Present_Position",self.arm_actuators) self.actuators_bus.read("Present_Position", self.arm_actuators)
# Set-up base actuators (velocity mode) # Set-up base actuators (velocity mode)
self.actuators_bus.write("Lock",0,self.base_actuators) self.actuators_bus.write("Lock", 0, self.base_actuators)
self.actuators_bus.write("Mode",[1,1,1],self.base_actuators) self.actuators_bus.write("Mode", [1, 1, 1], self.base_actuators)
self.actuators_bus.write("Lock",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:
@ -192,15 +190,17 @@ class LeKiwiRobot(Robot):
actuators_calib_path = self.calibration_dir / f"{self.config.id}.json" actuators_calib_path = self.calibration_dir / f"{self.config.id}.json"
if actuators_calib_path.exists(): if actuators_calib_path.exists():
with open(actuators_calib_path) as f: with open(actuators_calib_path,encoding="utf-8") as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
logging.info(f"Missing calibration file '{actuators_calib_path}'") logging.info("Missing calibration file '%s'",actuators_calib_path)
calibration = run_arm_manual_calibration(self.actuators_bus, self.robot_type, self.name, "follower") calibration = run_full_arm_calibration(
self.actuators_bus, self.robot_type, self.name, "follower"
)
logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'") logging.info("Calibration is done! Saving calibration file '%s'",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",encoding="utf-8") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.actuators_bus.set_calibration(calibration) self.actuators_bus.set_calibration(calibration)
@ -208,15 +208,15 @@ class LeKiwiRobot(Robot):
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."""
if not self.is_connected: if not self.is_connected:
raise DeviceNotConnectedError( raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.")
"LeKiwiRobot is not connected. You need to run `robot.connect()`."
)
obs_dict = {} obs_dict = {}
# Read actuators position for arm and vel for base # Read actuators position for arm and vel for base
before_read_t = time.perf_counter() before_read_t = time.perf_counter()
obs_dict[OBS_STATE] = self.actuators_bus.read("Present_Position",self.arm_actuators) + self.actuators_bus.read("Present_Speed", self.base_actuators) 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
@ -251,22 +251,22 @@ class LeKiwiRobot(Robot):
np.ndarray: the action sent to the motors, potentially clipped. np.ndarray: the action sent to the motors, potentially clipped.
""" """
if not self.is_connected: if not self.is_connected:
raise DeviceNotConnectedError( raise DeviceNotConnectedError("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 # 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_bus.read("Present_Position",self.arm_actuators) present_pos = self.actuators_bus.read("Present_Position", self.arm_actuators)
goal_pos[:6] = ensure_safe_goal_position(goal_pos[:6], 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): This happens synchronously # TODO(Steven): This happens synchronously
self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32),self.arm_actuators) 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) self.actuators_bus.write("Goal_Speed", goal_pos[6:].astype(np.int32), self.base_actuators)
return goal_pos return goal_pos
@ -280,16 +280,14 @@ class LeKiwiRobot(Robot):
def stop(self): def stop(self):
# TODO(Steven): Assumes there's only 3 motors for base # TODO(Steven): Assumes there's only 3 motors for base
logging.info("Stopping base") logging.info("Stopping base")
self.actuators_bus.write("Goal_Speed",[0,0,0],self.base_actuators) self.actuators_bus.write("Goal_Speed", [0, 0, 0], self.base_actuators)
logging.info("Base motors stopped") 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
if not self.is_connected: if not self.is_connected:
raise DeviceNotConnectedError( raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.")
"LeKiwiRobot is not connected. You need to run `robot.connect()`."
)
stop_event = threading.Event() stop_event = threading.Event()
observation_thread = threading.Thread( observation_thread = threading.Thread(
target=self.update_last_observation, args=(stop_event), daemon=True target=self.update_last_observation, args=(stop_event), daemon=True
@ -311,17 +309,16 @@ class LeKiwiRobot(Robot):
# except zmq.Again: # except zmq.Again:
# logging.warning("ZMQ again") # logging.warning("ZMQ again")
except Exception as e: except Exception as e:
logging.warning(f"[ERROR] Message fetching failed: {e}") logging.error("Message fetching failed: %s",e)
# Watchdog: stop the robot if no command is received for over 0.5 seconds. # Watchdog: stop the robot if no command is received for over 0.5 seconds.
now = time.time() now = time.time()
if now - last_cmd_time > 0.5: if now - last_cmd_time > 0.5:
self.stop() self.stop()
pass
with self.observation_lock: with self.observation_lock:
self.zmq_observation_socket.send_string(json.dumps(self.last_observation)) self.zmq_observation_socket.send_string(json.dumps(self.last_observation))
# Ensure a short sleep to avoid overloading the CPU. # Ensure a short sleep to avoid overloading the CPU.
elapsed = time.time() - loop_start_time elapsed = time.time() - loop_start_time
time.sleep( time.sleep(
@ -333,8 +330,7 @@ class LeKiwiRobot(Robot):
stop_event.set() stop_event.set()
observation_thread.join() observation_thread.join()
self.disconnect() self.disconnect()
pass
def print_logs(self): def print_logs(self):
# TODO(Steven): Refactor logger # TODO(Steven): Refactor logger
pass pass
@ -344,7 +340,7 @@ class LeKiwiRobot(Robot):
raise DeviceNotConnectedError( raise DeviceNotConnectedError(
"LeKiwi is not connected. You need to run `robot.connect()` before disconnecting." "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting."
) )
self.stop() self.stop()
self.actuators_bus.disconnect() self.actuators_bus.disconnect()
for cam in self.cameras.values(): for cam in self.cameras.values():
@ -353,7 +349,7 @@ class LeKiwiRobot(Robot):
self.cmd_socket.close() self.cmd_socket.close()
self.context.term() self.context.term()
self.is_connected = False self.is_connected = False
def __del__(self): def __del__(self):
if getattr(self, "is_connected", False): if getattr(self, "is_connected", False):
self.disconnect() self.disconnect()

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

@ -1,5 +1,6 @@
import abc import abc
from pathlib import Path from pathlib import Path
import enum
from typing import Any from typing import Any
import draccus import draccus
@ -9,7 +10,6 @@ from lerobot.common.motors import MotorCalibration
from .config import RobotConfig from .config import RobotConfig
import enum
class RobotMode(enum.Enum): class RobotMode(enum.Enum):
TELEOP = 0 TELEOP = 0

4
lerobot/common/teleoperators/keyboard/teleop_keyboard.py
View File

@ -64,7 +64,7 @@ class KeyboardTeleop(Teleoperator):
@property @property
def action_feature(self) -> dict: def action_feature(self) -> dict:
#TODO(Steven): Verify this is correct # TODO(Steven): Verify this is correct
return { return {
"dtype": "float32", "dtype": "float32",
"shape": (len(self.arm),), "shape": (len(self.arm),),
@ -76,7 +76,7 @@ class KeyboardTeleop(Teleoperator):
return {} return {}
def connect(self) -> None: def connect(self) -> None:
#TODO(Steven): Consider instead of raising a warning and then returning the status # TODO(Steven): Consider instead of raising a warning and then returning the status
# if self.is_connected: # if self.is_connected:
# logging.warning( # logging.warning(
# "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." # "ManipulatorRobot is already connected. Do not run `robot.connect()` twice."