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

Update Koch

This commit is contained in:
Simon Alibert 2025-03-23 19:32:26 +01:00
parent 5b46dc0b6a
commit 7152bc8aa7
2 changed files with 93 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
lerobot/common/robots/koch/robot_koch.py
View File

@ -17,8 +17,7 @@
import json
import logging
import time
import numpy as np
from typing import Any
from lerobot.common.cameras.utils import make_cameras_from_configs
from lerobot.common.constants import OBS_IMAGES, OBS_STATE
@ -26,6 +25,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
from lerobot.common.motors import CalibrationMode, Motor, TorqueMode
from lerobot.common.motors.dynamixel import (
DynamixelMotorsBus,
OperatingMode,
run_arm_calibration,
)
@ -88,6 +88,44 @@ class KochRobot(Robot):
}
return cam_ft
def configure(self) -> None:
for name in self.arm.names:
# Torque must be deactivated to change values in the motors' EEPROM area
# We assume that at configuration time, arm is in a rest position,
# and torque can be safely disabled to run calibration.
self.arm.write("Torque_Enable", name, TorqueMode.DISABLED.value)
if name != "gripper":
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while
# assembling the arm, you could end up with a servo with a position 0 or 4095 at a crucial
# point
self.arm.write("Operating_Mode", name, OperatingMode.Extended_Position.value)
# Use 'position control current based' for gripper to be limited by the limit of the current.
# For the follower gripper, it means it can grasp an object without forcing too much even tho,
# its goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
# For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
# to make it move, and it will move back to its original target position when we release the force.
self.arm.write("Operating_Mode", "gripper", OperatingMode.Current_Position.value)
# Set better PID values to close the gap between recorded states and actions
# TODO(rcadene): Implement an automatic procedure to set optimal PID values for each motor
self.arm.write("Position_P_Gain", "elbow_flex", 1500)
self.arm.write("Position_I_Gain", "elbow_flex", 0)
self.arm.write("Position_D_Gain", "elbow_flex", 600)
for name in self.arm.names:
self.arm.write("Torque_Enable", name, TorqueMode.ENABLED.value)
logging.info("Torque enabled.")
# Move gripper to 45 degrees so that we can use it as a trigger.
self.arm.write("Goal_Position", "gripper", self.config.gripper_open_degree)
@property
def is_connected(self) -> bool:
return self.arm.is_connected # TODO(aliberts): add cam.is_connected for cam in self.cameras
def connect(self) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(
@ -96,40 +134,11 @@ class KochRobot(Robot):
logging.info("Connecting arm.")
self.arm.connect()
# We assume that at connection time, arm is in a rest position,
# and torque can be safely disabled to run calibration.
self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
self.calibrate()
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't
# rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm,
# you could end up with a servo with a position 0 or 4095 at a crucial point See [
# https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11]
all_motors_except_gripper = [name for name in self.arm.motor_names if name != "gripper"]
if len(all_motors_except_gripper) > 0:
# 4 corresponds to Extended Position on Koch motors
self.arm.write("Operating_Mode", 4, all_motors_except_gripper)
# Use 'position control current based' for gripper to be limited by the limit of the current.
# For the follower gripper, it means it can grasp an object without forcing too much even tho,
# it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
# For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
# to make it move, and it will move back to its original target position when we release the force.
# 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288"
self.arm.write("Operating_Mode", 5, "gripper")
# Set better PID values to close the gap between recorded states and actions
# TODO(rcadene): Implement an automatic procedure to set optimal PID values for each motor
self.arm.write("Position_P_Gain", 1500, "elbow_flex")
self.arm.write("Position_I_Gain", 0, "elbow_flex")
self.arm.write("Position_D_Gain", 600, "elbow_flex")
logging.info("Activating torque.")
self.arm.write("Torque_Enable", TorqueMode.ENABLED.value)
self.configure()
# self.calibrate() # TODO
# Check arm can be read
self.arm.read("Present_Position")
self.arm.sync_read("Present_Position")
# Connect the cameras
for cam in self.cameras.values():
@ -157,18 +166,12 @@ class KochRobot(Robot):
self.arm.set_calibration(calibration)
def get_observation(self) -> dict[str, np.ndarray]:
"""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()`."
)
def get_observation(self) -> dict[str, Any]:
obs_dict = {}
# Read arm position
before_read_t = time.perf_counter()
obs_dict[OBS_STATE] = self.arm.read("Present_Position")
obs_dict[OBS_STATE] = self.arm.sync_read("Present_Position")
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
# Capture images from cameras
@ -180,7 +183,7 @@ class KochRobot(Robot):
return obs_dict
def send_action(self, action: np.ndarray) -> np.ndarray:
def send_action(self, action: dict[str, float]) -> dict[str, float]:
"""Command arm to move to a target joint configuration.
The relative action magnitude may be clipped depending on the configuration parameter
@ -188,29 +191,22 @@ class KochRobot(Robot):
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.
action (dict[str, float]): The goal positions for the motors.
Returns:
np.ndarray: the action sent to the motors, potentially clipped.
dict[str, float]: 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.arm.read("Present_Position")
goal_pos = ensure_safe_goal_position(goal_pos, present_pos, self.config.max_relative_target)
present_pos = self.arm.sync_read("Present_Position")
goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()}
goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target)
# Send goal position to the arm
self.arm.write("Goal_Position", goal_pos.astype(np.int32))
self.arm.sync_write("Goal_Position", goal_pos)
return goal_pos

78
lerobot/common/teleoperators/koch/teleop_koch.py
View File

@ -24,6 +24,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
from lerobot.common.motors import CalibrationMode, Motor, TorqueMode
from lerobot.common.motors.dynamixel import (
DynamixelMotorsBus,
OperatingMode,
run_arm_calibration,
)
@ -58,7 +59,6 @@ class KochTeleop(Teleoperator):
},
)
self.is_connected = False
self.logs = {}
@property
@ -73,6 +73,38 @@ class KochTeleop(Teleoperator):
def feedback_feature(self) -> dict:
return {}
def configure(self) -> None:
for name in self.arm.names:
# Torque must be deactivated to change values in the motors' EEPROM area
# We assume that at configuration time, arm is in a rest position,
# and torque can be safely disabled to run calibration.
self.arm.write("Torque_Enable", name, TorqueMode.DISABLED.value)
if name != "gripper":
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while
# assembling the arm, you could end up with a servo with a position 0 or 4095 at a crucial
# point
self.arm.write("Operating_Mode", name, OperatingMode.Extended_Position.value)
# Use 'position control current based' for gripper to be limited by the limit of the current.
# For the follower gripper, it means it can grasp an object without forcing too much even tho,
# its goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
# For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
# to make it move, and it will move back to its original target position when we release the force.
self.arm.write("Operating_Mode", "gripper", OperatingMode.Current_Position.value)
for name in self.arm.names:
self.arm.write("Torque_Enable", name, TorqueMode.ENABLED.value)
logging.info("Torque enabled.")
# Move gripper to 45 degrees so that we can use it as a trigger.
self.arm.write("Goal_Position", "gripper", self.config.gripper_open_degree)
@property
def is_connected(self) -> bool:
return self.arm.is_connected
def connect(self) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(
@ -81,38 +113,11 @@ class KochTeleop(Teleoperator):
logging.info("Connecting arm.")
self.arm.connect()
# We assume that at connection time, arm is in a rest position,
# and torque can be safely disabled to run calibration.
self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
self.calibrate()
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't
# rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm,
# you could end up with a servo with a position 0 or 4095 at a crucial point See [
# https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11]
all_motors_except_gripper = [name for name in self.arm.motor_names if name != "gripper"]
if len(all_motors_except_gripper) > 0:
# 4 corresponds to Extended Position on Koch motors
self.arm.write("Operating_Mode", 4, all_motors_except_gripper)
# Use 'position control current based' for gripper to be limited by the limit of the current.
# For the follower gripper, it means it can grasp an object without forcing too much even tho,
# it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
# For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
# to make it move, and it will move back to its original target position when we release the force.
# 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288"
self.arm.write("Operating_Mode", 5, "gripper")
# Enable torque on the gripper and move it to 45 degrees so that we can use it as a trigger.
logging.info("Activating torque.")
self.arm.write("Torque_Enable", TorqueMode.ENABLED.value, "gripper")
self.arm.write("Goal_Position", self.config.gripper_open_degree, "gripper")
self.configure()
# self.calibrate() # TODO
# Check arm can be read
self.arm.read("Present_Position")
self.is_connected = True
self.arm.sync_read("Present_Position")
def calibrate(self) -> None:
"""After calibration all motors function in human interpretable ranges.
@ -134,18 +139,18 @@ class KochTeleop(Teleoperator):
self.arm.set_calibration(calibration)
def get_action(self) -> np.ndarray:
def get_action(self) -> dict[str, float]:
"""The returned action does not have a batch dimension."""
# Read arm position
before_read_t = time.perf_counter()
action = self.arm.read("Present_Position")
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
start_time = time.perf_counter()
action = self.arm.sync_read("Present_Position")
self.logs["read_pos_dt_s"] = time.perf_counter() - start_time
return action
def send_feedback(self, feedback: np.ndarray) -> None:
# TODO(rcadene, aliberts): Implement force feedback
pass
raise NotImplementedError
def disconnect(self) -> None:
if not self.is_connected:
@ -154,4 +159,3 @@ class KochTeleop(Teleoperator):
)
self.arm.disconnect()
self.is_connected = False