refactor(robots): multiple changes from feedback
This commit is contained in:
Steven Palma
parent
d7b9866a7c
commit
a0657ee274
|
|
@ -19,6 +19,7 @@ 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
|
||||
|
||||
# TODO(Steven): Check validity of these features
|
||||
DUMMY_FEATURES = {
|
||||
"observation.state": {
|
||||
"dtype": "float64",
|
||||
|
|
@ -118,6 +119,7 @@ def main():
|
|||
# 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()
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,24 @@
|
|||
# TODO(Steven): Consider doing the following:
|
||||
# from enum import Enum
|
||||
# class MyControlTableKey(Enum):
|
||||
# ID = "ID"
|
||||
# GOAL_SPEED = "Goal_Speed"
|
||||
# ...
|
||||
#
|
||||
# MY_CONTROL_TABLE ={
|
||||
# MyControlTableKey.ID.value: (5,1)
|
||||
# MyControlTableKey.GOAL_SPEED.value: (46, 2)
|
||||
# ...
|
||||
# }
|
||||
# This allows me do to:
|
||||
# bus.write(MyControlTableKey.GOAL_SPEED, ...)
|
||||
# Instead of:
|
||||
# bus.write("Goal_Speed", ...)
|
||||
# This is important for two reasons:
|
||||
# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError
|
||||
# 2. We can change the value of the MyControlTableKey enums without impacting the client code
|
||||
|
||||
|
||||
# {data_name: (address, size_byte)}
|
||||
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table
|
||||
X_SERIES_CONTROL_TABLE = {
|
||||
|
|
|
|||
|
|
@ -2,6 +2,26 @@ FIRMWARE_MAJOR_VERSION = (0, 1)
|
|||
FIRMWARE_MINOR_VERSION = (1, 1)
|
||||
MODEL_NUMBER = (3, 2)
|
||||
|
||||
# TODO(Steven): Consider doing the following:
|
||||
# from enum import Enum
|
||||
# class MyControlTableKey(Enum):
|
||||
# ID = "ID"
|
||||
# GOAL_SPEED = "Goal_Speed"
|
||||
# ...
|
||||
#
|
||||
# MY_CONTROL_TABLE ={
|
||||
# MyControlTableKey.ID.value: (5,1)
|
||||
# MyControlTableKey.GOAL_SPEED.value: (46, 2)
|
||||
# ...
|
||||
# }
|
||||
# This allows me do to:
|
||||
# bus.write(MyControlTableKey.GOAL_SPEED, ...)
|
||||
# Instead of:
|
||||
# bus.write("Goal_Speed", ...)
|
||||
# This is important for two reasons:
|
||||
# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError
|
||||
# 2. We can change the value of the MyControlTableKey enums without impacting the client code
|
||||
|
||||
# data_name: (address, size_byte)
|
||||
# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0
|
||||
STS_SMS_SERIES_CONTROL_TABLE = {
|
||||
|
|
|
|||
|
|
@ -632,7 +632,8 @@ class MotorsBus(abc.ABC):
|
|||
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)
|
||||
# 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, most likely a gripper-like one)
|
||||
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:
|
||||
|
|
|
|||
|
|
@ -27,6 +27,8 @@ class RobotMode(enum.Enum):
|
|||
|
||||
|
||||
# TODO(Steven): Consider sending config at initial step over a socket
|
||||
# However, this isn't practical because anyways we have to configure the
|
||||
# socket ports to begin with
|
||||
@RobotConfig.register_subclass("lekiwi")
|
||||
@dataclass
|
||||
class LeKiwiConfig(RobotConfig):
|
||||
|
|
@ -50,6 +52,10 @@ class LeKiwiConfig(RobotConfig):
|
|||
}
|
||||
)
|
||||
|
||||
# Network Configuration
|
||||
port_zmq_cmd: int = 5555
|
||||
port_zmq_observations: int = 5556
|
||||
|
||||
|
||||
@RobotConfig.register_subclass("lekiwi_client")
|
||||
@dataclass
|
||||
|
|
|
|||
|
|
@ -116,9 +116,6 @@ class LeKiwi(Robot):
|
|||
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}")
|
||||
|
||||
|
|
@ -131,9 +128,15 @@ class LeKiwi(Robot):
|
|||
input("Move robot to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.bus.set_half_turn_homings(motors)
|
||||
|
||||
# TODO(Steven): Previously homig_offsets was called only on self.arm_motors
|
||||
# After a discussion, we said it was better to keep it like this and then
|
||||
# just populate with the rest of motors. However, I don't know which value
|
||||
# should we use for this
|
||||
# homing_offsets.update({k,None???} for k in self.base_motors)
|
||||
|
||||
# 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"])
|
||||
motor for motor in motors if any(keyword in motor for keyword in ["wheel", "wrist"])
|
||||
]
|
||||
unknown_range_motors = [motor for motor in motors if motor not in full_turn_motor]
|
||||
|
||||
|
|
@ -180,7 +183,7 @@ class LeKiwi(Robot):
|
|||
for name in self.base_motors:
|
||||
self.bus.write("Operating_Mode", name, OperatingMode.VELOCITY.value)
|
||||
|
||||
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!
|
||||
self.bus.enable_torque()
|
||||
|
||||
def get_observation(self) -> dict[str, Any]:
|
||||
if not self.is_connected:
|
||||
|
|
@ -191,7 +194,7 @@ class LeKiwi(Robot):
|
|||
# Read actuators position for arm and vel for base
|
||||
start = time.perf_counter()
|
||||
arm_pos = self.bus.sync_read("Present_Position", self.arm_motors)
|
||||
base_vel = self.bus.sync_read("Present_Speed", self.base_motors)
|
||||
base_vel = self.bus.sync_read("Present_Velocity", self.base_motors)
|
||||
obs_dict[OBS_STATE] = {**arm_pos, **base_vel}
|
||||
dt_ms = (time.perf_counter() - start) * 1e3
|
||||
logger.debug(f"{self} read state: {dt_ms:.1f}ms")
|
||||
|
|
@ -238,12 +241,12 @@ class LeKiwi(Robot):
|
|||
|
||||
# Send goal position to the actuators
|
||||
self.bus.sync_write("Goal_Position", arm_goal_pos)
|
||||
self.bus.sync_write("Goal_Speed", base_goal_vel)
|
||||
self.bus.sync_write("Goal_Velocity", base_goal_vel)
|
||||
|
||||
return {**arm_goal_pos, **base_goal_vel}
|
||||
|
||||
def stop_base(self):
|
||||
self.bus.sync_write("Goal_Speed", dict.fromkeys(self.base_motors, 0), num_retry=5)
|
||||
self.bus.sync_write("Goal_Velocity", dict.fromkeys(self.base_motors, 0), num_retry=5)
|
||||
logger.info("Base motors stopped")
|
||||
|
||||
def disconnect(self):
|
||||
|
|
|
|||
|
|
@ -24,10 +24,9 @@ import zmq
|
|||
|
||||
from lerobot.common.constants import OBS_IMAGES, OBS_STATE
|
||||
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError
|
||||
from lerobot.common.robots.config import RobotMode
|
||||
|
||||
from ..robot import Robot
|
||||
from .config_lekiwi import LeKiwiClientConfig
|
||||
from .config_lekiwi import LeKiwiClientConfig, RobotMode
|
||||
|
||||
|
||||
# TODO(Steven): This doesn't need to inherit from Robot
|
||||
|
|
@ -81,9 +80,8 @@ class LeKiwiClient(Robot):
|
|||
|
||||
@property
|
||||
def state_feature(self) -> dict:
|
||||
# TODO(Steven): Get this from the data fetched?
|
||||
# TODO(Steven): Motor names are unknown for the Daemon
|
||||
# Or assume its size/metadata?
|
||||
# TODO(Steven): Get this from the data fetched? Motor names are unknown for the Daemon
|
||||
# For now we assume its size/metadata is known
|
||||
return {
|
||||
"dtype": "float64",
|
||||
"shape": (9,),
|
||||
|
|
@ -108,9 +106,8 @@ class LeKiwiClient(Robot):
|
|||
|
||||
@property
|
||||
def camera_features(self) -> dict[str, dict]:
|
||||
# TODO(Steven): Get this from the data fetched?
|
||||
# TODO(Steven): camera names are unknown for the Daemon
|
||||
# Or assume its size/metadata?
|
||||
# TODO(Steven): Get this from the data fetched? Motor names are unknown for the Daemon
|
||||
# For now we assume its size/metadata is known
|
||||
# TODO(Steven): Check consistency of image sizes
|
||||
cam_ft = {
|
||||
"front": {
|
||||
|
|
@ -128,7 +125,8 @@ class LeKiwiClient(Robot):
|
|||
|
||||
@property
|
||||
def is_connected(self) -> bool:
|
||||
# TODO(Steven): Check instead the status of the sockets
|
||||
# TODO(Steven): Ideally we could check instead the status of the sockets
|
||||
# I didn't find any API that allows us to do that easily
|
||||
return self._is_connected
|
||||
|
||||
@property
|
||||
|
|
@ -138,6 +136,7 @@ class LeKiwiClient(Robot):
|
|||
def connect(self) -> None:
|
||||
"""Establishes ZMQ sockets with the remote mobile robot"""
|
||||
|
||||
# TODO(Steven): Consider instead returning a bool + warn
|
||||
if self._is_connected:
|
||||
raise DeviceAlreadyConnectedError(
|
||||
"LeKiwi Daemon is already connected. Do not run `robot.connect()` twice."
|
||||
|
|
@ -174,7 +173,7 @@ class LeKiwiClient(Robot):
|
|||
speed_int = -0x8000 # -32768 -> minimum negative value
|
||||
return speed_int
|
||||
|
||||
# Copied from robot_lekiwi MobileManipulator class
|
||||
# Copied from robot_lekiwi MobileManipulator class* (before the refactor)
|
||||
@staticmethod
|
||||
def _raw_to_degps(raw_speed: int) -> float:
|
||||
steps_per_deg = 4096.0 / 360.0
|
||||
|
|
@ -182,7 +181,7 @@ class LeKiwiClient(Robot):
|
|||
degps = magnitude / steps_per_deg
|
||||
return degps
|
||||
|
||||
# Copied from robot_lekiwi MobileManipulator class
|
||||
# Copied from robot_lekiwi MobileManipulator class* (before the refactor)
|
||||
def _body_to_wheel_raw(
|
||||
self,
|
||||
x_cmd: float,
|
||||
|
|
@ -285,6 +284,7 @@ class LeKiwiClient(Robot):
|
|||
|
||||
# TODO(Steven): This is flaky, for example, if we received a state but failed decoding the image, we will not update any value
|
||||
# TODO(Steven): All this function needs to be refactored
|
||||
# Copied from robot_lekiwi MobileManipulator class* (before the refactor)
|
||||
def _get_data(self):
|
||||
# Copied from robot_lekiwi.py
|
||||
"""Polls the video socket for up to 15 ms. If data arrives, decode only
|
||||
|
|
@ -369,7 +369,7 @@ class LeKiwiClient(Robot):
|
|||
if not self._is_connected:
|
||||
raise DeviceNotConnectedError("LeKiwiClient is not connected. You need to run `robot.connect()`.")
|
||||
|
||||
# TODO(Steven): remove hard-coded cam name
|
||||
# TODO(Steven): remove hard-coded cam names & dims
|
||||
# This is needed at init for when there's no comms
|
||||
obs_dict = {
|
||||
OBS_IMAGES: {"wrist": np.zeros(shape=(480, 640, 3)), "front": np.zeros(shape=(640, 480, 3))}
|
||||
|
|
@ -377,7 +377,7 @@ class LeKiwiClient(Robot):
|
|||
|
||||
frames, present_speed, remote_arm_state_tensor = self._get_data()
|
||||
body_state = self._wheel_raw_to_body(present_speed)
|
||||
# TODO(Steven): output isdict[str,Any] and we multiply by 1000.0. This should be more explicit and specify the expected type instead of Any
|
||||
# TODO(Steven): output is dict[str,Any] and we multiply by 1000.0. This should be more explicit and specify the expected type instead of Any
|
||||
body_state_mm = {k: v * 1000.0 for k, v in body_state.items()} # Convert x,y to mm/s
|
||||
|
||||
obs_dict[OBS_STATE] = {**remote_arm_state_tensor, **body_state_mm}
|
||||
|
|
@ -423,7 +423,7 @@ class LeKiwiClient(Robot):
|
|||
def configure(self):
|
||||
pass
|
||||
|
||||
# TODO(Steven): This assumes this call is always called from a keyboard teleop command
|
||||
# TODO(Steven): This assumes this call is always called with a keyboard as a teleop device. It breaks if we teleop with other device
|
||||
# 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)
|
||||
|
|
@ -454,7 +454,7 @@ class LeKiwiClient(Robot):
|
|||
|
||||
if self.robot_mode is RobotMode.AUTO:
|
||||
# TODO(Steven): Not yet implemented. The policy outputs might need a different conversion
|
||||
raise Exception
|
||||
raise InvalidActionError("Policy output as action input is not yet well defined")
|
||||
|
||||
goal_pos = {}
|
||||
# TODO(Steven): This assumes teleop mode is always used with keyboard. Tomorrow we could teleop with another device ... ?
|
||||
|
|
|
|||
|
|
@ -27,22 +27,17 @@ from lerobot.common.constants import OBS_IMAGES
|
|||
from .config_lekiwi import LeKiwiConfig
|
||||
from .lekiwi import LeKiwi
|
||||
|
||||
# TODO(Steven): Move this to the config file
|
||||
# Network Configuration
|
||||
PORT_ZMQ_CMD: int = 5555
|
||||
PORT_ZMQ_OBSERVATIONS: int = 5556
|
||||
|
||||
|
||||
class HostAgent:
|
||||
def __init__(self):
|
||||
def __init__(self, port_zmq_cmd, port_zmq_observations):
|
||||
self.zmq_context = zmq.Context()
|
||||
self.zmq_cmd_socket = self.zmq_context.socket(zmq.PULL)
|
||||
self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1)
|
||||
self.zmq_cmd_socket.bind(f"tcp://*:{PORT_ZMQ_CMD}")
|
||||
self.zmq_cmd_socket.bind(f"tcp://*:{port_zmq_cmd}")
|
||||
|
||||
self.zmq_observation_socket = self.zmq_context.socket(zmq.PUSH)
|
||||
self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1)
|
||||
self.zmq_observation_socket.bind(f"tcp://*:{PORT_ZMQ_OBSERVATIONS}")
|
||||
self.zmq_observation_socket.bind(f"tcp://*:{port_zmq_observations}")
|
||||
|
||||
def disconnect(self):
|
||||
self.zmq_observation_socket.close()
|
||||
|
|
@ -59,7 +54,7 @@ def main():
|
|||
robot.connect()
|
||||
|
||||
logging.info("Starting HostAgent")
|
||||
remote_agent = HostAgent()
|
||||
remote_agent = HostAgent(robot_config.port_zmq_cmd, robot_config.port_zmq_observations)
|
||||
|
||||
last_cmd_time = time.time()
|
||||
logging.info("Waiting for commands...")
|
||||
|
|
|
|||
|
|
@ -22,11 +22,10 @@ class Robot(abc.ABC):
|
|||
def __init__(self, config: RobotConfig):
|
||||
self.robot_type = self.name
|
||||
self.id = config.id
|
||||
self.robot_mode = config.robot_mode
|
||||
self.calibration_dir = (
|
||||
Path(config.calibration_dir)
|
||||
if config.calibration_dir
|
||||
else HF_LEROBOT_CALIBRATION / ROBOTS / self.name
|
||||
else Path(HF_LEROBOT_CALIBRATION / ROBOTS / self.name)
|
||||
)
|
||||
self.calibration_dir.mkdir(parents=True, exist_ok=True)
|
||||
self.calibration_fpath = self.calibration_dir / f"{self.id}.json"
|
||||
|
|
|
|||
|
|
@ -65,9 +65,10 @@ def make_robot_from_config(config: RobotConfig):
|
|||
|
||||
return ManipulatorRobot(config)
|
||||
elif isinstance(config, LeKiwiConfig):
|
||||
from lerobot.common.robots.mobile_manipulator import MobileManipulator
|
||||
|
||||
return MobileManipulator(config)
|
||||
# TODO(Steven): Change when we decide what to do with these scripts
|
||||
# from lerobot.common.robots.mobile_manipulator import MobileManipulator
|
||||
# return MobileManipulator(config)
|
||||
...
|
||||
else:
|
||||
from lerobot.common.robots.stretch3.robot_stretch3 import Stretch3Robot
|
||||
|
||||
|
|
|
|||
|
|
@ -22,5 +22,5 @@ from ..config import TeleoperatorConfig
|
|||
@TeleoperatorConfig.register_subclass("keyboard")
|
||||
@dataclass
|
||||
class KeyboardTeleopConfig(TeleoperatorConfig):
|
||||
# TODO(Steven): Maybe set in here the keys that we want to capture/listen
|
||||
# TODO(Steven): Consider setting in here the keys that we want to capture/listen
|
||||
mock: bool = False
|
||||
|
|
|
|||
|
|
@ -63,12 +63,8 @@ class KeyboardTeleop(Teleoperator):
|
|||
|
||||
@property
|
||||
def action_feature(self) -> dict:
|
||||
# TODO(Steven): Verify this is correct
|
||||
return {
|
||||
"dtype": "float32",
|
||||
"shape": (len(self.arm),),
|
||||
"names": {"motors": list(self.arm.motors)},
|
||||
}
|
||||
# TODO(Steven): Change this when we agree what should this return
|
||||
...
|
||||
|
||||
@property
|
||||
def feedback_feature(self) -> dict:
|
||||
|
|
@ -83,15 +79,15 @@ class KeyboardTeleop(Teleoperator):
|
|||
pass
|
||||
|
||||
def connect(self) -> None:
|
||||
# TODO(Steven): Consider instead of raising a warning and then returning the status
|
||||
# TODO(Steven): Consider early return instead of raising a warning
|
||||
# if self._is_connected:
|
||||
# logging.warning(
|
||||
# "ManipulatorRobot is already connected. Do not run `robot.connect()` twice."
|
||||
# "Keyboard is already connected. Do not run `robot.connect()` twice."
|
||||
# )
|
||||
# return self._is_connected
|
||||
if self._is_connected:
|
||||
raise DeviceAlreadyConnectedError(
|
||||
"ManipulatorRobot is already connected. Do not run `robot.connect()` twice."
|
||||
"Keyboard is already connected. Do not run `robot.connect()` twice."
|
||||
)
|
||||
|
||||
if PYNPUT_AVAILABLE:
|
||||
|
|
|
|||
|
|
@ -45,9 +45,6 @@ class Teleoperator(abc.ABC):
|
|||
def is_connected(self) -> bool:
|
||||
pass
|
||||
|
||||
# TODO(Steven): I think connect() should return a bool, such that the client/application code can check if the device connected successfully
|
||||
# if not device.connect():
|
||||
# raise DeviceNotConnectedError(f"{device} failed to connect")
|
||||
@abc.abstractmethod
|
||||
def connect(self) -> None:
|
||||
"""Connects to the teleoperator."""
|
||||
|
|
|
|||
|
|
@ -422,10 +422,11 @@ def control_robot(cfg: ControlPipelineConfig):
|
|||
elif isinstance(cfg.control, ReplayControlConfig):
|
||||
replay(robot, cfg.control)
|
||||
elif isinstance(cfg.control, RemoteRobotConfig):
|
||||
from lerobot.common.robots.lekiwi.old_lekiwi_remote import run_lekiwi
|
||||
|
||||
_init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_remote")
|
||||
run_lekiwi(cfg.robot)
|
||||
...
|
||||
# TODO(Steven): Change this when we decide what to do with the control_robot script
|
||||
# from lerobot.common.robots.lekiwi.old_lekiwi_remote import run_lekiwi
|
||||
# _init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_remote")
|
||||
# run_lekiwi(cfg.robot)
|
||||
|
||||
if robot.is_connected:
|
||||
# Disconnect manually to avoid a "Core dump" during process
|
||||
|
|
|
|||
Loading…
Reference in New Issue