refactor(robots): lekiwi v0.2
This commit is contained in:
Steven Palma
Steven Palma
parent
6a3cdc0489
commit
982f8c0c16
|
|
@ -0,0 +1,37 @@
|
|||
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
|
||||
|
||||
|
||||
@RobotConfig.register_subclass("daemon_lekiwi")
|
||||
@dataclass
|
||||
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
|
||||
remote_ip: str = "192.168.0.193"
|
||||
port_zmq_cmd: int = 5555
|
||||
port_zmq_observations: int = 5556
|
||||
|
||||
teleop_keys: dict[str, str] = field(
|
||||
default_factory=lambda: {
|
||||
# Movement
|
||||
"forward": "w",
|
||||
"backward": "s",
|
||||
"left": "a",
|
||||
"right": "d",
|
||||
"rotate_left": "z",
|
||||
"rotate_right": "x",
|
||||
# Speed control
|
||||
"speed_up": "r",
|
||||
"speed_down": "f",
|
||||
# quit teleop
|
||||
"quit": "q",
|
||||
}
|
||||
)
|
||||
|
|
@ -2,22 +2,15 @@ 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
|
||||
|
||||
|
||||
@RobotConfig.register_subclass("lekiwi")
|
||||
@dataclass
|
||||
class LeKiwiRobotConfig(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
|
||||
ip: str = "192.168.0.193"
|
||||
port: int = 5555
|
||||
video_port: int = 5556
|
||||
port_zmq_cmd: int = 5555
|
||||
port_zmq_observations: int = 5556
|
||||
|
||||
cameras: dict[str, CameraConfig] = field(
|
||||
default_factory=lambda: {
|
||||
|
|
@ -31,59 +24,16 @@ class LeKiwiRobotConfig(RobotConfig):
|
|||
)
|
||||
|
||||
calibration_dir: str = ".cache/calibration/lekiwi"
|
||||
|
||||
port_motor_bus = "/dev/ttyACM0"
|
||||
|
||||
leader_arms: dict[str, MotorsBusConfig] = field(
|
||||
default_factory=lambda: {
|
||||
"main": FeetechMotorsBusConfig(
|
||||
port="/dev/tty.usbmodem585A0077581",
|
||||
motors={
|
||||
# name: (index, model)
|
||||
"shoulder_pan": [1, "sts3215"],
|
||||
"shoulder_lift": [2, "sts3215"],
|
||||
"elbow_flex": [3, "sts3215"],
|
||||
"wrist_flex": [4, "sts3215"],
|
||||
"wrist_roll": [5, "sts3215"],
|
||||
"gripper": [6, "sts3215"],
|
||||
},
|
||||
),
|
||||
}
|
||||
)
|
||||
|
||||
follower_arms: dict[str, MotorsBusConfig] = field(
|
||||
default_factory=lambda: {
|
||||
"main": FeetechMotorsBusConfig(
|
||||
port="/dev/ttyACM0",
|
||||
motors={
|
||||
# name: (index, model)
|
||||
"shoulder_pan": [1, "sts3215"],
|
||||
"shoulder_lift": [2, "sts3215"],
|
||||
"elbow_flex": [3, "sts3215"],
|
||||
"wrist_flex": [4, "sts3215"],
|
||||
"wrist_roll": [5, "sts3215"],
|
||||
"gripper": [6, "sts3215"],
|
||||
"left_wheel": (7, "sts3215"),
|
||||
"back_wheel": (8, "sts3215"),
|
||||
"right_wheel": (9, "sts3215"),
|
||||
},
|
||||
),
|
||||
}
|
||||
)
|
||||
|
||||
teleop_keys: dict[str, str] = field(
|
||||
default_factory=lambda: {
|
||||
# Movement
|
||||
"forward": "w",
|
||||
"backward": "s",
|
||||
"left": "a",
|
||||
"right": "d",
|
||||
"rotate_left": "z",
|
||||
"rotate_right": "x",
|
||||
# Speed control
|
||||
"speed_up": "r",
|
||||
"speed_down": "f",
|
||||
# quit teleop
|
||||
"quit": "q",
|
||||
}
|
||||
)
|
||||
|
||||
mock: bool = False
|
||||
# TODO(Steven): consider split this into arm and base
|
||||
shoulder_pan: tuple = (1, "sts3215")
|
||||
shoulder_lift: tuple = (2, "sts3215")
|
||||
elbow_flex: tuple=(3, "sts3215")
|
||||
wrist_flex: tuple=(4, "sts3215")
|
||||
wrist_roll: tuple=(5, "sts3215")
|
||||
gripper: tuple =(6, "sts3215")
|
||||
left_wheel: tuple= (7, "sts3215")
|
||||
back_wheel: tuple = (8, "sts3215")
|
||||
right_wheel: tuple = (9, "sts3215")
|
||||
|
|
|
|||
|
|
@ -0,0 +1,437 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import json
|
||||
import logging
|
||||
import time
|
||||
import threading
|
||||
import numpy as np
|
||||
import time
|
||||
# import torch
|
||||
import base64
|
||||
import cv2
|
||||
import torch
|
||||
|
||||
from lerobot.common.cameras.utils import make_cameras_from_configs
|
||||
from lerobot.common.constants import OBS_IMAGES, OBS_STATE
|
||||
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
|
||||
from ..robot import Robot
|
||||
from ..utils import ensure_safe_goal_position
|
||||
from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
|
||||
import zmq
|
||||
|
||||
# TODO(Steven): This doesn't need to inherit from Robot
|
||||
# But we do it for now to offer a familiar API
|
||||
# TODO(Steven): This doesn't need to take care of the
|
||||
# mapping from teleop to motor commands, but given that
|
||||
# we already have a middle-man (this class) we add it here
|
||||
class DaemonLeKiwiRobot(Robot):
|
||||
|
||||
config_class = DaemonLeKiwiRobotConfig
|
||||
name = "daemonlekiwi"
|
||||
|
||||
def __init__(self, config: DaemonLeKiwiRobotConfig):
|
||||
super().__init__(config)
|
||||
self.config = config
|
||||
self.id = config.id
|
||||
self.robot_type = config.type
|
||||
|
||||
self.max_relative_target = config.max_relative_target
|
||||
|
||||
self.remote_ip = config.remote_ip
|
||||
self.port_zmq_cmd = config.port_zmq_cmd
|
||||
self.port_zmq_observations = config.port_zmq_observations
|
||||
|
||||
self.teleop_keys = config.teleop_keys
|
||||
|
||||
self.zmq_context = None
|
||||
self.zmq_cmd_socket = None
|
||||
self.zmq_observation_socket = None
|
||||
|
||||
self.last_frames = {}
|
||||
self.last_present_speed = {}
|
||||
self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
|
||||
|
||||
# Define three speed levels and a current index
|
||||
self.speed_levels = [
|
||||
{"xy": 0.1, "theta": 30}, # slow
|
||||
{"xy": 0.2, "theta": 60}, # medium
|
||||
{"xy": 0.3, "theta": 90}, # fast
|
||||
]
|
||||
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.logs = {}
|
||||
|
||||
@property
|
||||
def state_feature(self) -> dict:
|
||||
# TODO(Steven): Get this from the data fetched?
|
||||
# return {
|
||||
# "dtype": "float32",
|
||||
# "shape": (len(self.actuators),),
|
||||
# "names": {"motors": list(self.actuators.motors)},
|
||||
# }
|
||||
pass
|
||||
|
||||
@property
|
||||
def action_feature(self) -> dict:
|
||||
return self.state_feature
|
||||
|
||||
@property
|
||||
def camera_features(self) -> dict[str, dict]:
|
||||
# TODO(Steven): Fetch this info or set it static?
|
||||
# cam_ft = {}
|
||||
# for cam_key, cam in self.cameras.items():
|
||||
# cam_ft[cam_key] = {
|
||||
# "shape": (cam.height, cam.width, cam.channels),
|
||||
# "names": ["height", "width", "channels"],
|
||||
# "info": None,
|
||||
# }
|
||||
# return cam_ft
|
||||
pass
|
||||
|
||||
def connect(self) -> None:
|
||||
if self.is_connected:
|
||||
raise DeviceAlreadyConnectedError(
|
||||
"LeKiwi Daemon is already connected. Do not run `robot.connect()` twice."
|
||||
)
|
||||
|
||||
self.zmq_context = zmq.Context()
|
||||
self.zmq_cmd_socket = self.zmq_context.socket(zmq.PUSH)
|
||||
zmq_cmd_locator = f"tcp://{self.remote_ip}:{self.port_zmq_cmd}"
|
||||
self.zmq_cmd_socket.connect(zmq_cmd_locator)
|
||||
self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1)
|
||||
|
||||
self.zmq_observation_socket = self.zmq_context.socket(zmq.PULL)
|
||||
zmq_observations_locator = f"tcp://{self.remote_ip}:{self.port_zmq_observations}"
|
||||
self.zmq_observation_socket.connect(zmq_observations_locator)
|
||||
self.zmq_observation_socket.setsockopt(zmq.CONFLATE,1)
|
||||
|
||||
self.is_connected = True
|
||||
|
||||
def calibrate(self) -> None:
|
||||
# TODO(Steven): Nothing to calibrate
|
||||
pass
|
||||
|
||||
# Consider moving these static functions out of the class
|
||||
# Copied from robot_lekiwi MobileManipulator class
|
||||
@staticmethod
|
||||
def degps_to_raw(degps: float) -> int:
|
||||
steps_per_deg = 4096.0 / 360.0
|
||||
speed_in_steps = abs(degps) * steps_per_deg
|
||||
speed_int = int(round(speed_in_steps))
|
||||
if speed_int > 0x7FFF:
|
||||
speed_int = 0x7FFF
|
||||
if degps < 0:
|
||||
return speed_int | 0x8000
|
||||
else:
|
||||
return speed_int & 0x7FFF
|
||||
|
||||
# Copied from robot_lekiwi MobileManipulator class
|
||||
@staticmethod
|
||||
def raw_to_degps(raw_speed: int) -> float:
|
||||
steps_per_deg = 4096.0 / 360.0
|
||||
magnitude = raw_speed & 0x7FFF
|
||||
degps = magnitude / steps_per_deg
|
||||
if raw_speed & 0x8000:
|
||||
degps = -degps
|
||||
return degps
|
||||
|
||||
# Copied from robot_lekiwi MobileManipulator class
|
||||
def body_to_wheel_raw(
|
||||
self,
|
||||
x_cmd: float,
|
||||
y_cmd: float,
|
||||
theta_cmd: float,
|
||||
wheel_radius: float = 0.05,
|
||||
base_radius: float = 0.125,
|
||||
max_raw: int = 3000,
|
||||
) -> dict:
|
||||
"""
|
||||
Convert desired body-frame velocities into wheel raw commands.
|
||||
|
||||
Parameters:
|
||||
x_cmd : Linear velocity in x (m/s).
|
||||
y_cmd : Linear velocity in y (m/s).
|
||||
theta_cmd : Rotational velocity (deg/s).
|
||||
wheel_radius: Radius of each wheel (meters).
|
||||
base_radius : Distance from the center of rotation to each wheel (meters).
|
||||
max_raw : Maximum allowed raw command (ticks) per wheel.
|
||||
|
||||
Returns:
|
||||
A dictionary with wheel raw commands:
|
||||
{"left_wheel": value, "back_wheel": value, "right_wheel": value}.
|
||||
|
||||
Notes:
|
||||
- Internally, the method converts theta_cmd to rad/s for the kinematics.
|
||||
- The raw command is computed from the wheels angular speed in deg/s
|
||||
using degps_to_raw(). If any command exceeds max_raw, all commands
|
||||
are scaled down proportionally.
|
||||
"""
|
||||
# Convert rotational velocity from deg/s to rad/s.
|
||||
theta_rad = theta_cmd * (np.pi / 180.0)
|
||||
# Create the body velocity vector [x, y, theta_rad].
|
||||
velocity_vector = np.array([x_cmd, y_cmd, theta_rad])
|
||||
|
||||
# Define the wheel mounting angles with a -90° offset.
|
||||
angles = np.radians(np.array([240, 120, 0]) - 90)
|
||||
# Build the kinematic matrix: each row maps body velocities to a wheel’s linear speed.
|
||||
# The third column (base_radius) accounts for the effect of rotation.
|
||||
m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
|
||||
|
||||
# Compute each wheel’s linear speed (m/s) and then its angular speed (rad/s).
|
||||
wheel_linear_speeds = m.dot(velocity_vector)
|
||||
wheel_angular_speeds = wheel_linear_speeds / wheel_radius
|
||||
|
||||
# Convert wheel angular speeds from rad/s to deg/s.
|
||||
wheel_degps = wheel_angular_speeds * (180.0 / np.pi)
|
||||
|
||||
# Scaling
|
||||
steps_per_deg = 4096.0 / 360.0
|
||||
raw_floats = [abs(degps) * steps_per_deg for degps in wheel_degps]
|
||||
max_raw_computed = max(raw_floats)
|
||||
if max_raw_computed > max_raw:
|
||||
scale = max_raw / max_raw_computed
|
||||
wheel_degps = wheel_degps * scale
|
||||
|
||||
# Convert each wheel’s angular speed (deg/s) to a raw integer.
|
||||
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]}
|
||||
|
||||
# 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
|
||||
) -> 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_radius: Radius of each wheel (meters).
|
||||
base_radius : Distance from the robot center to each wheel (meters).
|
||||
|
||||
Returns:
|
||||
A tuple (x_cmd, y_cmd, theta_cmd) where:
|
||||
x_cmd : Linear velocity in x (m/s).
|
||||
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])
|
||||
# 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.
|
||||
wheel_linear_speeds = wheel_radps * wheel_radius
|
||||
|
||||
# Define the wheel mounting angles with a -90° offset.
|
||||
angles = np.radians(np.array([240, 120, 0]) - 90)
|
||||
m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
|
||||
|
||||
# Solve the inverse kinematics: body_velocity = M⁻¹ · wheel_linear_speeds.
|
||||
m_inv = np.linalg.inv(m)
|
||||
velocity_vector = m_inv.dot(wheel_linear_speeds)
|
||||
x_cmd, y_cmd, theta_rad = velocity_vector
|
||||
theta_cmd = theta_rad * (180.0 / np.pi)
|
||||
return (x_cmd, y_cmd, theta_cmd)
|
||||
|
||||
def get_data(self):
|
||||
"""Polls the video socket for up to 15 ms. If data arrives, decode only
|
||||
the *latest* message, returning frames, speed, and arm state. If
|
||||
nothing arrives for any field, use the last known values."""
|
||||
|
||||
frames = {}
|
||||
present_speed = {}
|
||||
remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32)
|
||||
|
||||
# Poll up to 15 ms
|
||||
poller = zmq.Poller()
|
||||
poller.register(self.zmq_observation_socket, zmq.POLLIN)
|
||||
socks = dict(poller.poll(15))
|
||||
if self.zmq_observation_socket not in socks or socks[self.zmq_observation_socket] != zmq.POLLIN:
|
||||
# No new data arrived → reuse ALL old data
|
||||
return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
|
||||
|
||||
# Drain all messages, keep only the last
|
||||
last_msg = None
|
||||
while True:
|
||||
try:
|
||||
obs_string = self.zmq_observation_socket.recv_string(zmq.NOBLOCK)
|
||||
last_msg = obs_string
|
||||
except zmq.Again:
|
||||
break
|
||||
|
||||
if not last_msg:
|
||||
# No new message → also reuse old
|
||||
return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
|
||||
|
||||
# Decode only the final message
|
||||
try:
|
||||
observation = json.loads(last_msg)
|
||||
|
||||
#TODO(Steven): Check this
|
||||
images_dict = observation.get("images", {})
|
||||
new_speed = observation.get("present_speed", {})
|
||||
new_arm_state = observation.get("follower_arm_state", None)
|
||||
|
||||
# Convert images
|
||||
for cam_name, image_b64 in images_dict.items():
|
||||
if image_b64:
|
||||
jpg_data = base64.b64decode(image_b64)
|
||||
np_arr = np.frombuffer(jpg_data, dtype=np.uint8)
|
||||
frame_candidate = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
|
||||
if frame_candidate is not None:
|
||||
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 new_arm_state is not None and frames is not None:
|
||||
self.last_frames = frames
|
||||
|
||||
remote_arm_state_tensor = torch.tensor(new_arm_state, dtype=torch.float32)
|
||||
self.last_remote_arm_state = remote_arm_state_tensor
|
||||
|
||||
present_speed = new_speed
|
||||
self.last_present_speed = new_speed
|
||||
else:
|
||||
frames = self.last_frames
|
||||
|
||||
remote_arm_state_tensor = self.last_remote_arm_state
|
||||
|
||||
present_speed = self.last_present_speed
|
||||
|
||||
except Exception as e:
|
||||
print(f"[DEBUG] Error decoding video message: {e}")
|
||||
# If decode fails, fall back to old data
|
||||
return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
|
||||
return frames, present_speed, remote_arm_state_tensor
|
||||
|
||||
# TODO(Steven): The returned space is different from the get_observation of LeKiwiRobot
|
||||
# This returns body-frames velocities instead of wheel pos/speeds
|
||||
def get_observation(self) -> dict[str, np.ndarray]:
|
||||
"""
|
||||
Capture observations from the remote robot: current follower arm positions,
|
||||
present wheel speeds (converted to body-frame velocities: x, y, theta),
|
||||
and a camera frame.
|
||||
"""
|
||||
if not self.is_connected:
|
||||
raise DeviceNotConnectedError(
|
||||
"DaemonLeKiwiRobot is not connected. You need to run `robot.connect()`."
|
||||
)
|
||||
|
||||
obs_dict = {}
|
||||
|
||||
# TODO(Steven): Check this
|
||||
frames, present_speed, remote_arm_state_tensor = self.get_data()
|
||||
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
|
||||
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)
|
||||
|
||||
obs_dict = {OBS_STATE: combined_state_tensor}
|
||||
|
||||
# Loop over each configured camera
|
||||
for cam_name, cam in self.cameras.items():
|
||||
frame = frames.get(cam_name, None)
|
||||
if frame is None:
|
||||
# Create a black image using the camera's configured width, height, and channels
|
||||
frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8)
|
||||
obs_dict[f"{OBS_IMAGES}.{cam_name}"] = torch.from_numpy(frame)
|
||||
|
||||
return obs_dict
|
||||
|
||||
def from_teleop_action_to_motor_action(self, action):
|
||||
# # Speed control
|
||||
# elif key.char == self.teleop_keys["speed_up"]:
|
||||
# self.speed_index = min(self.speed_index + 1, 2)
|
||||
# print(f"Speed index increased to {self.speed_index}")
|
||||
# elif key.char == self.teleop_keys["speed_down"]:
|
||||
# self.speed_index = max(self.speed_index - 1, 0)
|
||||
# print(f"Speed index decreased to {self.speed_index}")
|
||||
pass
|
||||
|
||||
# TODO(Steven)
|
||||
def send_action(self, action: np.ndarray) -> np.ndarray:
|
||||
# Copied from S100 robot
|
||||
"""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:
|
||||
action (np.ndarray): array containing the goal positions for the motors.
|
||||
|
||||
Raises:
|
||||
RobotDeviceNotConnectedError: if robot is not connected.
|
||||
|
||||
Returns:
|
||||
np.ndarray: the action sent to the motors, potentially clipped.
|
||||
"""
|
||||
if not self.is_connected:
|
||||
raise DeviceNotConnectedError(
|
||||
"ManipulatorRobot is not connected. You need to run `robot.connect()`."
|
||||
)
|
||||
|
||||
goal_pos = action
|
||||
|
||||
# Cap goal position when too far away from present position.
|
||||
# /!\ Slower fps expected due to reading from the follower.
|
||||
if self.config.max_relative_target is not None:
|
||||
present_pos = self.actuators.read("Present_Position")
|
||||
goal_pos = ensure_safe_goal_position(goal_pos, present_pos, self.config.max_relative_target)
|
||||
|
||||
# Send goal position to the actuators
|
||||
# TODO(Steven): Base motors should set a vel instead
|
||||
self.actuators.write("Goal_Position", goal_pos.astype(np.int32))
|
||||
|
||||
return goal_pos
|
||||
|
||||
def print_logs(self):
|
||||
# TODO(Steven): Refactor logger
|
||||
pass
|
||||
|
||||
def disconnect(self):
|
||||
if not self.is_connected:
|
||||
raise DeviceNotConnectedError(
|
||||
"LeKiwi is not connected. You need to run `robot.connect()` before disconnecting."
|
||||
)
|
||||
# TODO(Steven): Consider sending a stop to the remote mobile robot
|
||||
self.zmq_observation_socket.close()
|
||||
self.zmq_cmd_socket.close()
|
||||
self.zmq_context.term()
|
||||
self.is_connected = False
|
||||
|
||||
def __del__(self):
|
||||
if getattr(self, "is_connected", False):
|
||||
self.disconnect()
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig
|
||||
from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
|
||||
from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
|
||||
from .daemon_lekiwi import DaemonLeKiwiRobot
|
||||
import time
|
||||
import logging
|
||||
|
||||
def main():
|
||||
|
||||
logging.info("Configuring Teleop Devices")
|
||||
leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511")
|
||||
leader_arm = SO100Teleop(leader_arm_config)
|
||||
|
||||
keyboard_config = KeyboardTeleopConfig()
|
||||
keyboard = KeyboardTeleop(keyboard_config)
|
||||
|
||||
logging.info("Connecting Teleop Devices")
|
||||
leader_arm.connect()
|
||||
keyboard.connect()
|
||||
|
||||
logging.info("Configuring LeKiwiRobot Daemon")
|
||||
robot_config = DaemonLeKiwiRobotConfig()
|
||||
robot = DaemonLeKiwiRobot(robot_config)
|
||||
|
||||
logging.info("Connecting remote LeKiwiRobot")
|
||||
robot.connect() # Establishes ZMQ sockets with the remote mobile robot
|
||||
|
||||
logging.info("Starting LeKiwiRobot teleoperation")
|
||||
start = time.perf_counter()
|
||||
duration = 0
|
||||
while duration < 20:
|
||||
|
||||
arm_action = leader_arm.get_action()
|
||||
base_action = keyboard.get_action()
|
||||
action = {
|
||||
**arm_action,
|
||||
**base_action
|
||||
}
|
||||
robot.send_action(action) # Translates to motor space + sends over ZMQ
|
||||
robot.get_observation() # Receives over ZMQ, translate to body-frame vel
|
||||
|
||||
duration = time.perf_counter() - start
|
||||
|
||||
logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon")
|
||||
robot.disconnect() # Cleans ZMQ comms
|
||||
leader_arm.disconnect()
|
||||
keyboard.disconnect()
|
||||
|
||||
logging.info("Finished LeKiwiRobot cleanly")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
def main():
|
||||
teleop_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511")
|
||||
teleop_arm = SO100Teleop(teleop_arm_config)
|
||||
|
||||
keyboard_config = KeyboardTeleopConfig()
|
||||
keyboard = KeyboardTeleop(keyboard_config)
|
||||
|
||||
robot_config = kiwiconfig(port="/dev/tty.usbmodem575E0032081")
|
||||
robot = KiwiRobotDaemon(robot_config)
|
||||
|
||||
teleop_arm.connect()
|
||||
keyboard.connect()
|
||||
robot.connect() # Establish ZMQ sockets with the mobile robot
|
||||
|
||||
start = time.perf_counter()
|
||||
duration = 0
|
||||
while duration < 20:
|
||||
|
||||
arm_action = teleop_arm.get_action()
|
||||
base_action = keyboard.get_action()
|
||||
action = {
|
||||
**arm_action,
|
||||
# **base_action ??
|
||||
}
|
||||
robot.send_action(action) # sends over ZMQ
|
||||
# robot.get_observation() # receives over ZMQ
|
||||
|
||||
print(action)
|
||||
duration = time.perf_counter() - start
|
||||
|
||||
robot.disconnect() # cleans ZMQ comms
|
||||
teleop.disconnect()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
import logging
|
||||
from .configuration_lekiwi import LeKiwiRobotConfig
|
||||
from .lekiwi_robot import LeKiwiRobot
|
||||
|
||||
def main():
|
||||
|
||||
logging.info("Configuring LeKiwiRobot")
|
||||
robot_config = LeKiwiRobotConfig()
|
||||
robot = LeKiwiRobot(robot_config)
|
||||
|
||||
logging.info("Connecting LeKiwiRobot")
|
||||
robot.connect()
|
||||
|
||||
# Remotely teleoperated
|
||||
logging.info("Starting LeKiwiRobot teleoperation")
|
||||
robot.run()
|
||||
|
||||
logging.info("Finished LeKiwiRobot cleanly")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -21,6 +21,8 @@ import threading
|
|||
import numpy as np
|
||||
import time
|
||||
# import torch
|
||||
import base64
|
||||
import cv2
|
||||
|
||||
from lerobot.common.cameras.utils import make_cameras_from_configs
|
||||
from lerobot.common.constants import OBS_IMAGES, OBS_STATE
|
||||
|
|
@ -49,21 +51,17 @@ class LeKiwiRobot(Robot):
|
|||
def __init__(self, config: LeKiwiRobotConfig):
|
||||
super().__init__(config)
|
||||
self.config = config
|
||||
# self.robot_type = config.type
|
||||
# self.id = config.id
|
||||
self.remote_ip = config.ip
|
||||
self.remote_port = config.port
|
||||
self.remote_port_video = config.video_port
|
||||
# self.logs = {}
|
||||
# TODO(Steven): This should go to teleop
|
||||
# self.teleop_keys = self.config.teleop_keys
|
||||
self.id = config.id
|
||||
|
||||
self.port_zmq_cmd = config.port_zmq_cmd
|
||||
self.port_zmq_observations = config.port_zmq_observations
|
||||
|
||||
# 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
|
||||
# what we consider 'lekiwi robot' always uses the FeetechMotorsBus
|
||||
# TODO(Steve): We will need to have a key for arm and base for calibration
|
||||
# TODO(Steven): We will need to have a key for arm and base for calibration
|
||||
self.actuators = FeetechMotorsBus(
|
||||
port=self.config.port,
|
||||
port=self.config.port_motor_bus,
|
||||
motors={
|
||||
"shoulder_pan": config.shoulder_pan,
|
||||
"shoulder_lift": config.shoulder_lift,
|
||||
|
|
@ -77,24 +75,18 @@ class LeKiwiRobot(Robot):
|
|||
},
|
||||
)
|
||||
|
||||
|
||||
|
||||
#TODO(Steven): Consider removing cameras from configs
|
||||
self.cameras = make_cameras_from_configs(config.cameras)
|
||||
|
||||
self.is_connected = False
|
||||
self.logs = {}
|
||||
self.cameras = make_cameras_from_configs(config.cameras)
|
||||
|
||||
self.observation_lock = threading.Lock()
|
||||
self.last_observation = None
|
||||
# self.last_frames = {}
|
||||
# self.last_present_speed = {}
|
||||
# self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
|
||||
|
||||
# ZeroMQ context and sockets.
|
||||
self.context = None
|
||||
self.cmd_socket = None
|
||||
self.observation_socket = None
|
||||
self.zmq_context = None
|
||||
self.zmq_cmd_socket = None
|
||||
self.zmq_observation_socket = None
|
||||
|
||||
self.is_connected = False
|
||||
self.logs = {}
|
||||
|
||||
|
||||
|
||||
|
|
@ -121,15 +113,15 @@ class LeKiwiRobot(Robot):
|
|||
}
|
||||
return cam_ft
|
||||
|
||||
def setup_zmq_sockets(self, config):
|
||||
def setup_zmq_sockets(self):
|
||||
context = zmq.Context()
|
||||
cmd_socket = context.socket(zmq.PULL)
|
||||
cmd_socket.setsockopt(zmq.CONFLATE, 1)
|
||||
cmd_socket.bind(f"tcp://*:{config.port}")
|
||||
cmd_socket.bind(f"tcp://*:{self.port_zmq_cmd}")
|
||||
|
||||
observation_socket = context.socket(zmq.PUSH)
|
||||
observation_socket.setsockopt(zmq.CONFLATE, 1)
|
||||
observation_socket.bind(f"tcp://*:{config.video_port}")
|
||||
observation_socket.bind(f"tcp://*:{self.port_zmq_observations}")
|
||||
|
||||
return context, cmd_socket, observation_socket
|
||||
|
||||
|
|
@ -176,10 +168,14 @@ class LeKiwiRobot(Robot):
|
|||
cam.connect()
|
||||
|
||||
logging.info("Connecting ZMQ sockets.")
|
||||
self.context, self.cmd_socket, self.observation_socket = self.setup_zmq_sockets(self.config)
|
||||
self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets(self.config)
|
||||
|
||||
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:
|
||||
# Copied from S100 robot
|
||||
"""After calibration all motors function in human interpretable ranges.
|
||||
|
|
@ -192,7 +188,6 @@ class LeKiwiRobot(Robot):
|
|||
with open(actuators_calib_path) as f:
|
||||
calibration = json.load(f)
|
||||
else:
|
||||
# TODO(rcadene): display a warning in __init__ if calibration file not available
|
||||
logging.info(f"Missing calibration file '{actuators_calib_path}'")
|
||||
calibration = run_arm_manual_calibration(self.actuators, self.robot_type, self.name, "follower")
|
||||
|
||||
|
|
@ -207,7 +202,7 @@ class LeKiwiRobot(Robot):
|
|||
"""The returned observations do not have a batch dimension."""
|
||||
if not self.is_connected:
|
||||
raise DeviceNotConnectedError(
|
||||
"ManipulatorRobot is not connected. You need to run `robot.connect()`."
|
||||
"LeKiwiRobot is not connected. You need to run `robot.connect()`."
|
||||
)
|
||||
|
||||
obs_dict = {}
|
||||
|
|
@ -221,7 +216,12 @@ class LeKiwiRobot(Robot):
|
|||
# Capture images from cameras
|
||||
for cam_key, cam in self.cameras.items():
|
||||
before_camread_t = time.perf_counter()
|
||||
obs_dict[f"{OBS_IMAGES}.{cam_key}"] = cam.async_read()
|
||||
frame = cam.async_read()
|
||||
ret, buffer = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), 90])
|
||||
if ret:
|
||||
obs_dict[f"{OBS_IMAGES}.{cam_key}"] = base64.b64encode(buffer).decode("utf-8")
|
||||
else:
|
||||
obs_dict[f"{OBS_IMAGES}.{cam_key}"] = ""
|
||||
self.logs[f"read_camera_{cam_key}_dt_s"] = cam.logs["delta_timestamp_s"]
|
||||
self.logs[f"async_read_camera_{cam_key}_dt_s"] = time.perf_counter() - before_camread_t
|
||||
|
||||
|
|
@ -246,7 +246,7 @@ class LeKiwiRobot(Robot):
|
|||
"""
|
||||
if not self.is_connected:
|
||||
raise DeviceNotConnectedError(
|
||||
"ManipulatorRobot is not connected. You need to run `robot.connect()`."
|
||||
"LeKiwiRobot is not connected. You need to run `robot.connect()`."
|
||||
)
|
||||
|
||||
goal_pos = action
|
||||
|
|
@ -265,15 +265,21 @@ class LeKiwiRobot(Robot):
|
|||
|
||||
def update_last_observation(self, stop_event):
|
||||
while not stop_event.is_set():
|
||||
obs = self.get_observation()
|
||||
with self.observation_lock:
|
||||
self.last_observation = self.get_observation()
|
||||
self.last_observation = obs
|
||||
# TODO(Steven): Consider adding a delay to not starve the CPU
|
||||
|
||||
def stop(self):
|
||||
# TODO(Steven): Base motors speed should be set to 0
|
||||
pass
|
||||
|
||||
def run(self):
|
||||
# Copied from run_lekiwi in lekiwi_remote.py
|
||||
# TODO(Steven): Csnsider with, finally
|
||||
# Copied logic from run_lekiwi in lekiwi_remote.py
|
||||
if not self.is_connected:
|
||||
self.connect()
|
||||
raise DeviceNotConnectedError(
|
||||
"LeKiwiRobot is not connected. You need to run `robot.connect()`."
|
||||
)
|
||||
|
||||
stop_event = threading.Event()
|
||||
observation_thread = threading.Thread(
|
||||
|
|
@ -302,12 +308,11 @@ class LeKiwiRobot(Robot):
|
|||
# Watchdog: stop the robot if no command is received for over 0.5 seconds.
|
||||
now = time.time()
|
||||
if now - last_cmd_time > 0.5:
|
||||
# TODO(Steven): Implement stop()
|
||||
#self.stop()
|
||||
self.stop()
|
||||
pass
|
||||
|
||||
with self.observation_lock:
|
||||
self.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.
|
||||
elapsed = time.time() - loop_start_time
|
||||
|
|
@ -317,11 +322,13 @@ class LeKiwiRobot(Robot):
|
|||
except KeyboardInterrupt:
|
||||
print("Shutting down LeKiwi server.")
|
||||
finally:
|
||||
#TODO(Steven): Implement finally
|
||||
stop_event.set()
|
||||
observation_thread.join()
|
||||
self.disconnect()
|
||||
pass
|
||||
|
||||
def print_logs(self):
|
||||
# TODO(aliberts): move robot-specific logs logic here
|
||||
# TODO(Steven): Refactor logger
|
||||
pass
|
||||
|
||||
def disconnect(self):
|
||||
|
|
@ -329,11 +336,16 @@ class LeKiwiRobot(Robot):
|
|||
raise DeviceNotConnectedError(
|
||||
"LeKiwi is not connected. You need to run `robot.connect()` before disconnecting."
|
||||
)
|
||||
# TODO(Steven): Base motors speed should be set to 0
|
||||
# TODO(Steven): Close ZMQ sockets
|
||||
# TODO(Steven): Stop main loop threads
|
||||
|
||||
self.stop()
|
||||
self.actuators.disconnect()
|
||||
for cam in self.cameras.values():
|
||||
cam.disconnect()
|
||||
|
||||
self.observation_socket.close()
|
||||
self.cmd_socket.close()
|
||||
self.context.term()
|
||||
self.is_connected = False
|
||||
|
||||
def __del__(self):
|
||||
if getattr(self, "is_connected", False):
|
||||
self.disconnect()
|
||||
Loading…
Reference in New Issue