diff --git a/rlbench/action_modes/arm_action_modes.py b/rlbench/action_modes/arm_action_modes.py index 2336aee4f..c8fcb04f9 100644 --- a/rlbench/action_modes/arm_action_modes.py +++ b/rlbench/action_modes/arm_action_modes.py @@ -328,11 +328,15 @@ def action(self, scene: Scene, action: np.ndarray): 'Could not perform IK via Jacobian; most likely due to current ' 'end-effector pose being too far from the given target pose. ' 'Try limiting/bounding your action space.') from e + done = False prev_values = None + max_steps = 50 + steps = 0 + # Move until reached target joint positions or until we stop moving # (e.g. when we collide wth something) - while not done: + while not done and steps < max_steps: scene.step() cur_positions = scene.robot.arm.get_joint_positions() reached = np.allclose(cur_positions, joint_positions, atol=0.01) @@ -342,10 +346,12 @@ def action(self, scene: Scene, action: np.ndarray): cur_positions, prev_values, atol=0.001) prev_values = cur_positions done = reached or not_moving + steps += 1 def action_shape(self, scene: Scene) -> tuple: return 7, + class ERJointViaIK(ArmActionMode): """High-level action where target EE pose + Elbow angle is given in ER space (End-Effector and Elbow) and reached via IK. @@ -503,4 +509,4 @@ def action(self, scene: Scene, action: np.ndarray): steps += 1 def action_shape(self, _: Scene) -> tuple: - return self._action_shape \ No newline at end of file + return self._action_shape