From 97086cdcdf46e2b2663447ba16de2c9b09ceef29 Mon Sep 17 00:00:00 2001 From: Alexander Soare Date: Mon, 26 Aug 2024 12:28:16 +0100 Subject: [PATCH] Make gripper_open_degree a config param (#379) --- lerobot/common/robot_devices/robots/koch.py | 17 +++++++++++------ lerobot/configs/robot/koch.yaml | 3 +++ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/lerobot/common/robot_devices/robots/koch.py b/lerobot/common/robot_devices/robots/koch.py index f5966999a..064ea629e 100644 --- a/lerobot/common/robot_devices/robots/koch.py +++ b/lerobot/common/robot_devices/robots/koch.py @@ -26,7 +26,6 @@ URL_TEMPLATE = ( # In nominal degree range ]-180, +180[ ZERO_POSITION_DEGREE = 0 ROTATED_POSITION_DEGREE = 90 -GRIPPER_OPEN_DEGREE = 35.156 def assert_drive_mode(drive_mode): @@ -165,6 +164,11 @@ class KochRobotConfig: follower_arms: dict[str, MotorsBus] = field(default_factory=lambda: {}) cameras: dict[str, Camera] = field(default_factory=lambda: {}) + # Optionally set the leader arm in torque mode with the gripper motor set to this angle. This makes it + # possible to squeeze the gripper and have it spring back to an open position on its own. If None, the + # gripper is not put in torque mode. + gripper_open_degree: float | None = None + class KochRobot: # TODO(rcadene): Implement force feedback @@ -339,11 +343,12 @@ class KochRobot: print(f"Activating torque on {name} follower arm.") self.follower_arms[name].write("Torque_Enable", 1) - # Enable torque on the gripper of the leader arms, and move it to 45 degrees, - # so that we can use it as a trigger to close the gripper of the follower arms. - for name in self.leader_arms: - self.leader_arms[name].write("Torque_Enable", 1, "gripper") - self.leader_arms[name].write("Goal_Position", GRIPPER_OPEN_DEGREE, "gripper") + if self.config.gripper_open_degree is not None: + # Set the leader arm in torque mode with the gripper motor set to an angle. This makes it possible + # to squeeze the gripper and have it spring back to an open position on its own. + for name in self.leader_arms: + self.leader_arms[name].write("Torque_Enable", 1, "gripper") + self.leader_arms[name].write("Goal_Position", self.config.gripper_open_degree, "gripper") # Connect the cameras for name in self.cameras: diff --git a/lerobot/configs/robot/koch.yaml b/lerobot/configs/robot/koch.yaml index 224040ab2..084b36242 100644 --- a/lerobot/configs/robot/koch.yaml +++ b/lerobot/configs/robot/koch.yaml @@ -37,3 +37,6 @@ cameras: fps: 30 width: 640 height: 480 +# Sets the leader arm in torque mode with the gripper motor set to this angle. This makes it possible +# to squeeze the gripper and have it spring back to an open position on its own. +gripper_open_degree: 35.156