diff --git a/docs/source/tutorials/planning_with_fixed_joints.rst b/docs/source/tutorials/planning_with_fixed_joints.rst index 989691f..183a5ae 100644 --- a/docs/source/tutorials/planning_with_fixed_joints.rst +++ b/docs/source/tutorials/planning_with_fixed_joints.rst @@ -19,7 +19,7 @@ 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: @@ -27,6 +27,6 @@ Notice we have abstracted away how to decouple this motion into two stages. Here :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. \ No newline at end of file +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. diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py index 808938b..330c165 100644 --- a/mplib/examples/detect_collision.py +++ b/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, diff --git a/mplib/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py index badfdc5..e2f9dcb 100644 --- a/mplib/examples/two_stage_motion.py +++ b/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,10 +160,8 @@ 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 @@ -170,7 +169,7 @@ def demo(self): # 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) diff --git a/mplib/planner.py b/mplib/planner.py index 6969cf1..ae21053 100644 --- a/mplib/planner.py +++ b/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) diff --git a/tests/test_fcl_model.py b/tests/test_fcl_model.py index 6404ff4..245bf21 100644 --- a/tests/test_fcl_model.py +++ b/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__": diff --git a/tests/test_planner.py b/tests/test_planner.py index 3bfc337..a6dcb31 100644 --- a/tests/test_planner.py +++ b/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")