Merge pull request #41 from thisjustin123/justin/cloth-grasp

Example Cloth Grasp. Works for now

pyrcareworld/Test/test_cloth_grasp.py

@@ -0,0 +1,51 @@
+from pyrcareworld.envs.rcareworld_env import RCareWorld
+
+# Script to test cloth grasping. Paired with "Cloth Grasp" scene.
+if __name__ == "__main__":
+    env = RCareWorld()
+
+    # Create a new cloth representation.
+    cloth = env.create_cloth(id=100, name="Cloth", is_in_scene=True)
+
+    # Create a new robot.
+    robot = env.create_robot(
+        id=315893,
+        # Note: "3158930" is the scene gripper id, but it seems to not work.
+        gripper_list=[315893],
+        robot_name="kinova_gen3_7dof-robotiq85",
+        base_pos=[0, 0, 0],
+    )
+
+    # Note: We use a General Gripper Script on the robot to get the GripperClose() and GripperOpen() functions to have visual effect.
+
+    # ...
+    for i in range(200):
+        env.step()
+
+    # Move to pants.
+    for i in range(100):
+        robot.moveTo([0, 0.33, 0.477])
+        env.step()
+
+    # Grasp. Cloth Grasper script on C# causes grasp.
+    robot.GripperClose()
+
+    # Grasp...
+    for i in range(100):
+        env.step()
+
+    # Move up.
+    for i in range(100):
+        robot.moveTo([0, 0.9, 0.3])
+        env.step()
+
+    # ...
+    for i in range(100):
+        env.step()
+
+    # Release.
+    robot.GripperOpen()
+
+    # ...
+    for i in range(200):
+        env.step()

pyrcareworld/pyrcareworld/envs/rcareworld_env.py

@@ -290,7 +290,7 @@ def create_robot(
         urdf_path: str = None,
         base_pos: list = [0, 0, 0],
         base_orn=[-0.707107, -0.707107, -0.707107, 0.707107],
-    ) -> None:
+    ) -> Robot:
         """
         Create a robot in the scene
         :param id: robot id
@@ -321,7 +321,9 @@ def create_robot(
         this_robot = self.robot_dict[id]
         return this_robot
 
-    def create_object(self, id: int, name: str, is_in_scene: bool):
+    def create_object(
+        self, id: int, name: str, is_in_scene: bool
+    ) -> RCareWorldBaseObject:
         """create object
 
         Args:
@@ -367,7 +369,7 @@ def create_cloth(self, id: int, name: str, is_in_scene: bool) -> Cloth:
         this_cloth = self.cloth_dict[id]
         return this_cloth
 
-    def create_skin(self, id: int, name: str, is_in_scene: bool):
+    def create_skin(self, id: int, name: str, is_in_scene: bool) -> Skin:
         """create skin
 
         Args:
@@ -390,7 +392,7 @@ def create_camera(
         height: int = 480,
         fov: float = 60,
         is_in_scene: bool = False,
-    ):
+    ) -> Camera:
         """create camera
 
         Args: