Fix examples/tests and Planner

docs/source/tutorials/planning_with_fixed_joints.rst

@@ -19,14 +19,14 @@ The optional `link_names` and `joint_names` parameters used to order the joints
    :language: python
    :start-after: # pickup ankor
    :end-before: # pickup ankor end
-   :emphasize-lines: 14
+   :emphasize-lines: 12
 
 Notice we have abstracted away how to decouple this motion into two stages. Here is the function definition:
 
 .. literalinclude:: ../../../mplib/examples/two_stage_motion.py
    :language: python
    :start-after: # move_in_two_stage ankor
    :end-before: # move_in_two_stage ankor end
-   :emphasize-lines: 18
+   :emphasize-lines: 16
 
-The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9.
+The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9.

mplib/examples/detect_collision.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 
+from mplib import Pose
 from mplib.collision_detection import fcl
 from mplib.examples.demo_setup import DemoSetup
 
@@ -45,13 +46,9 @@ def demo(self):
         # floor ankor
         floor = fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
         # create a collision object for the floor, with a 10cm offset in the z direction
-        floor_fcl_collision_object = fcl.CollisionObject(
-            floor, [0, 0, -0.1], [1, 0, 0, 0]
-        )
+        floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1]))
         # update the planning world with the floor collision object
-        self.planner.planning_world.add_normal_object(
-            "floor", floor_fcl_collision_object
-        )
+        self.planner.planning_world.add_object("floor", floor_fcl_collision_object)
         # floor ankor end
         print("\n----- self-collision-free qpos -----")
         # if the joint qpos does not include the gripper joints,

mplib/examples/two_stage_motion.py

@@ -3,6 +3,7 @@
 import numpy as np
 import sapien.core as sapien
 
+from mplib import Pose
 from mplib.collision_detection import fcl
 from mplib.examples.demo_setup import DemoSetup
 
@@ -159,18 +160,16 @@ def demo(self):
         self.add_point_cloud()
         # also add the target as a collision object so we don't hit it
         fcl_red_cube = fcl.Box([0.04, 0.04, 0.14])
-        collision_object = fcl.CollisionObject(
-            fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]
-        )
-        self.planner.planning_world.add_normal_object("target", collision_object)
+        collision_object = fcl.CollisionObject(fcl_red_cube, Pose(p=[0.7, 0, 0.07]))
+        self.planner.planning_world.add_object("target", collision_object)
 
         # go above the target
         pickup_pose[2] += 0.2
         self.move_in_two_stage(pickup_pose)
         # pickup ankor end
         self.open_gripper()
         # move down to pick
-        self.planner.planning_world.remove_normal_object(
+        self.planner.planning_world.remove_object(
             "target"
         )  # remove the object so we don't report collision
         pickup_pose[2] -= 0.12
@@ -182,7 +181,7 @@ def demo(self):
         self.planner.robot.set_qpos(self.robot.get_qpos(), True)
 
         # add the attached box to the planning world
-        self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
+        self.planner.update_attached_box([0.04, 0.04, 0.12], Pose(p=[0, 0, 0.14]))
 
         pickup_pose[2] += 0.12
         result = self.plan_without_base(pickup_pose, has_attach=True)

mplib/planner.py

@@ -67,9 +67,9 @@ def __init__(
                 srdf := self.urdf.with_name(self.urdf.stem + "_mplib.srdf")
             ).is_file():
                 print(f"No SRDF file provided but found {srdf}")
-                self.srdf = srdf
             else:
-                self.srdf = generate_srdf(self.urdf, new_package_keyword, verbose=True)
+                srdf = generate_srdf(urdf, new_package_keyword, verbose=True)
+        self.srdf = srdf
 
         # replace package:// keyword if exists
         self.urdf = replace_urdf_package_keyword(self.urdf, new_package_keyword)

tests/test_fcl_model.py

@@ -60,15 +60,11 @@ def test_remove_collision_pairs_from_srdf(self):
             self.assertIn(pair, old_collision_pairs)
 
     def test_collision(self):
-        collisions = self.model.collide_full()
+        collisions = self.model.check_self_collision()
         self.assertGreater(len(collisions), 0)
-        # some of them are collisions
-        is_collision = [collision.is_collision() for collision in collisions]
-        self.assertTrue(any(is_collision))
         self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"])
-        collisions = self.model.collide_full()
-        is_collision = [collision.is_collision() for collision in collisions]
-        self.assertFalse(any(is_collision))
+        collisions = self.model.check_self_collision()
+        self.assertEqual(len(collisions), 0)
 
 
 if __name__ == "__main__":

tests/test_planner.py

@@ -131,13 +131,9 @@ def test_self_collision(self):
     def test_env_collision(self):
         floor = fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
         # create a collision object for the floor, with a 10cm offset in the z direction
-        floor_fcl_collision_object = fcl.CollisionObject(
-            floor, Pose([0, 0, -0.1], [1, 0, 0, 0])
-        )
+        floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1]))
         # update the planning world with the floor collision object
-        self.planner.planning_world.add_normal_object(
-            "floor", floor_fcl_collision_object
-        )
+        self.planner.planning_world.add_object("floor", floor_fcl_collision_object)
 
         env_collision_qpos = [
             0,
@@ -151,7 +147,7 @@ def test_env_collision(self):
         self.assertTrue(self.planner.check_for_env_collision(env_collision_qpos))
 
         # remove the floor and check for env-collision returns no collision
-        self.planner.remove_normal_object("floor")
+        self.planner.remove_object("floor")
         self.assertFalse(self.planner.check_for_env_collision(env_collision_qpos))
 
     def test_IK(self):
@@ -173,17 +169,13 @@ def test_IK(self):
         # now put down a floor and check that the robot can't reach the pose
         floor = fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
         # create a collision object for the floor, with a 10cm offset in the z direction
-        floor_fcl_collision_object = fcl.CollisionObject(
-            floor, Pose([0, 0, -0.1], [1, 0, 0, 0])
-        )
+        floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1]))
 
         under_floor_target_pose = deepcopy(self.target_pose)
         under_floor_target_pose.set_p([0.4, 0.3, -0.1])
         status, _ = self.planner.IK(under_floor_target_pose, self.init_qpos)
         self.assertEqual(status, "Success")
-        self.planner.planning_world.add_normal_object(
-            "floor", floor_fcl_collision_object
-        )
+        self.planner.planning_world.add_object("floor", floor_fcl_collision_object)
         status, _ = self.planner.IK(under_floor_target_pose, self.init_qpos)
         self.assertNotEqual(status, "Success")