Added support for suction cups. Added Dobot.

pyrep/objects/joint.py

@@ -36,16 +36,36 @@ def get_joint_position(self) -> float:
         """
         return vrep.simGetJointPosition(self._handle)
 
-    def set_joint_position(self, position: float) -> None:
-        """	Sets the intrinsic position of a joint.
+    def set_joint_position(self, position: float,
+                           allow_force_mode=True) -> None:
+        """Sets the intrinsic position of the joint.
 
-        May have no effect depending on the joint mode. This function cannot
-        be used with spherical joints.
-
-        :param position: Position of the joint (angular or linear value
-            depending on the joint type).
+        :param positions: A list of positions of the joints (angular or linear
+            values depending on the joint type).
+        :param allow_force_mode: If True, then the position can be set even
+            when the joint mode is in Force mode. It will disable dynamics,
+            move the joint, and then re-enable dynamics.
         """
+        if not allow_force_mode:
+            vrep.simSetJointPosition(self._handle, position)
+            return
+
+        is_model = self.is_model()
+        if not is_model:
+            self.set_model(True)
+
+        prior = vrep.simGetModelProperty(self.get_handle())
+        p = prior | vrep.sim_modelproperty_not_dynamic
+        # Disable the dynamics
+        vrep.simSetModelProperty(self._handle, p)
+
         vrep.simSetJointPosition(self._handle, position)
+        self.set_joint_target_position(position)
+        vrep.simExtStep(True)  # Have to step once for changes to take effect
+
+        # Re-enable the dynamics
+        vrep.simSetModelProperty(self._handle, prior)
+        self.set_model(is_model)
 
     def get_joint_target_position(self) -> float:
         """Retrieves the target position of a joint.

pyrep/robots/arms/dobot.py

@@ -0,0 +1,7 @@
+from pyrep.robots.arms.arm import Arm
+
+
+class Dobot(Arm):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Dobot', 4)

pyrep/robots/end_effectors/baxter_suction_cup.py

@@ -0,0 +1,7 @@
+from pyrep.robots.end_effectors.suction_cup import SuctionCup
+
+
+class BaxterSuctionCup(SuctionCup):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'BaxterSuctionCup')

pyrep/robots/end_effectors/dobot_suction_cup.py

@@ -0,0 +1,7 @@
+from pyrep.robots.end_effectors.suction_cup import SuctionCup
+
+
+class DobotSuctionCup(SuctionCup):
+
+    def __init__(self, count: int = 0):
+        super().__init__(count, 'Dobot_suctionCup')

pyrep/robots/end_effectors/gripper.py

@@ -17,8 +17,6 @@ def __init__(self, count: int, name: str, joint_names: List[str]):
         suffix = '' if count == 0 else '#%d' % (count - 1)
         prox_name = '%s_attachProxSensor%s' % (name, suffix)
         attach_name = '%s_attachPoint%s' % (name, suffix)
-        # prox_name = prox_name if count == 0 else prox_name + '%d' % (count - 1)
-        # attach_name = attach_name if count == 0 else attach_name + '%d' % (count - 1)
         self._proximity_sensor = ProximitySensor(prox_name)
         self._attach_point = ForceSensor(attach_name)
         self._old_parents = []

pyrep/robots/end_effectors/suction_cup.py

@@ -0,0 +1,57 @@
+from typing import List
+from pyrep.objects.object import Object
+from pyrep.objects.shape import Shape
+from pyrep.objects.proximity_sensor import ProximitySensor
+from pyrep.objects.force_sensor import ForceSensor
+
+
+class SuctionCup(Shape):
+    """Represents all suction cups.
+    """
+
+    def __init__(self, count: int, name: str, base_name: str = None):
+        suffix = '' if count == 0 else '#%d' % (count - 1)
+        super().__init__(name + suffix if base_name is None else base_name + suffix)
+        self._proximity_sensor = ProximitySensor('%s_sensor%s' % (name, suffix))
+        self._attach_point = ForceSensor('%s_connection%s' % (name, suffix))
+        self._old_parents = []
+        self._grasped_objects = []
+
+    def grasp(self, obj: Object) -> bool:
+        """Attach the object to the suction cup if it is detected.
+
+        Note: The does not move the object up to the suction cup. Therefore, the
+        proximity sensor should have a short range in order for the suction
+        grasp to look realistic.
+
+        :param obj: The object to grasp if detected.
+        :return: True if the object was detected/grasped.
+        """
+        detected = self._proximity_sensor.is_detected(obj)
+        # Check if detected and that we are not already grasping it.
+        if detected and obj not in self._grasped_objects:
+            self._grasped_objects.append(obj)
+            self._old_parents.append(obj.get_parent())
+            obj.set_parent(self._attach_point, keep_in_place=True)
+        return detected
+
+    def release(self) -> None:
+        """Release any objects that have been sucked.
+
+        Note: The does not actuate the gripper, but instead simply detaches any
+        grasped objects.
+        """
+        for grasped_obj, old_parent in zip(
+                self._grasped_objects, self._old_parents):
+            # Check if the object still exists
+            if grasped_obj.still_exists():
+                grasped_obj.set_parent(old_parent, keep_in_place=True)
+        self._grasped_objects = []
+        self._old_parents = []
+
+    def get_grasped_objects(self) -> List[Object]:
+        """Gets the objects that are currently in the suction cup.
+
+        :return: A list of grasped objects.
+        """
+        return self._grasped_objects

pyrep/robots/robot_component.py

@@ -85,7 +85,8 @@ def set_joint_positions(self, positions: List[float],
         self._assert_len(positions)
 
         if not allow_force_mode:
-            [j.set_joint_position(p) for j, p in zip(self.joints, positions)]
+            [j.set_joint_position(p, allow_force_mode)
+             for j, p in zip(self.joints, positions)]
             return
 
         is_model = self.is_model()
@@ -97,7 +98,8 @@ def set_joint_positions(self, positions: List[float],
         # Disable the dynamics
         vrep.simSetModelProperty(self._handle, p)
 
-        [j.set_joint_position(p) for j, p in zip(self.joints, positions)]
+        [j.set_joint_position(p, allow_force_mode)
+         for j, p in zip(self.joints, positions)]
         [j.set_joint_target_position(p) for j, p in zip(self.joints, positions)]
         vrep.simExtStep(True)  # Have to step once for changes to take effect
 

tests/test_arms_and_configuration_paths.py

@@ -17,6 +17,7 @@
 from pyrep.robots.arms.ur3 import UR3
 from pyrep.robots.arms.ur5 import UR5
 from pyrep.robots.arms.ur10 import UR10
+from pyrep.robots.arms.dobot import Dobot
 
 ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), 'assets')
 
@@ -33,6 +34,7 @@
     ('UR3', UR3),
     ('UR5', UR5),
     ('UR10', UR10),
+    ('Dobot', Dobot),
 ]
 
 

tests/test_suction_cups.py

@@ -0,0 +1,51 @@
+import unittest
+from tests.core import TestCore
+from pyrep import PyRep
+from pyrep.objects.shape import Shape
+from os import path
+
+from pyrep.robots.end_effectors.dobot_suction_cup import DobotSuctionCup
+from pyrep.robots.end_effectors.baxter_suction_cup import BaxterSuctionCup
+
+ASSET_DIR = path.join(path.dirname(path.abspath(__file__)), 'assets')
+
+SUCTION_CUPS = [
+    ('DobotSuctionCup', DobotSuctionCup),
+    ('BaxterSuctionCup', BaxterSuctionCup),
+]
+
+
+class TestSuctionCups(TestCore):
+
+    def setUp(self):
+        self.pyrep = PyRep()
+        self.pyrep.launch(path.join(
+            ASSET_DIR, 'test_scene_robots.ttt'), headless=True)
+        self.pyrep.step()
+        self.pyrep.start()
+
+    def test_get_suction_cup(self):
+        for cup_name, cup_type in SUCTION_CUPS:
+            with self.subTest(suction_cup=cup_name):
+                cup = cup_type()
+                self.assertIsInstance(cup, cup_type)
+
+    def test_grasp_and_release_with_suction(self):
+        for cup_name, cup_type in SUCTION_CUPS:
+            with self.subTest(suction_cup=cup_name):
+                suction_cube = Shape('%s_cube' % cup_name)
+                cube = Shape('cube')
+                cup = cup_type()
+                self.pyrep.step()
+                grasped = cup.grasp(cube)
+                self.assertFalse(grasped)
+                self.assertEqual(len(cup.get_grasped_objects()), 0)
+                grasped = cup.grasp(suction_cube)
+                self.assertTrue(grasped)
+                self.assertListEqual(cup.get_grasped_objects(), [suction_cube])
+                cup.release()
+                self.assertEqual(len(cup.get_grasped_objects()), 0)
+
+
+if __name__ == '__main__':
+    unittest.main()