From 02d3cd086e651e3e650c4becbf71126e10c95bd9 Mon Sep 17 00:00:00 2001 From: thisjustin123 Date: Fri, 5 Apr 2024 15:51:12 -0400 Subject: [PATCH 1/4] Add code for initialize cloth position --- pyrcareworld/Test/test_cloth_init.py | 35 +++++++++++++++++++ pyrcareworld/pyrcareworld/agents/cloth.py | 14 ++++++++ .../pyrcareworld/attributes/cloth_attr.py | 27 ++++++++++++++ 3 files changed, 76 insertions(+) create mode 100644 pyrcareworld/Test/test_cloth_init.py diff --git a/pyrcareworld/Test/test_cloth_init.py b/pyrcareworld/Test/test_cloth_init.py new file mode 100644 index 00000000..5f6bcebf --- /dev/null +++ b/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) diff --git a/pyrcareworld/pyrcareworld/agents/cloth.py b/pyrcareworld/pyrcareworld/agents/cloth.py index eac568b0..17503d02 100644 --- a/pyrcareworld/pyrcareworld/agents/cloth.py +++ b/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(), + ) diff --git a/pyrcareworld/pyrcareworld/attributes/cloth_attr.py b/pyrcareworld/pyrcareworld/attributes/cloth_attr.py index 24d6b472..e362877d 100755 --- a/pyrcareworld/pyrcareworld/attributes/cloth_attr.py +++ b/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 From 93bf3bac4e2a03e024d595ff3cd612c4a72780b5 Mon Sep 17 00:00:00 2001 From: thisjustin123 Date: Wed, 10 Apr 2024 23:00:30 -0400 Subject: [PATCH 2/4] Add test cloth grasp environment --- pyrcareworld/Test/test_cloth_grasp.py | 49 +++++++++++++++++++ .../pyrcareworld/envs/rcareworld_env.py | 2 +- 2 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 pyrcareworld/Test/test_cloth_grasp.py diff --git a/pyrcareworld/Test/test_cloth_grasp.py b/pyrcareworld/Test/test_cloth_grasp.py new file mode 100644 index 00000000..0759e4b2 --- /dev/null +++ b/pyrcareworld/Test/test_cloth_grasp.py @@ -0,0 +1,49 @@ +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], + ) + + # ... + 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() diff --git a/pyrcareworld/pyrcareworld/envs/rcareworld_env.py b/pyrcareworld/pyrcareworld/envs/rcareworld_env.py index 21fdebb6..8e6e2664 100644 --- a/pyrcareworld/pyrcareworld/envs/rcareworld_env.py +++ b/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 From 1952ba5f1ee71570c39c3aa6db2995441705fb66 Mon Sep 17 00:00:00 2001 From: thisjustin123 Date: Wed, 10 Apr 2024 23:18:44 -0400 Subject: [PATCH 3/4] Add english note --- pyrcareworld/Test/test_cloth_grasp.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pyrcareworld/Test/test_cloth_grasp.py b/pyrcareworld/Test/test_cloth_grasp.py index 0759e4b2..6b5eb373 100644 --- a/pyrcareworld/Test/test_cloth_grasp.py +++ b/pyrcareworld/Test/test_cloth_grasp.py @@ -16,6 +16,8 @@ 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() From 6b328a6e1c82b9ac9eda731b693d5da8cd20a12e Mon Sep 17 00:00:00 2001 From: thisjustin123 Date: Mon, 15 Apr 2024 18:16:16 -0400 Subject: [PATCH 4/4] Add explicit types in `create_...` functions --- pyrcareworld/pyrcareworld/envs/rcareworld_env.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/pyrcareworld/pyrcareworld/envs/rcareworld_env.py b/pyrcareworld/pyrcareworld/envs/rcareworld_env.py index 8e6e2664..a13020bc 100644 --- a/pyrcareworld/pyrcareworld/envs/rcareworld_env.py +++ b/pyrcareworld/pyrcareworld/envs/rcareworld_env.py @@ -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: