Skip to content

Commit 44e220a

Browse files
committed
FIx leader gripper bug
1 parent 3bdf118 commit 44e220a

File tree

1 file changed

+14
-1
lines changed

1 file changed

+14
-1
lines changed

src/lerobot/teleoperators/bi_yam_leader/bi_yam_leader.py

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -183,17 +183,25 @@ def setup_motors(self) -> None:
183183
def get_action(self) -> dict[str, float]:
184184
"""
185185
Get action from both leader arms by reading their current joint positions.
186+
187+
For teaching handles (no physical gripper), we add a default gripper position
188+
to match the follower arm's expected 7 DOFs (6 joints + 1 gripper).
186189
187190
Returns:
188-
Dictionary with joint positions for both arms
191+
Dictionary with joint positions for both arms (including gripper)
189192
"""
190193
action_dict = {}
191194

192195
# Get left arm observations
193196
left_obs = self.left_arm.get_observations()
194197
left_joint_pos = left_obs["joint_pos"]
198+
199+
# If no gripper (teaching handle), add default gripper position (fully open = 1.0)
195200
if "gripper_pos" in left_obs:
196201
left_joint_pos = np.concatenate([left_joint_pos, left_obs["gripper_pos"]])
202+
else:
203+
# Teaching handle: add default gripper value (1.0 = fully open)
204+
left_joint_pos = np.concatenate([left_joint_pos, [1.0]])
197205

198206
# Add with "left_" prefix
199207
for i, pos in enumerate(left_joint_pos):
@@ -202,8 +210,13 @@ def get_action(self) -> dict[str, float]:
202210
# Get right arm observations
203211
right_obs = self.right_arm.get_observations()
204212
right_joint_pos = right_obs["joint_pos"]
213+
214+
# If no gripper (teaching handle), add default gripper position (fully open = 1.0)
205215
if "gripper_pos" in right_obs:
206216
right_joint_pos = np.concatenate([right_joint_pos, right_obs["gripper_pos"]])
217+
else:
218+
# Teaching handle: add default gripper value (1.0 = fully open)
219+
right_joint_pos = np.concatenate([right_joint_pos, [1.0]])
207220

208221
# Add with "right_" prefix
209222
for i, pos in enumerate(right_joint_pos):

0 commit comments

Comments
 (0)