Force feedback (#22)

* Remove Compute Time to Move Method

* Added Force Feedback

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
+        # Minimum time to move for the arm
-        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)
         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.driver.set_all_positions(values.tolist(), self.MIN_TIME_TO_MOVE, False)
-            self.prev_write_time = self.current_write_time
 
         # 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.")
 

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",
             ),
         }
     )

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