diff --git a/lerobot/common/robot_devices/motors/trossen_arm_driver.py b/lerobot/common/robot_devices/motors/trossen_arm_driver.py
index 77e1465b..cc06be3c 100644
--- a/lerobot/common/robot_devices/motors/trossen_arm_driver.py
+++ b/lerobot/common/robot_devices/motors/trossen_arm_driver.py
@@ -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.")
diff --git a/lerobot/common/robot_devices/robots/configs.py b/lerobot/common/robot_devices/robots/configs.py
index e45daee6..ecb27ef7 100644
--- a/lerobot/common/robot_devices/robots/configs.py
+++ b/lerobot/common/robot_devices/robots/configs.py
@@ -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(
@@ -754,9 +764,24 @@ class TrossenAISoloRobotConfig(ManipulatorRobotConfig):
@RobotConfig.register_subclass("trossen_ai_mobile")
@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",
),
}
)
diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py
index 452abbb1..bbc0f323 100644
--- a/lerobot/common/robot_devices/robots/manipulator.py
+++ b/lerobot/common/robot_devices/robots/manipulator.py
@@ -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