Sawyer arm - tip location problematic

[bibbygoodwin]

Hi,

I have opened this same issue in RLBench (stepjam/RLBench#94). Feel free to close whichever is in the less appropriate place!

TL;DR: Sawyer 'tip' in middle of wrist, causes problems.

I first posted this problem on the RLBench discord channel a few months ago and was advised to make an RLBench issue. After a little more investigation, I think the root of the problem lies in PyRep, so am opening it here.

In short: I get unexpected behaviour using the Sawyer robot in RLBench, but I think it stems from the PyRep model. Below are gifs of a corner reaching task, executed on the Panda and Sawyer. Though the waypoint target's Euler is (0,0,0), and though this is clearly reflected in the Panda solution, the Sawyer arm performs a strange rotation.

I believe the explanation is that this is for collision avoidance (and it's possible that I'm not using the Euler restriction correctly, hence why a solution can be found at all). The reason collision avoidance is needed, I believe, is that the 'tip' of the Sawyer robot in PyRep is not defined, as it is for the Panda, as the space between the gripper fingers, but instead is a point in the middle of the wrist. The next 3 images show, respectively: 1st: the Panda 'tip' 2nd: the Sawyer tip seen from a distance (though selected in CoppeliaSim, you can't see anything) 3rd: a zoom-in on the Sawyer tip from the same viewpoint - it is found within the wrist, and appears to have an orientation 90º to that of the Panda.

In terms of reproducibility, the gif is from a custom task, but you can see that an error occurs when using the sawyer, but not the panda, in a simple setting with the following (apologies for RLBench dependency):

from rlbench.environment import Environment
from rlbench.action_modes import ActionMode, ArmActionMode
from rlbench.tasks import ReachTarget
import numpy as np
action_mode = ActionMode(ArmActionMode.ABS_JOINT_VELOCITY)
env = Environment(action_mode, robot_configuration='sawyer')
env.launch()
task = env.get_task(ReachTarget)
demo = task.get_demos(1, live_demos=True)`

Running with env = Environment(action_mode) in place of env = Environment(action_mode, robot_configuration='sawyer') runs without incident and collects a Panda arm demo. With the Sawyer, I get the following:

Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/home/me/Code/RLBench/rlbench/task_environment.py", line 352, in get_demos
    amount, callable_each_step, max_attempts)
  File "/home/me/Code/RLBench/rlbench/task_environment.py", line 365, in _get_live_demos
    self.reset()
  File "/home/me/Code/RLBench/rlbench/task_environment.py", line 87, in reset
    % self._task.get_name()) from e
rlbench.task_environment.TaskEnvironmentError: Could not place the task reach_target in the scene. This should not happen, please raise an issues on this task.

[telejesus2]

If I am not mistaken only the Panda model was designed to collect demonstrations. For the other models the 'tip' is not placed in the right place to collect demos, so you should change the model yourself. I did it for my robot (UR3 + Baxter Gripper) and it works fine. Just be sure to match the Panda's tip orientation, because it has the same orientation as the objects in the demos (at least the blocks in the stack_blocks task I tested). If you don't your arm will twist weirdly to grab the blocks.

[stepjam]

Closed in favour of RLBench issue https://github.com/stepjam/RLBench/issues/94