Clean collision verbose, set_robot_pose, change ur5 gripper clearance (…

…#39) Resolves #40. * merge renderer.py: add texture to objects, ignore base * merge Change kuka_kmr default position * Change verbose for the collision class is_config_valid function * Clean up unused attributes from demo * Add new robot base pose * Upload new poses * Mistake in changes (old version) * Update benchmark saving method * Update benchmark saving method * Enlarge tunnel walls * New task data files * Change shelf_1.pkl * Resampled two of the panda shelf0 poses * removing old configs * adding new configs * Update test_collisions.py * fixing unit test --------- Co-authored-by: David Kovář <[email protected]> Co-authored-by: dyymon <[email protected]>
agimus-project · Sep 13, 2023 · b8f1c0b · b8f1c0b
1 parent 7ff6495
commit b8f1c0b
Show file tree

Hide file tree

Showing 20 changed files with 75 additions and 79 deletions.
diff --git a/guided_tamp_benchmark/benchmark/benchmark.py b/guided_tamp_benchmark/benchmark/benchmark.py
@@ -25,16 +25,20 @@ class Benchmark:
     The 'res' variable is instance of class BenchmarkResult:
     """
 
-    def __init__(self):
-        self.results = defaultdict(
-            lambda: defaultdict(
+    def __init__(self, results_path: str | pathlib.Path):
+        self.results_path = results_path
+        if pathlib.Path(results_path).exists():
+            self.load_benchmark()
+        else:
+            self.results = defaultdict(
                 lambda: defaultdict(
                     lambda: defaultdict(
-                        lambda: defaultdict(lambda: defaultdict(BenchmarkResult))
+                        lambda: defaultdict(
+                            lambda: defaultdict(lambda: defaultdict(BenchmarkResult))
+                        )
                     )
                 )
             )
-        )
 
     def do_benchmark(
         self,
@@ -88,11 +92,13 @@ def do_benchmark(
             benchmark_result.subsampled_path = path_as_config
             benchmark_result.number_of_grasps = task.compute_n_grasps(path_as_config)
 
-    def save_benchmark(self, results_path: str | pathlib.Path):
+    def save_benchmark(self):
         """saves the benchmarking results to the given file"""
-        dill.dump(self.results, open(results_path, "wb"))
+        with open(self.results_path, "wb") as f:
+            dill.dump(self.results, f)
 
-    def load_benchmark(self, results_path: str | pathlib.Path):
+    def load_benchmark(self):
         """Load the benchmarking results from a given file. The results are stored
         internally."""
-        self.results = dill.load(open(results_path, "rb"))
+        with open(self.results_path, "rb") as f:
+            self.results = dill.load(f)
diff --git a/guided_tamp_benchmark/models/data/robots/ur5/ur5.srdf b/guided_tamp_benchmark/models/data/robots/ur5/ur5.srdf
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot name="ur5">
     <gripper name="gripper" clearance="0.08">
-        <position>0 0 0.12 0.5 -0.5 -0.5 -0.5</position>
+        <position>0 0 0.125 0.5 -0.5 -0.5 -0.5</position>
         <link name="panda_hand"/>
     </gripper>
 

diff --git a/guided_tamp_benchmark/models/furniture/tunnel.py b/guided_tamp_benchmark/models/furniture/tunnel.py
@@ -180,18 +180,18 @@ def urdf(
         <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
     </inertial>
     <visual>
-        <origin xyz="{0} {lengths[1] / 2 + 2.5} {lengths[2] / 2}" rpy="0 0 0" />
+        <origin xyz="{0} {lengths[1] / 2 + 5} {lengths[2] / 2}" rpy="0 0 0" />
         <geometry>
-            <box size="{collision_thickness} 5 5"/>
+            <box size="{collision_thickness} 10 10"/>
         </geometry>
         <material name="red">
         <color rgba="1 0 0 {walls_alpha}"/>
         </material>
     </visual>
     <collision>
-        <origin xyz="{0} {lengths[1] / 2 + 2.5} {lengths[2] / 2}" rpy="0 0 0" />
+        <origin xyz="{0} {lengths[1] / 2 + 5} {lengths[2] / 2}" rpy="0 0 0" />
         <geometry>
-            <box size="{collision_thickness} 5 5"/>
+            <box size="{collision_thickness} 10 10"/>
         </geometry>
     </collision>
 </link>
@@ -202,18 +202,18 @@ def urdf(
         <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
     </inertial>
     <visual>
-        <origin xyz="{0} {-lengths[1] / 2 - 2.5} {lengths[2] / 2}" rpy="0 0 0" />
+        <origin xyz="{0} {-lengths[1] / 2 - 5} {lengths[2] / 2}" rpy="0 0 0" />
         <geometry>
-            <box size="{collision_thickness} 5 5"/>
+            <box size="{collision_thickness} 10 10"/>
         </geometry>
         <material name="red">
         <color rgba="1 0 0 {walls_alpha}"/>
         </material>
     </visual>
     <collision>
-        <origin xyz="{0} {-lengths[1] / 2 - 2.5} {lengths[2] / 2}" rpy="0 0 0" />
+        <origin xyz="{0} {-lengths[1] / 2 - 5} {lengths[2] / 2}" rpy="0 0 0" />
         <geometry>
-            <box size="{collision_thickness} 5 5"/>
+            <box size="{collision_thickness} 10 10"/>
         </geometry>
     </collision>
 </link>
@@ -224,18 +224,18 @@ def urdf(
         <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
     </inertial>
     <visual>
-        <origin xyz="{0} {0} {lengths[2] / 2 + (2.5 - lengths[2]) / 2 + lengths[2] / 2}" rpy="0 0 0"/>
+        <origin xyz="{0} {0} {lengths[2] / 2 + (5 - lengths[2]) / 2 + lengths[2] / 2}" rpy="0 0 0"/>
         <geometry>
-            <box size="{collision_thickness} {lengths[1]} {2.5 - lengths[2]}"/>
+            <box size="{collision_thickness} {lengths[1]} {5 - lengths[2]}"/>
         </geometry>
         <material name="red">
         <color rgba="1 0 0 {walls_alpha}"/>
         </material>
     </visual>
     <collision>
-        <origin xyz="{0} {0} {lengths[2] / 2 + (2.5 - lengths[2]) / 2 + lengths[2] / 2}" rpy="0 0 0"/>
+        <origin xyz="{0} {0} {lengths[2] / 2 + (5 - lengths[2]) / 2 + lengths[2] / 2}" rpy="0 0 0"/>
         <geometry>
-            <box size="{collision_thickness} {lengths[1]} {2.5 - lengths[2]}"/>
+            <box size="{collision_thickness} {lengths[1]} {5 - lengths[2]}"/>
         </geometry>
     </collision>
 </link>

diff --git a/guided_tamp_benchmark/tasks/collisions.py b/guided_tamp_benchmark/tasks/collisions.py
@@ -272,9 +272,10 @@ class Collision:
     """The collision class consists of Pinocchio urdf and collision models and
     functions for collision checking and Pinocchio model rendering."""
 
-    def __init__(self, task):
+    def __init__(self, task, verbose=False):
         """Initilizie with task eg. ShelfTask..."""
         self.task = task
+        self.verbose = verbose
         self.pin_mod, self.col_mod = _create_model(
             remove_tunnel_collisions=task.task_name == "tunnel",
             **_extract_from_task(task),
@@ -295,14 +296,8 @@ def is_config_valid(self, configuration: Configuration) -> bool:
             for k in range(len(self.col_mod.collisionPairs)):
                 cr = geom_data.collisionResults[k]
                 cp = self.col_mod.collisionPairs[k]
-                print(
-                    "collision pair:",
-                    cp.first,
-                    ",",
-                    cp.second,
-                    "- collision:",
-                    "Yes" if cr.isCollision() else "No",
-                )
+                if self.verbose and cr.isCollision():
+                    print(f"Collision at pair {cp.first}-{cp.second}")
             return False
         else:
             return True

diff --git a/guided_tamp_benchmark/tasks/data/shelf_0.pkl b/guided_tamp_benchmark/tasks/data/shelf_0.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_0_panda_poses.pkl b/guided_tamp_benchmark/tasks/data/shelf_0_panda_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_0_ur5_poses.pkl b/guided_tamp_benchmark/tasks/data/shelf_0_ur5_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_1.pkl b/guided_tamp_benchmark/tasks/data/shelf_1.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_1_panda_poses.pkl b/guided_tamp_benchmark/tasks/data/shelf_1_panda_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_1_ur5_poses.pkl b/guided_tamp_benchmark/tasks/data/shelf_1_ur5_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_2.pkl b/guided_tamp_benchmark/tasks/data/shelf_2.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_2_panda_poses.pkl b/guided_tamp_benchmark/tasks/data/shelf_2_panda_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/shelf_2_ur5_poses.pkl b/guided_tamp_benchmark/tasks/data/shelf_2_ur5_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/tunnel_0.pkl b/guided_tamp_benchmark/tasks/data/tunnel_0.pkl
diff --git a/guided_tamp_benchmark/tasks/data/tunnel_0_panda_poses.pkl b/guided_tamp_benchmark/tasks/data/tunnel_0_panda_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/tunnel_0_ur5_poses.pkl b/guided_tamp_benchmark/tasks/data/tunnel_0_ur5_poses.pkl
diff --git a/guided_tamp_benchmark/tasks/data/waiter_0.pkl b/guided_tamp_benchmark/tasks/data/waiter_0.pkl
diff --git a/tests/test_benchmark.py b/tests/test_benchmark.py
@@ -13,21 +13,21 @@
 class TestBenchmarkScript(unittest.TestCase):
     def test_saving(self):
         path = Path("/tmp/results.pkl")
-        b = Benchmark()
-        b.save_benchmark(path)
+        b = Benchmark(path)
+        b.save_benchmark()
         self.assertTrue(path.exists())
 
     def test_save_load(self):
         path = Path("/tmp/results1.pkl")
-        b = Benchmark()
+        b = Benchmark(path)
         b.results["planner"]["shelf1"][0]["panda"][0][0] = BenchmarkResult(
             True, 10.0, (1, 2, 3.0), list(), 10
         )
-        b.save_benchmark(path)
+        b.save_benchmark()
         self.assertTrue(path.exists())
         del b
-        b = Benchmark()
-        b.load_benchmark(path)
+        b = Benchmark(path)
+        b.load_benchmark()
         self.assertTrue("planner" in b.results)
         self.assertTrue("shelf1" in b.results["planner"])
         res: BenchmarkResult = b.results["planner"]["shelf1"][0]["panda"][0][0]

diff --git a/tests/test_collisions.py b/tests/test_collisions.py
@@ -13,7 +13,7 @@
 
 
 def collision_check_cycle(
-        task: BaseTask, configs: List[Configuration]
+    task: BaseTask, configs: List[Configuration]
 ) -> Tuple[bool, int]:
     for i, c in enumerate(configs):
         if not task.collision.is_config_valid(c):
@@ -23,7 +23,7 @@ def collision_check_cycle(
 
 
 def grasp_check_cycle(
-        task: BaseTask, configs: List[Configuration], graps: list
+    task: BaseTask, configs: List[Configuration], graps: list
 ) -> bool:
     for i, c in enumerate(configs):
         if task._check_grasp_constraint(c, delta=0.0001) == graps[i]:
@@ -33,7 +33,7 @@ def grasp_check_cycle(
 
 
 def placement_check_cycle(
-        task: BaseTask, configs: List[Configuration], placement: list
+    task: BaseTask, configs: List[Configuration], placement: list
 ) -> bool:
     for i, c in enumerate(configs):
         if not task._check_place_constraint(c) == placement[i]:
@@ -54,78 +54,73 @@ def test_collision_checking(self):
                 WaiterTask(0, KukaMobileIIWARobot(), 1),
                 data["waiter_0_kmr_0"]["configs"],
             ),
-            (True, 64),
+            (False, -1),
         )
         self.assertEqual(
             collision_check_cycle(
-                TunnelTask(0, UR5Robot(), 1), data["tunnel_0_ur_1"]["configs"]
+                TunnelTask(0, PandaRobot(), 0), data["tunnel_0_panda_0"]["configs"]
             ),
-            (True, 72),
+            (False, -1),
         )
         self.assertEqual(
             collision_check_cycle(
-                ShelfTask(1, PandaRobot(), 1), data["shelf_1_panda_1"]["configs"]
+                ShelfTask(1, UR5Robot(), 3), data["shelf_1_ur_3"]["configs"]
             ),
-            (True, 0),
+            (False, -1),
         )
 
     def test_grasp_checking(self):
         """checks whether the grasp checking gives out the same results as previously"""
         path = pathlib.Path(__file__).parent.joinpath("test_configs.pkl")
         data = pickle.load(open(path, "rb"))
         task = WaiterTask(0, KukaMobileIIWARobot(), 1)
-        for i, c in enumerate(data["waiter_0_kmr_0"]["configs"]):
-            data["waiter_0_kmr_0"]["grasps"][i] = task._check_place_constraint(c)
-
-        task = TunnelTask(0, UR5Robot(), 1)
-        for i, c in enumerate(data["tunnel_0_ur_1"]["configs"]):
-            data["tunnel_0_ur_1"]["grasps"][i] = task._check_place_constraint(c)
-
-        task = ShelfTask(1, PandaRobot(), 1)
-        for i, c in enumerate(data["shelf_1_panda_1"]["configs"]):
-            data["shelf_1_panda_1"]["grasps"][i] = task._check_place_constraint(c)
-
 
         self.assertEqual(
             grasp_check_cycle(
                 WaiterTask(0, KukaMobileIIWARobot(), 1),
                 data["waiter_0_kmr_0"]["configs"],
                 data["waiter_0_kmr_0"]["grasps"],
             ),
-            False,
+            True,
         )
         self.assertEqual(
             grasp_check_cycle(
-                TunnelTask(0, UR5Robot(), 1),
-                data["tunnel_0_ur_1"]["configs"],
-                data["tunnel_0_ur_1"]["grasps"],
+                TunnelTask(0, PandaRobot(), 0),
+                data["tunnel_0_panda_0"]["configs"],
+                data["tunnel_0_panda_0"]["grasps"],
             ),
             True,
         )
         self.assertEqual(
             grasp_check_cycle(
-                ShelfTask(1, PandaRobot(), 1),
-                data["shelf_1_panda_1"]["configs"],
-                data["shelf_1_panda_1"]["grasps"],
+                ShelfTask(1, UR5Robot(), 3),
+                data["shelf_1_ur_3"]["configs"],
+                data["shelf_1_ur_3"]["grasps"],
             ),
-            False,
+            True,
         )
 
     def test_grasp_counting(self):
         """checks whether the grasp counting gives out the same results as previously"""
         path = pathlib.Path(__file__).parent.joinpath("test_configs.pkl")
         data = pickle.load(open(path, "rb"))
         self.assertEqual(
-            ShelfTask(1, PandaRobot(), 1).compute_n_grasps(
-                data["shelf_1_panda_1"]["configs"]
+            WaiterTask(0, KukaMobileIIWARobot(), 0).compute_n_grasps(
+                data["waiter_0_kmr_0"]["configs"]
             ),
-            2,
+            data["waiter_0_kmr_0"]["n_grasps"],
         )
         self.assertEqual(
-            TunnelTask(0, UR5Robot(), 1).compute_n_grasps(
-                data["tunnel_0_ur_1"]["configs"]
+            ShelfTask(1, UR5Robot(), 3).compute_n_grasps(
+                data["shelf_1_ur_3"]["configs"]
             ),
-            3,
+            data["shelf_1_ur_3"]["n_grasps"],
+        )
+        self.assertEqual(
+            TunnelTask(0, PandaRobot(), 0).compute_n_grasps(
+                data["tunnel_0_panda_0"]["configs"]
+            ),
+            data["tunnel_0_panda_0"]["n_grasps"],
         )
 
     def test_placement_checking(self):
@@ -140,23 +135,23 @@ def test_placement_checking(self):
                 data["waiter_0_kmr_0"]["configs"],
                 data["waiter_0_kmr_0"]["placements"],
             ),
-            False,
+            True,
         )
         self.assertEqual(
             placement_check_cycle(
-                TunnelTask(0, UR5Robot(), 1),
-                data["tunnel_0_ur_1"]["configs"],
-                data["tunnel_0_ur_1"]["placements"],
+                TunnelTask(0, PandaRobot(), 0),
+                data["tunnel_0_panda_0"]["configs"],
+                data["tunnel_0_panda_0"]["placements"],
             ),
-            False,
+            True,
         )
         self.assertEqual(
             placement_check_cycle(
-                ShelfTask(1, PandaRobot(), 1),
-                data["shelf_1_panda_1"]["configs"],
-                data["shelf_1_panda_1"]["placements"],
+                ShelfTask(1, UR5Robot(), 3),
+                data["shelf_1_ur_3"]["configs"],
+                data["shelf_1_ur_3"]["placements"],
             ),
-            False,
+            True,
         )
 
 

diff --git a/tests/test_configs.pkl b/tests/test_configs.pkl