forked from tangger/lerobot
precomit nits
This commit is contained in:
committed by
AdilZouitine
parent
ab85147296
commit
ba477e81ce
@@ -14,10 +14,11 @@
|
|||||||
# 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.
|
||||||
|
|
||||||
from typing import Dict, List, Optional
|
|
||||||
|
|
||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
|
from typing import Dict, List
|
||||||
|
|
||||||
from lerobot.common.cameras import CameraConfig
|
from lerobot.common.cameras import CameraConfig
|
||||||
|
|
||||||
from ..config import RobotConfig
|
from ..config import RobotConfig
|
||||||
|
|
||||||
|
|
||||||
@@ -25,6 +26,7 @@ from ..config import RobotConfig
|
|||||||
@dataclass
|
@dataclass
|
||||||
class SO100FollowerEndEffectorConfig(RobotConfig):
|
class SO100FollowerEndEffectorConfig(RobotConfig):
|
||||||
"""Configuration for the SO100FollowerEndEffector robot."""
|
"""Configuration for the SO100FollowerEndEffector robot."""
|
||||||
|
|
||||||
# Port to connect to the arm
|
# Port to connect to the arm
|
||||||
port: str
|
port: str
|
||||||
|
|
||||||
@@ -39,7 +41,9 @@ class SO100FollowerEndEffectorConfig(RobotConfig):
|
|||||||
cameras: dict[str, CameraConfig] = field(default_factory=dict)
|
cameras: dict[str, CameraConfig] = field(default_factory=dict)
|
||||||
|
|
||||||
# Default bounds for the end-effector position (in meters)
|
# Default bounds for the end-effector position (in meters)
|
||||||
bounds: Dict[str, List[float]] = field(default_factory=lambda: {
|
bounds: Dict[str, List[float]] = field(
|
||||||
"min": [0.1, -0.3, 0.0], # min x, y, z
|
default_factory=lambda: {
|
||||||
"max": [0.4, 0.3, 0.4], # max x, y, z
|
"min": [0.1, -0.3, 0.0], # min x, y, z
|
||||||
} )
|
"max": [0.4, 0.3, 0.4], # max x, y, z
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|||||||
@@ -15,16 +15,17 @@
|
|||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
|
|
||||||
import logging
|
import logging
|
||||||
import numpy as np
|
|
||||||
from typing import Any, Dict
|
from typing import Any, Dict
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
from lerobot.common.errors import DeviceNotConnectedError
|
from lerobot.common.errors import DeviceNotConnectedError
|
||||||
from lerobot.common.model.kinematics import RobotKinematics
|
from lerobot.common.model.kinematics import RobotKinematics
|
||||||
|
from lerobot.common.motors import Motor, MotorNormMode
|
||||||
|
from lerobot.common.motors.feetech import FeetechMotorsBus
|
||||||
|
|
||||||
from ..so100_follower import SO100Follower
|
from ..so100_follower import SO100Follower
|
||||||
from .config_so100_follower_end_effector import SO100FollowerEndEffectorConfig
|
from .config_so100_follower_end_effector import SO100FollowerEndEffectorConfig
|
||||||
from lerobot.common.motors import Motor, MotorNormMode
|
|
||||||
from lerobot.common.motors.feetech import FeetechMotorsBus
|
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
@@ -104,12 +105,16 @@ class SO100FollowerEndEffector(SO100Follower):
|
|||||||
# Convert action to numpy array if not already
|
# Convert action to numpy array if not already
|
||||||
if isinstance(action, dict):
|
if isinstance(action, dict):
|
||||||
if all(k in action for k in ["delta_x", "delta_y", "delta_z", "gripper"]):
|
if all(k in action for k in ["delta_x", "delta_y", "delta_z", "gripper"]):
|
||||||
action = np.array([action["delta_x"], action["delta_y"], action["delta_z"], action["gripper"]], dtype=np.float32)
|
action = np.array(
|
||||||
|
[action["delta_x"], action["delta_y"], action["delta_z"], action["gripper"]],
|
||||||
|
dtype=np.float32,
|
||||||
|
)
|
||||||
else:
|
else:
|
||||||
logger.warning(f"Expected action keys 'delta_x', 'delta_y', 'delta_z', got {list(action.keys())}")
|
logger.warning(
|
||||||
|
f"Expected action keys 'delta_x', 'delta_y', 'delta_z', got {list(action.keys())}"
|
||||||
|
)
|
||||||
action = np.zeros(4, dtype=np.float32)
|
action = np.zeros(4, dtype=np.float32)
|
||||||
|
|
||||||
|
|
||||||
# Read current joint positions
|
# Read current joint positions
|
||||||
current_joint_pos = self.bus.sync_read("Present_Position")
|
current_joint_pos = self.bus.sync_read("Present_Position")
|
||||||
|
|
||||||
@@ -145,7 +150,7 @@ class SO100FollowerEndEffector(SO100Follower):
|
|||||||
# Create joint space action dictionary
|
# Create joint space action dictionary
|
||||||
joint_action = {
|
joint_action = {
|
||||||
f"{joint_names[i]}.pos": target_joint_values_in_degrees[i]
|
f"{joint_names[i]}.pos": target_joint_values_in_degrees[i]
|
||||||
for i in range(len(joint_names)-1) # Exclude gripper
|
for i in range(len(joint_names) - 1) # Exclude gripper
|
||||||
}
|
}
|
||||||
|
|
||||||
# Handle gripper separately if included in action
|
# Handle gripper separately if included in action
|
||||||
@@ -156,6 +161,7 @@ class SO100FollowerEndEffector(SO100Follower):
|
|||||||
joint_action["gripper.pos"] = current_joint_pos[-1]
|
joint_action["gripper.pos"] = current_joint_pos[-1]
|
||||||
|
|
||||||
import time
|
import time
|
||||||
|
|
||||||
time.sleep(0.001)
|
time.sleep(0.001)
|
||||||
# Send joint space action to parent class
|
# Send joint space action to parent class
|
||||||
return super().send_action(joint_action)
|
return super().send_action(joint_action)
|
||||||
@@ -131,4 +131,3 @@ def get_arm_id(name, arm_type):
|
|||||||
like Aloha, it could be left_follower, right_follower, left_leader, or right_leader.
|
like Aloha, it could be left_follower, right_follower, left_leader, or right_leader.
|
||||||
"""
|
"""
|
||||||
return f"{name}_{arm_type}"
|
return f"{name}_{arm_type}"
|
||||||
|
|
||||||
|
|||||||
@@ -14,17 +14,14 @@
|
|||||||
# 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 argparse
|
|
||||||
import logging
|
import logging
|
||||||
import sys
|
|
||||||
import time
|
import time
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import torch
|
import torch
|
||||||
|
|
||||||
from lerobot.common.utils.robot_utils import busy_wait
|
|
||||||
from lerobot.common.utils.utils import init_logging
|
|
||||||
from lerobot.common.robots.kinematics import RobotKinematics
|
from lerobot.common.robots.kinematics import RobotKinematics
|
||||||
|
from lerobot.common.utils.robot_utils import busy_wait
|
||||||
|
|
||||||
|
|
||||||
class InputController:
|
class InputController:
|
||||||
|
|||||||
@@ -15,20 +15,22 @@
|
|||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
|
|
||||||
import sys
|
import sys
|
||||||
|
from enum import IntEnum
|
||||||
from queue import Queue
|
from queue import Queue
|
||||||
from typing import Any
|
from typing import Any
|
||||||
from enum import IntEnum
|
|
||||||
|
|
||||||
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
from ..teleoperator import Teleoperator
|
from ..teleoperator import Teleoperator
|
||||||
from .configuration_gamepad import GamepadTeleopConfig
|
from .configuration_gamepad import GamepadTeleopConfig
|
||||||
|
|
||||||
|
|
||||||
class GripperAction(IntEnum):
|
class GripperAction(IntEnum):
|
||||||
CLOSE = 0
|
CLOSE = 0
|
||||||
STAY = 1
|
STAY = 1
|
||||||
OPEN = 2
|
OPEN = 2
|
||||||
|
|
||||||
|
|
||||||
gripper_action_map = {
|
gripper_action_map = {
|
||||||
"close": GripperAction.CLOSE.value,
|
"close": GripperAction.CLOSE.value,
|
||||||
"open": GripperAction.OPEN.value,
|
"open": GripperAction.OPEN.value,
|
||||||
|
|||||||
Reference in New Issue
Block a user