2475645f5f
Modified gym_manipulator.py and find_joint_limits to adhere to the refactor of robot devices Modified the configuration of envs to take into account the refactor
139 lines
3.8 KiB
Python
139 lines
3.8 KiB
Python
#!/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 sys
|
||
from enum import IntEnum
|
||
from queue import Queue
|
||
from typing import Any
|
||
|
||
import numpy as np
|
||
|
||
from ..teleoperator import Teleoperator
|
||
from .configuration_gamepad import GamepadTeleopConfig
|
||
|
||
|
||
class GripperAction(IntEnum):
|
||
CLOSE = 0
|
||
STAY = 1
|
||
OPEN = 2
|
||
|
||
|
||
gripper_action_map = {
|
||
"close": GripperAction.CLOSE.value,
|
||
"open": GripperAction.OPEN.value,
|
||
"stay": GripperAction.STAY.value,
|
||
}
|
||
|
||
|
||
class GamepadTeleop(Teleoperator):
|
||
"""
|
||
Teleop class to use gamepad inputs for control.
|
||
"""
|
||
|
||
config_class = GamepadTeleopConfig
|
||
name = "gamepad"
|
||
|
||
def __init__(self, config: GamepadTeleopConfig):
|
||
super().__init__(config)
|
||
self.config = config
|
||
self.robot_type = config.type
|
||
|
||
self.event_queue = Queue()
|
||
self.current_pressed = {}
|
||
self.listener = None
|
||
self.logs = {}
|
||
|
||
self.gamepad = None
|
||
|
||
@property
|
||
def action_features(self) -> dict:
|
||
if self.config.use_gripper:
|
||
return {
|
||
"dtype": "float32",
|
||
"shape": (4,),
|
||
"names": {"delta_x": 0, "delta_y": 1, "delta_z": 2, "gripper": 3},
|
||
}
|
||
else:
|
||
return {
|
||
"dtype": "float32",
|
||
"shape": (3,),
|
||
"names": {"delta_x": 0, "delta_y": 1, "delta_z": 2},
|
||
}
|
||
|
||
@property
|
||
def feedback_features(self) -> dict:
|
||
return {}
|
||
|
||
@property
|
||
def is_connected(self) -> bool:
|
||
pass
|
||
|
||
@property
|
||
def is_calibrated(self) -> bool:
|
||
return True
|
||
|
||
def connect(self) -> None:
|
||
# use HidApi for macos
|
||
if sys.platform == "darwin":
|
||
# NOTE: On macOS, pygame doesn’t reliably detect input from some controllers so we fall back to hidapi
|
||
from lerobot.scripts.server.end_effector_control_utils import GamepadControllerHID as Gamepad
|
||
else:
|
||
from lerobot.scripts.server.end_effector_control_utils import GamepadController as Gamepad
|
||
|
||
self.gamepad = Gamepad(
|
||
x_step_size=self.config.x_step_size,
|
||
y_step_size=self.config.y_step_size,
|
||
z_step_size=self.config.z_step_size,
|
||
)
|
||
self.gamepad.start()
|
||
|
||
def calibrate(self) -> None:
|
||
pass
|
||
|
||
def configure(self):
|
||
pass
|
||
|
||
def get_action(self) -> dict[str, Any]:
|
||
# Update the controller to get fresh inputs
|
||
self.gamepad.update()
|
||
|
||
# Get movement deltas from the controller
|
||
delta_x, delta_y, delta_z = self.gamepad.get_deltas()
|
||
|
||
# Create action from gamepad input
|
||
gamepad_action = np.array([delta_x, delta_y, delta_z], dtype=np.float32)
|
||
|
||
action_dict = {
|
||
"delta_x": gamepad_action[0],
|
||
"delta_y": gamepad_action[1],
|
||
"delta_z": gamepad_action[2],
|
||
}
|
||
|
||
# Default gripper action is to stay
|
||
gripper_action = GripperAction.STAY.value
|
||
if self.config.use_gripper:
|
||
gripper_command = self.gamepad.gripper_command()
|
||
gripper_action = gripper_action_map[gripper_command]
|
||
action_dict["gripper"] = gripper_action
|
||
|
||
return action_dict
|
||
|
||
def send_feedback(self, feedback: dict[str, Any]) -> None:
|
||
pass
|
||
|
||
def disconnect(self) -> None:
|
||
pass
|