Added minimal support for Robotiq85 gripper.

pyrep/robots/end_effectors/robotiq85_gripper.py

@@ -0,0 +1,58 @@
+from typing import List
+
+from pyrep.objects import Joint
+from pyrep.robots.end_effectors.gripper import Gripper, POSITION_ERROR
+import numpy as np
+
+POSITION_ERROR = 0.00001
+
+
+class Robotiq85Gripper(Gripper):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'ROBOTIQ_85', ['ROBOTIQ_85_active1',
+                                               'ROBOTIQ_85_active2'])
+        self._open_amount_query_joints = [
+            Joint(jname) for jname in [
+                'ROBOTIQ_85_Rjoint1', 'ROBOTIQ_85_Ljoint1']]
+
+    def actuate(self, amount: float, velocity: float) -> bool:
+        if amount != 0.0 and amount != 1.0:
+            raise ValueError(
+                'This gripper currently only supports fully open or closed.')
+
+        current_positions = self.get_joint_positions()
+        vel = velocity if amount == 1.0 else -velocity
+        done = True
+        for i, (j, cur, prev) in enumerate(zip(
+                self.joints, current_positions,
+                self._prev_positions)):
+            # Check if the joint has moved much
+            not_moving = (prev is not None and
+                          np.fabs(cur - prev) < POSITION_ERROR)
+            if not_moving:
+                j.set_joint_target_velocity(0)
+                continue
+            done = False
+            self._prev_vels[i] = vel  # type: ignore
+            j.set_joint_target_velocity(vel)
+        self._prev_positions = current_positions  # type: ignore
+        if done:
+            self._prev_positions = [None] * self._num_joints
+            self._prev_vels = [None] * self._num_joints
+            self.set_joint_target_velocities([0.0] * self._num_joints)
+        return done
+
+    def get_open_amount(self) -> List[float]:
+        """Gets the gripper open state. 1 means open, whilst 0 means closed.
+
+        :return: A list of floats between 0 and 1 representing the gripper open
+            state for each joint. 1 means open, whilst 0 means closed.
+        """
+        joint_intervals = np.array([j.get_joint_interval()[1]
+                     for j in self._open_amount_query_joints])
+        joint_range = joint_intervals[:, 1] - joint_intervals[:, 0]
+        # Flip open amount
+        return list(1. - np.clip((np.array(
+            [j.get_joint_position() for j in self._open_amount_query_joints]
+        ) - joint_intervals[:, 0]) / joint_range, 0.0, 1.0))

tests/test_grippers.py

@@ -1,4 +1,6 @@
 import unittest
+
+from pyrep.robots.end_effectors.robotiq85_gripper import Robotiq85Gripper
 from tests.core import TestCore
 from pyrep import PyRep
 from os import path
@@ -15,6 +17,7 @@
     ('BaxterGripper', BaxterGripper, 0.04),
     ('MicoGripper', MicoGripper, 0.2),
     ('JacoGripper', JacoGripper, 0.2),
+    ('Robotiq85Gripper', Robotiq85Gripper, 0.04),
 ]
 
 
@@ -48,14 +51,15 @@ def test_close_open_gripper(self):
                         self.fail('Took too many steps to close')
                 done = False
                 i = 0
+                open_amount = 1.0 if gripper_name == 'Robotiq85Gripper' else 0.8
                 while not done:
-                    done = gripper.actuate(0.8, velocity=vel)
+                    done = gripper.actuate(open_amount, velocity=vel)
                     self.pyrep.step()
                     i += 1
                     if i > 1000:
                         self.fail('Took too many steps to open')
                 self.assertAlmostEqual(
-                    gripper.get_open_amount()[0], 0.8, delta=0.05)
+                    gripper.get_open_amount()[0], open_amount, delta=0.05)
 
     def test_get_duplicate_gripper(self):
         g = BaxterGripper(1)