Merge pull request #45 from empriselab/dev

Dev to main sync

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/Test/test_cloth_init.py

@@ -0,0 +1,35 @@
+from pyrcareworld.envs.rcareworld_env import RCareWorld
+import random
+
+# Script to test cloth initial positions.
+if __name__ == "__main__":
+    env = RCareWorld()
+
+    # Create a new cloth representation.
+    cloth = env.create_cloth(id=100, name="Cloth", is_in_scene=True)
+
+    # Every 200th step, sets the cloth particles to a random position in a cube around some random point in the scene.
+    step = 0
+    for _ in range(10000):
+        env.step()
+        step += 1
+
+        if step % 200 == 0:
+            # Make a random 3 vector in the range of [0, 1].
+            random_pos = [random.uniform(0, 1) for _ in range(3)]
+
+            positions = []
+
+            # There happens to be 705 particles in this scene.
+            for _ in range(705):
+                random_relative_pos = [random.uniform(-0.2, 0.2) for _ in range(3)]
+
+                my_pos = [a + b for a, b in zip(random_pos, random_relative_pos)]
+
+                # Add the new position to the list.
+                positions.append(my_pos)
+
+            # Convert the list of positions to a mapping.
+            positions = dict(enumerate(positions))
+
+            cloth.initializeParticlePositions(positions)

pyrcareworld/pyrcareworld/agents/cloth.py

@@ -29,3 +29,17 @@ def removeParticleAnchor(self, particle_group_name: str):
             id=self.id,
             particle_group_name=particle_group_name,
         )
+
+    def initializeParticlePositions(self, mappings: dict):
+        """
+        Sends a message containing a mapping from particle indices to their initial positions. Particles will teleport to this position when this function is called.
+
+        Args:
+            mappings (dict): A mapping from particle indices to their initial positions. Each position is a list of floats with at least 3 elements, corresponding to [x, y, z].
+        """
+        self.env.instance_channel.set_action(
+            "InitializeParticlePositions",
+            id=self.id,
+            particle_indices=mappings.keys(),
+            positions=mappings.values(),
+        )

pyrcareworld/pyrcareworld/attributes/cloth_attr.py

@@ -44,3 +44,30 @@ def RemoveParticleAnchor(kwargs: dict) -> OutgoingMessage:
     msg.write_string(kwargs["particle_group_name"])
 
     return msg
+
+
+def InitializeParticlePositions(kwargs: dict) -> OutgoingMessage:
+    """
+    Sends a message containing a mapping from particle indices to their initial positions. Particles will teleport to this position when this function is called.
+    """
+    compulsory_params = ["id", "particle_indices", "positions"]
+    utility.CheckKwargs(kwargs, compulsory_params)
+
+    msg = OutgoingMessage()
+
+    msg.write_int32(kwargs["id"])
+    msg.write_string("InitializeParticlePositions")
+    # Float list of particle indices. May assume all integers.
+    msg.write_float32_list(kwargs["particle_indices"])
+
+    # Extract X, Y, and Z from positions into separate lists.
+    xs = [p[0] for p in kwargs["positions"]]
+    ys = [p[1] for p in kwargs["positions"]]
+    zs = [p[2] for p in kwargs["positions"]]
+
+    # Send x, then y, then z.
+    msg.write_float32_list(xs)
+    msg.write_float32_list(ys)
+    msg.write_float32_list(zs)
+
+    return msg

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: