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

Force feedback (#22) 

* Remove Compute Time to Move Method

* Added Force Feedback
This commit is contained in:
shantanuparab-tr 2025-04-03 14:37:45 -05:00 committed by GitHub
parent 9c44e8da8f
commit 5db796f3ff
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 39 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
lerobot/common/robot_devices/motors/trossen_arm_driver.py
View File

@ -10,14 +10,12 @@ from lerobot.common.robot_devices.utils import (
RobotDeviceNotConnectedError,
)
PITCH_CIRCLE_RADIUS = 0.00875 # meters
VEL_LIMITS = [3.375, 3.375, 3.375, 7.0, 7.0, 7.0, 12.5 * PITCH_CIRCLE_RADIUS]
TROSSEN_ARM_MODELS = {
"V0_LEADER": [trossen.Model.wxai_v0, trossen.StandardEndEffector.wxai_v0_leader],
"V0_FOLLOWER": [trossen.Model.wxai_v0, trossen.StandardEndEffector.wxai_v0_follower],
}
class TrossenArmDriver:
"""
The `TrossenArmDriver` class provides an interface for controlling
@ -63,7 +61,6 @@ class TrossenArmDriver:
```
"""
def __init__(
self,
config: TrossenArmDriverConfig,
@ -79,7 +76,7 @@ class TrossenArmDriver:
self.home_pose = [0, np.pi/3, np.pi/6, np.pi/5, 0, 0, 0]
self.sleep_pose = [0, 0, 0, 0, 0, 0, 0]
self.motors={
self.motors = {
# name: (index, model)
"joint_0": [1, "4340"],
"joint_1": [2, "4340"],
@ -90,15 +87,7 @@ class TrossenArmDriver:
"joint_6": [7, "4310"],
}
self.prev_write_time = 0
self.current_write_time = None
# To prevent DiscontinuityError due to large jumps in position in short time.
# We scale the time to move based on the distance between the start and goal values and the maximum speed of the motors.
# The below factor is used to scale the time to move.
self.TIME_SCALING_FACTOR = 3.0
# Minimum time to move for the arm (This is a tuning parameter)
# Minimum time to move for the arm
self.MIN_TIME_TO_MOVE = 3.0 / self.fps
def connect(self):
@ -138,7 +127,6 @@ class TrossenArmDriver:
# Allow to read and write
self.is_connected = True
def reconnect(self):
try:
model_name, model_end_effector = TROSSEN_ARM_MODELS[self.model]
@ -155,7 +143,6 @@ class TrossenArmDriver:
self.is_connected = True
@property
def motor_names(self) -> list[str]:
return list(self.motors.keys())
@ -197,6 +184,8 @@ class TrossenArmDriver:
values = self.driver.get_positions()
values[:-1] = np.degrees(values[:-1]) # Convert all joints except gripper
values[-1] = values[-1] * 10000 # Convert gripper to range (0-450)
elif data_name == "External_Efforts":
values = self.driver.get_external_efforts()
else:
values = None
print(f"Data name: {data_name} is not supported for reading.")
@ -208,16 +197,6 @@ class TrossenArmDriver:
values = np.array(values, dtype=np.float32)
return values
def compute_time_to_move(self, goal_values: np.ndarray):
# Compute the time to move based on the distance between the start and goal values
# and the maximum speed of the motors
current_pose = self.driver.get_positions()
displacement = abs(goal_values - current_pose)
time_to_move_all_joints = self.TIME_SCALING_FACTOR*displacement / VEL_LIMITS
time_to_move = max(time_to_move_all_joints)
time_to_move = max(time_to_move, self.MIN_TIME_TO_MOVE)
return time_to_move
def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None):
if not self.is_connected:
raise RobotDeviceNotConnectedError(
@ -232,8 +211,7 @@ class TrossenArmDriver:
# Convert back to radians for joints
values[:-1] = np.radians(values[:-1]) # Convert all joints except gripper
values[-1] = values[-1] / 10000 # Convert gripper back to range (0-0.045)
self.driver.set_all_positions(values.tolist(), self.compute_time_to_move(values), False)
self.prev_write_time = self.current_write_time
self.driver.set_all_positions(values.tolist(), self.MIN_TIME_TO_MOVE, False)
# Enable or disable the torque of the motors
elif data_name == "Torque_Enable":
@ -248,6 +226,8 @@ class TrossenArmDriver:
self.driver.set_all_velocities([0.0] * self.driver.get_num_joints(), 0.0, False)
self.driver.set_all_modes(trossen.Mode.position)
self.driver.set_all_positions(self.home_pose, 2.0, False)
elif data_name == "External_Efforts":
self.driver.set_all_external_efforts(values, 0.0, False)
else:
print(f"Data name: {data_name} value: {values} is not supported for writing.")

29
lerobot/common/robot_devices/robots/configs.py
View File

@ -630,6 +630,11 @@ class TrossenAIStationaryRobotConfig(ManipulatorRobotConfig):
# then to gradually add more motors (by uncommenting), until you can teleoperate both arms fully
max_relative_target: int | None = 5
# Gain applied to external efforts sensed on the follower arm and transmitted to the leader arm.
# This enables the user to feel external forces (e.g., contact with objects) through force feedback.
# A value of 0.0 disables force feedback. A good starting value for a responsive experience is 0.1.
force_feedback_gain: float = 0.0
leader_arms: dict[str, MotorsBusConfig] = field(
default_factory=lambda: {
"left": TrossenArmDriverConfig(
@ -653,7 +658,7 @@ class TrossenAIStationaryRobotConfig(ManipulatorRobotConfig):
),
"right": TrossenArmDriverConfig(
ip="192.168.1.4",
model = "V0_FOLLOWER",
model="V0_FOLLOWER",
),
}
)
@ -709,6 +714,11 @@ class TrossenAISoloRobotConfig(ManipulatorRobotConfig):
# then to gradually add more motors (by uncommenting), until you can teleoperate both arms fully
max_relative_target: int | None = 5
# Gain applied to external efforts sensed on the follower arm and transmitted to the leader arm.
# This enables the user to feel external forces (e.g., contact with objects) through force feedback.
# A value of 0.0 disables force feedback. A good starting value for a responsive experience is 0.1.
force_feedback_gain: float = 0.0
leader_arms: dict[str, MotorsBusConfig] = field(
default_factory=lambda: {
"main": TrossenArmDriverConfig(
@ -755,8 +765,23 @@ class TrossenAISoloRobotConfig(ManipulatorRobotConfig):
@dataclass
class TrossenAIMobileRobotConfig(RobotConfig):
# /!\ FOR SAFETY, READ THIS /!\
# `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.
# For Trossen AI Arms, for every goal position request, motor rotations are capped at 5 degrees by default.
# When you feel more confident with teleoperation or running the policy, you can extend
# this safety limit and even removing it by setting it to `null`.
# Also, everything is expected to work safely out-of-the-box, but we highly advise to
# first try to teleoperate the grippers only (by commenting out the rest of the motors in this yaml),
# then to gradually add more motors (by uncommenting), until you can teleoperate both arms fully
max_relative_target: int | None = 5
# Gain applied to external efforts sensed on the follower arm and transmitted to the leader arm.
# This enables the user to feel external forces (e.g., contact with objects) through force feedback.
# A value of 0.0 disables force feedback. A good starting value for a responsive experience is 0.1.
force_feedback_gain: float = 0.0
enable_motor_torque: bool = False
leader_arms: dict[str, MotorsBusConfig] = field(
@ -782,7 +807,7 @@ class TrossenAIMobileRobotConfig(RobotConfig):
),
"right": TrossenArmDriverConfig(
ip="192.168.1.4",
model = "V0_FOLLOWER",
model="V0_FOLLOWER",
),
}
)

3
lerobot/common/robot_devices/robots/manipulator.py
View File

@ -165,6 +165,7 @@ class ManipulatorRobot:
self.leader_arms = make_motors_buses_from_configs(self.config.leader_arms)
self.follower_arms = make_motors_buses_from_configs(self.config.follower_arms)
self.cameras = make_cameras_from_configs(self.config.cameras)
self.force_feedback_gain = self.config.force_feedback_gain
self.is_connected = False
self.logs = {}
@ -494,6 +495,8 @@ class ManipulatorRobot:
goal_pos = goal_pos.numpy().astype(np.float32)
self.follower_arms[name].write("Goal_Position", goal_pos)
external_efforts = -1 * self.force_feedback_gain * self.follower_arms[name].read("External_Efforts")
self.leader_arms[name].write("External_Efforts", external_efforts)
self.logs[f"write_follower_{name}_goal_pos_dt_s"] = time.perf_counter() - before_fwrite_t
# Early exit when recording data is not requested