Skip to content

Add step limit to EndEffectorPoseViaIK action to prevent infinite loops #281

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add step limit to EndEffectorPoseViaIK action to prevent infinite loops #281

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions rlbench/action_modes/arm_action_modes.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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.
Expand Down Expand Up @@ -503,4 +509,4 @@ def action(self, scene: Scene, action: np.ndarray):
steps += 1

def action_shape(self, _: Scene) -> tuple:
return self._action_shape
return self._action_shape
Loading