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

fix(lekiwi): HW fixes v0.4

This commit is contained in:
Steven Palma 2025-03-17 18:49:05 +01:00
parent db874c6812
commit e1d6a6f00e
No known key found for this signature in database
5 changed files with 33 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,6 +13,8 @@ class DaemonLeKiwiRobotConfig(RobotConfig):
id = "daemonlekiwi"
calibration_dir: str = ".cache/calibration/lekiwi"
teleop_keys: dict[str, str] = field(
default_factory=lambda: {
# Movement

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

@ -16,10 +16,10 @@ class LeKiwiRobotConfig(RobotConfig):
cameras: dict[str, CameraConfig] = field(
default_factory=lambda: {
"front": OpenCVCameraConfig(
camera_index="/dev/video0", fps=30, width=640, height=480, rotation=90
camera_index="/dev/video1", fps=30, width=640, height=480, rotation=90
),
"wrist": OpenCVCameraConfig(
camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180
camera_index="/dev/video4", fps=30, width=640, height=480, rotation=180
),
}
)

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

@ -62,7 +62,7 @@ class DaemonLeKiwiRobot(Robot):
self.zmq_observation_socket = None
self.last_frames = {}
self.last_present_speed = {}
self.last_present_speed = [0, 0, 0]
self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
# Define three speed levels and a current index
@ -223,13 +223,13 @@ class DaemonLeKiwiRobot(Robot):
# Copied from robot_lekiwi MobileManipulator class
def wheel_raw_to_body(
self, wheel_raw: dict, wheel_radius: float = 0.05, base_radius: float = 0.125
self, wheel_raw: np.array, wheel_radius: float = 0.05, base_radius: float = 0.125
) -> tuple:
"""
Convert wheel raw command feedback back into body-frame velocities.
Parameters:
wheel_raw : Dictionary with raw wheel commands (keys: "left_wheel", "back_wheel", "right_wheel").
wheel_raw : Vector with raw wheel commands ("left_wheel", "back_wheel", "right_wheel").
wheel_radius: Radius of each wheel (meters).
base_radius : Distance from the robot center to each wheel (meters).
@ -239,15 +239,9 @@ class DaemonLeKiwiRobot(Robot):
y_cmd : Linear velocity in y (m/s).
theta_cmd : Rotational velocity in deg/s.
"""
# Extract the raw values in order.
raw_list = [
int(wheel_raw.get("left_wheel", 0)),
int(wheel_raw.get("back_wheel", 0)),
int(wheel_raw.get("right_wheel", 0)),
]
# Convert each raw command back to an angular speed in deg/s.
wheel_degps = np.array([DaemonLeKiwiRobot.raw_to_degps(r) for r in raw_list])
wheel_degps = np.array([DaemonLeKiwiRobot.raw_to_degps(int(r)) for r in wheel_raw])
# Convert from deg/s to rad/s.
wheel_radps = wheel_degps * (np.pi / 180.0)
# Compute each wheel’s linear speed (m/s) from its angular speed.
@ -264,6 +258,7 @@ class DaemonLeKiwiRobot(Robot):
theta_cmd = theta_rad * (180.0 / np.pi)
return (x_cmd, y_cmd, theta_cmd)
# TODO(Steven): This is flaky, for example, if we received a state but failed decoding the image, we will not update any value
def get_data(self):
# Copied from robot_lekiwi.py
"""Polls the video socket for up to 15 ms. If data arrives, decode only
@ -271,10 +266,10 @@ class DaemonLeKiwiRobot(Robot):
nothing arrives for any field, use the last known values."""
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.empty(6, dtype=torch.float32)
# Poll up to 15 ms
poller = zmq.Poller()
@ -303,7 +298,9 @@ class DaemonLeKiwiRobot(Robot):
# Decode only the final message
try:
observation = json.loads(last_msg)
observation[OBS_STATE] = np.array(observation[OBS_STATE])
# TODO(Steven): Consider getting directly the item with observation[OBS_STATE]
state_observation = {k: v for k, v in observation.items() if k.startswith(OBS_STATE)}
image_observation = {k: v for k, v in observation.items() if k.startswith(OBS_IMAGES)}
@ -320,10 +317,10 @@ class DaemonLeKiwiRobot(Robot):
if state_observation is not None and frames is not None:
self.last_frames = frames
remote_arm_state_tensor = torch.tensor(state_observation[:6], dtype=torch.float32)
remote_arm_state_tensor = torch.tensor(state_observation[OBS_STATE][:6], dtype=torch.float32)
self.last_remote_arm_state = remote_arm_state_tensor
present_speed = state_observation[6:]
present_speed = state_observation[OBS_STATE][6:]
self.last_present_speed = present_speed
else:
frames = self.last_frames

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

@ -32,7 +32,7 @@ def main():
logging.info("Starting LeKiwiRobot teleoperation")
start = time.perf_counter()
duration = 0
while duration < 20:
while duration < 100:
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

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

@ -203,6 +203,7 @@ class LeKiwiRobot(Robot):
self.actuators_bus.set_calibration(calibration)
# TODO(Steven): Should this be dict[str, Any] ?
def get_observation(self) -> dict[str, np.ndarray]:
"""The returned observations do not have a batch dimension."""
if not self.is_connected:
@ -274,13 +275,17 @@ class LeKiwiRobot(Robot):
def update_last_observation(self, stop_event):
while not stop_event.is_set():
obs = self.get_observation()
obs[OBS_STATE] = obs[OBS_STATE].tolist() # Needed for np.array be serializable
with self.observation_lock:
self.last_observation = obs
# TODO(Steven): Consider adding a delay to not starve the CPU
# TODO(Steven): Check this value
time.sleep(0.5)
def stop(self):
# TODO(Steven): Assumes there's only 3 motors for base
logging.info("Stopping base")
# TODO(Steven): Check if these operations are thread safe!
self.actuators_bus.write("Goal_Speed", [0, 0, 0], self.base_actuators)
logging.info("Base motors stopped")
@ -299,23 +304,28 @@ class LeKiwiRobot(Robot):
logging.info("LeKiwi robot server started. Waiting for commands...")
try:
while True:
start = time.perf_counter()
duration = 0
while duration < 100:
loop_start_time = time.time()
try:
msg = self.zmq_cmd_socket.recv_string(zmq.NOBLOCK)
data = np.array(json.loads(msg))
self.send_action(data)
_action_sent = self.send_action(data)
last_cmd_time = time.time()
except zmq.Again:
logging.warning("ZMQ again")
except Exception as e:
logging.error("Message fetching failed: %s", e)
# TODO(Steven): Check this value
# Watchdog: stop the robot if no command is received for over 0.5 seconds.
now = time.time()
if now - last_cmd_time > 0.5:
self.stop()
# TODO(Steven): This doesn't seem to be thread safe!
# self.stop()
pass
with self.observation_lock:
# TODO(Steven): This operation is blocking if no listener is available
@ -323,9 +333,12 @@ class LeKiwiRobot(Robot):
# Ensure a short sleep to avoid overloading the CPU.
elapsed = time.time() - loop_start_time
# TODO(Steven): Check this value
time.sleep(
max(0.033 - elapsed, 0)
) # If robot jitters increase the sleep and monitor cpu load with `top` in cmd
duration = time.perf_counter() - start
except KeyboardInterrupt:
print("Shutting down LeKiwi server.")
finally:
@ -349,7 +362,7 @@ class LeKiwiRobot(Robot):
cam.disconnect()
self.zmq_observation_socket.close()
self.zmq_cmd_socket.close()
self.context.term()
self.zmq_context.term()
self.is_connected = False
def __del__(self):