Added group shape function and get visual objects of robot components.

Author: stepjam

pyrep/backend/vrep.py

@@ -1013,3 +1013,12 @@ def simGetExplicitHandling(generalObjectHandle):
     flag = lib.simGetExplicitHandling(generalObjectHandle)
     _check_return(flag)
     return flag
+
+
+def simUngroupShape(shapeHandle):
+    count = ffi.new('int*')
+    shapes = lib.simUngroupShape(shapeHandle, count)
+    _check_null_return(shapes)
+    handles = [shapes[i] for i in range(count[0])]
+    # simReleaseBuffer(shapes)
+    return handles

pyrep/objects/cartesian_path.py

@@ -58,7 +58,7 @@ def create(show_line: bool = True, show_orientation: bool = True,
         handle = vrep.simCreatePath(attributes)
         return CartesianPath(handle)
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         return ObjectType.PATH
 
     def get_pose_on_path(self, relative_distance: float

pyrep/objects/dummy.py

@@ -19,5 +19,5 @@ def create(size=0.01) -> 'Dummy':
         handle = vrep.simCreateDummy(size, None)
         return Dummy(handle)
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         return ObjectType.DUMMY

pyrep/objects/force_sensor.py

@@ -8,7 +8,7 @@ class ForceSensor(Object):
     """An object able to measure forces and torques that are applied to it.
     """
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         return ObjectType.FORCE_SENSOR
 
     def read(self) -> Tuple[List[float], List[float]]:

pyrep/objects/joint.py

@@ -15,7 +15,7 @@ class Joint(Object):
     def __init__(self, name_or_handle: Union[str, int]):
         super().__init__(name_or_handle)
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         return ObjectType.JOINT
 
     def get_joint_type(self) -> JointType:

pyrep/objects/object.py

@@ -17,7 +17,7 @@ def __init__(self, name_or_handle: Union[str, int]):
             self._handle = name_or_handle
         else:
             self._handle = vrep.simGetObjectHandle(name_or_handle)
-            assert_type = self.get_type()
+            assert_type = self._get_requested_type()
             actual = ObjectType(vrep.simGetObjectType(self._handle))
             if actual != assert_type:
                 raise WrongObjectTypeError(
@@ -50,12 +50,19 @@ def get_object_type(name: str) -> ObjectType:
         """
         return ObjectType(vrep.simGetObjectType(vrep.simGetObjectHandle(name)))
 
+    def _get_requested_type(self) -> ObjectType:
+        """Used for internally checking assumptions user made about object type.
+
+        :return: Type of the object.
+        """
+        raise NotImplementedError('Must be overridden.')
+
     def get_type(self) -> ObjectType:
         """Gets the type of the object.
 
         :return: Type of the object.
         """
-        raise NotImplementedError('Must be overridden.')
+        return ObjectType(vrep.simGetObjectType(self._handle))
 
     def get_handle(self) -> int:
         """Gets the internal handle of this object.

pyrep/objects/proximity_sensor.py

@@ -10,7 +10,7 @@ class ProximitySensor(Object):
     sensors.
     """
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         return ObjectType.PROXIMITY_SENSOR
 
     def is_detected(self, obj: Object) -> bool:

pyrep/objects/shape.py

@@ -120,7 +120,7 @@ def create_mesh(vertices: List[float], indices: List[int],
             options, shading_angle, vertices, indices)
         return Shape(handle)
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         return ObjectType.SHAPE
 
     def is_respondable(self) -> bool:
@@ -348,3 +348,11 @@ def set_texture(self, texture: Texture, mapping_mode: TextureMappingMode,
         vrep.simSetShapeTexture(
             self.get_handle(), texture.get_texture_id(), mapping_mode.value,
             options, list(uv_scaling), position, orientation)
+
+    def ungroup(self) -> List['Shape']:
+        """Ungroups a compound shape into several simple shapes.
+
+        :return: A list of shapes.
+        """
+        handles = vrep.simUngroupShape(self.get_handle())
+        return [Shape(handle) for handle in handles]

pyrep/objects/vision_sensor.py

@@ -104,7 +104,7 @@ def create(resolution: List[int], explicit_handling=False,
             vs.set_orientation(orientation)
         return vs
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         return ObjectType.VISION_SENSOR
 
     def capture_rgb(self) -> np.ndarray:

pyrep/robots/robot_component.py

@@ -1,4 +1,7 @@
 from typing import List, Tuple
+
+from pyrep.objects.shape import Shape
+
 from pyrep.backend import vrep
 from pyrep.const import JointType
 from pyrep.objects.object import Object
@@ -40,7 +43,7 @@ def copy(self) -> 'RobotComponent':
             num = 0
         return self.__class__(num)
 
-    def get_type(self) -> ObjectType:
+    def _get_requested_type(self) -> ObjectType:
         """Gets the type of the object.
 
         :return: Type of the object.
@@ -236,6 +239,17 @@ def set_joint_mode(self, value: JointMode) -> None:
         """
         [j.set_joint_mode(value) for j in self.joints]
 
+    def get_visuals(self) -> List[Object]:
+        """Gets a list of the visual elements of this component.
+
+        Can be useful for methods such as domain randomization.
+        Should ideally be overridden for each robot.
+
+        :return: A list of visual shapes.
+        """
+        tree = self.get_objects_in_tree(ObjectType.SHAPE, exclude_base=False)
+        return [obj for obj in tree if 'visual' in obj.get_name()]
+
     def _assert_len(self, inputs: list) -> None:
         if len(self.joints) != len(inputs):
             raise RuntimeError(

tests/test_joint_groups.py

@@ -53,6 +53,9 @@ def test_get_joint_upper_velocity_limits(self):
         self.assertEqual(
             len(self.robot.get_joint_upper_velocity_limits()), self.num_joints)
 
+    def test_get_visuals(self):
+        self.assertEqual(len(self.robot.get_visuals()), 10)
+
 
 if __name__ == '__main__':
     unittest.main()

tests/test_objects.py

@@ -1,4 +1,7 @@
 import unittest
+
+from pyrep.const import ObjectType
+
 from tests.core import TestCore
 from pyrep.objects.shape import Shape
 from pyrep.objects.dummy import Dummy
@@ -23,6 +26,9 @@ def test_equality(self):
     def test_get_handle(self):
         self.assertGreater(self.dynamic_cube.get_handle(), 0)
 
+    def test_get_type(self):
+        self.assertEqual(self.dynamic_cube.get_type(), ObjectType.SHAPE)
+
     def test_still_exists(self):
         self.assertTrue(self.dynamic_cube.still_exists())
         self.assertFalse(Shape(-1).still_exists())