diff --git a/data/panda/panda_on_rail.urdf b/data/panda/panda_on_rail.urdf index 8d2db25d..3a7c91dc 100644 --- a/data/panda/panda_on_rail.urdf +++ b/data/panda/panda_on_rail.urdf @@ -15,12 +15,12 @@ - + - + @@ -39,7 +39,7 @@ - + diff --git a/include/mplib/collision_detection/collision_matrix.h b/include/mplib/collision_detection/collision_matrix.h index 9b6e40e3..4c2dd845 100644 --- a/include/mplib/collision_detection/collision_matrix.h +++ b/include/mplib/collision_detection/collision_matrix.h @@ -154,8 +154,7 @@ class AllowedCollisionMatrix { void clear(); /** - * Get sorted names of all existing elements (including - * default_entries_) + * Get sorted names of all existing elements (including ``default_entries_``) */ std::vector getAllEntryNames() const; diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 7fdcaadd..ddf91496 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -98,7 +98,7 @@ class PlanningWorldTpl { void addArticulation(const ArticulatedModelPtr &model, bool planned = false); /** - * Removes the articulation with given name if exists. Updates acm_ + * Removes the articulation with given name if exists. Updates ``acm_`` * * @param name: name of the articulated model * @return: ``true`` if success, ``false`` if articulation with given name does not @@ -177,7 +177,7 @@ class PlanningWorldTpl { /** * Removes (and detaches) the collision object with given name if exists. - * Updates acm_ + * Updates ``acm_`` * * @param name: name of the non-articulated collision object * @return: ``true`` if success, ``false`` if the non-articulated object @@ -211,7 +211,7 @@ class PlanningWorldTpl { * at its current pose. If the object is currently attached, disallow collision * between the object and previous touch_links. * - * Updates acm_ to allow collisions between attached object and touch_links. + * Updates ``acm_`` to allow collisions between attached object and touch_links. * * @param name: name of the non-articulated object to attach * @param art_name: name of the planned articulation to attach to @@ -229,10 +229,10 @@ class PlanningWorldTpl { * sets touch_links as the name of self links that collide with the object * in the current state. * - * Updates acm_ to allow collisions between attached object and touch_links. + * Updates ``acm_`` to allow collisions between attached object and touch_links. * * If the object is already attached, the touch_links of the attached object - * is preserved and acm_ remains unchanged. + * is preserved and ``acm_`` remains unchanged. * * @param name: name of the non-articulated object to attach * @param art_name: name of the planned articulation to attach to @@ -247,7 +247,7 @@ class PlanningWorldTpl { * at given pose. If the object is currently attached, disallow collision * between the object and previous touch_links. * - * Updates acm_ to allow collisions between attached object and touch_links. + * Updates ``acm_`` to allow collisions between attached object and touch_links. * * @param name: name of the non-articulated object to attach * @param art_name: name of the planned articulation to attach to @@ -266,10 +266,10 @@ class PlanningWorldTpl { * sets touch_links as the name of self links that collide with the object * in the current state. * - * Updates acm_ to allow collisions between attached object and touch_links. + * Updates ``acm_`` to allow collisions between attached object and touch_links. * * If the object is already attached, the touch_links of the attached object - * is preserved and acm_ remains unchanged. + * is preserved and ``acm_`` remains unchanged. * * @param name: name of the non-articulated object to attach * @param art_name: name of the planned articulation to attach to @@ -284,33 +284,33 @@ class PlanningWorldTpl { /** * Attaches given object (w/ p_geom) to specified link of articulation at given pose. * This is done by removing the object and then adding and attaching object. - * As a result, all previous acm_ entries with the object are removed + * As a result, all previous ``acm_`` entries with the object are removed * * @param name: name of the non-articulated object to attach - * @param p_geom: pointer to a CollisionGeometry object + * @param obj_geom: pointer to a CollisionGeometry object * @param art_name: name of the planned articulation to attach to * @param link_id: index of the link of the planned articulation to attach to * @param pose: attached pose (relative pose from attached link to object) * @param touch_links: link names that the attached object touches */ - void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, + void attachObject(const std::string &name, const CollisionGeometryPtr &obj_geom, const std::string &art_name, int link_id, const Pose &pose, const std::vector &touch_links); /** * Attaches given object (w/ p_geom) to specified link of articulation at given pose. * This is done by removing the object and then adding and attaching object. - * As a result, all previous acm_ entries with the object are removed. + * As a result, all previous ``acm_`` entries with the object are removed. * Automatically sets touch_links as the name of self links * that collide with the object in the current state (auto touch_links). * * @param name: name of the non-articulated object to attach - * @param p_geom: pointer to a CollisionGeometry object + * @param obj_geom: pointer to a CollisionGeometry object * @param art_name: name of the planned articulation to attach to * @param link_id: index of the link of the planned articulation to attach to * @param pose: attached pose (relative pose from attached link to object) */ - void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, + void attachObject(const std::string &name, const CollisionGeometryPtr &obj_geom, const std::string &art_name, int link_id, const Pose &pose); /** @@ -349,7 +349,7 @@ class PlanningWorldTpl { /** * Detaches object with given name. - * Updates acm_ to disallow collision between the object and touch_links. + * Updates ``acm_`` to disallow collision between the object and touch_links. * * @param name: name of the non-articulated object to detach * @param also_remove: whether to also remove object from world @@ -358,6 +358,15 @@ class PlanningWorldTpl { */ bool detachObject(const std::string &name, bool also_remove = false); + /** + * Detaches all attached objects. + * Updates ``acm_`` to disallow collision between the object and touch_links. + * + * @param also_remove: whether to also remove objects from world + * @return: ``true`` if success, ``false`` if there are no attached objects + */ + bool detachAllObjects(bool also_remove = false); + /// @brief Prints global pose of all attached bodies void printAttachedBodyPose() const; @@ -375,6 +384,18 @@ class PlanningWorldTpl { /// @brief Get the current allowed collision matrix AllowedCollisionMatrixPtr getAllowedCollisionMatrix() const { return acm_; } + /** + * Set the allowed collision. For more comprehensive API, please get the + * ``AllowedCollisionMatrix`` object and use its methods. + * + * @param name1: name of the first object + * @param name2: name of the second object + */ + void setAllowedCollision(const std::string &name1, const std::string &name2, + bool allowed) { + acm_->setEntry(name1, name2, allowed); + } + /** * Check if the current state is in collision (with the environment or self * collision). diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py index eddf5b7f..c2622455 100644 --- a/mplib/examples/collision_avoidance.py +++ b/mplib/examples/collision_avoidance.py @@ -1,5 +1,7 @@ +import numpy as np import sapien.core as sapien +from mplib import Pose from mplib.examples.demo_setup import DemoSetup from mplib.sapien_utils import SapienPlanner, SapienPlanningWorld @@ -36,56 +38,45 @@ def __init__(self): # red box is the target we want to grab builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) - builder.add_box_visual(half_size=[0.02, 0.02, 0.06]) - red_cube = builder.build(name="red_cube") - red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) + builder.add_box_visual(half_size=[0.02, 0.02, 0.06], material=[1, 0, 0]) + self.red_cube = builder.build(name="red_cube") + self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) # green box is the landing pad on which we want to place the red box builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) - builder.add_box_visual(half_size=[0.04, 0.04, 0.005]) + builder.add_box_visual(half_size=[0.04, 0.04, 0.005], material=[0, 1, 0]) green_cube = builder.build(name="green_cube") green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) # blue box is the obstacle we want to avoid builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) - builder.add_box_visual(half_size=[0.05, 0.2, 0.1]) + builder.add_box_visual(half_size=[0.05, 0.2, 0.1], material=[0, 0, 1]) blue_cube = builder.build(name="blue_cube") blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) planning_world = SapienPlanningWorld(self.scene, [self.robot]) self.planner = SapienPlanner(planning_world, "panda_hand") + # disable collision between the base and the ground + self.planner.planning_world.get_allowed_collision_matrix().set_entry( + "panda_link0", "ground_1", True + ) - def add_point_cloud(self): - """We tell the planner about the obstacle through a point cloud""" - import trimesh - - # add_point_cloud ankor - box = trimesh.creation.box([0.1, 0.4, 0.2]) - points, _ = trimesh.sample.sample_surface(box, 1000) - points += [0.55, 0, 0.1] - self.planner.update_point_cloud(points, resolution=0.02) - # add_point_cloud ankor end - - def demo(self, with_screw=True, use_point_cloud=True, use_attach=True): + def demo(self, with_screw=True, use_attach=True): """ We pick up the red box while avoiding the blue box and place it back down on top of the green box. """ - pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] - delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] - - # tell the planner where the obstacle is - if use_point_cloud: - self.add_point_cloud() + pickup_pose = Pose([0.7, 0, 0.12], [0, 1, 0, 0]) + delivery_pose = Pose([0.4, 0.3, 0.13], [0, 1, 0, 0]) # move to the pickup pose - pickup_pose[2] += 0.2 + pickup_pose.p[2] += 0.2 # no need to check collision against attached object since nothing picked up yet self.move_to_pose(pickup_pose, with_screw) self.open_gripper() - pickup_pose[2] -= 0.12 + pickup_pose.p[2] -= 0.12 # no attach since nothing picked up yet self.move_to_pose(pickup_pose, with_screw) self.close_gripper() @@ -94,24 +85,21 @@ def demo(self, with_screw=True, use_point_cloud=True, use_attach=True): # use_attach ankor if use_attach: - self.planner.update_attached_box( - [0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0] - ) + idx = self.planner.user_link_names.index("panda_hand") + self.planner.planning_world.attach_object(self.red_cube, self.robot, idx) # use_attach ankor end # move to the delivery pose - pickup_pose[2] += 0.12 + pickup_pose.p[2] += 0.12 self.move_to_pose(pickup_pose, with_screw) - delivery_pose[2] += 0.2 + delivery_pose.p[2] += 0.2 self.move_to_pose(delivery_pose, with_screw) - delivery_pose[2] -= 0.12 + delivery_pose.p[2] -= 0.1 self.move_to_pose(delivery_pose, with_screw) self.open_gripper() - delivery_pose[2] += 0.12 + delivery_pose.p[2] += 0.12 if use_attach: - ret = self.planner.detach_object( - f"robot_{self.planner.move_group_link_id}_box", also_remove=True - ) + ret = self.planner.planning_world.detach_all_objects() assert ret, "object is not attached" self.move_to_pose(delivery_pose, with_screw) @@ -126,4 +114,4 @@ def demo(self, with_screw=True, use_point_cloud=True, use_attach=True): the delivery pose """ demo = PlanningDemo() - demo.demo(False, True, True) + demo.demo(False, True) diff --git a/mplib/examples/constrained_planning.py b/mplib/examples/constrained_planning.py index 2a03d0f4..9918f985 100644 --- a/mplib/examples/constrained_planning.py +++ b/mplib/examples/constrained_planning.py @@ -3,6 +3,7 @@ import numpy as np import transforms3d +from mplib import Pose from mplib.examples.demo_setup import DemoSetup @@ -36,7 +37,7 @@ def get_eef_z(self): """Helper function for constraint""" ee_idx = self.planner.link_name_2_idx[self.planner.move_group] ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx) - mat = transforms3d.quaternions.quat2mat(ee_pose[3:]) + mat = transforms3d.quaternions.quat2mat(ee_pose.q) return mat[:, 2] def make_f(self): @@ -94,9 +95,9 @@ def demo(self): self.planner.robot.set_qpos(starting_qpos[:7]) # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis) poses = [ - [-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478], - [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478], - [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0], + Pose([-0.4, -0.3, 0.28], [0.0704682, -0.5356872, 0.8342834, 0.1097478]), + Pose([0.6, 0.1, 0.44], [0.0704682, -0.5356872, -0.8342834, -0.1097478]), + Pose([0, -0.3, 0.5], [0.1304237, -0.9914583, 0, 0]), ] # add some point cloud to make the planning more challenging diff --git a/mplib/examples/demo.py b/mplib/examples/demo.py index 8ac3573c..4a8a7ebf 100644 --- a/mplib/examples/demo.py +++ b/mplib/examples/demo.py @@ -1,5 +1,6 @@ import sapien.core as sapien +from mplib import Pose from mplib.examples.demo_setup import DemoSetup @@ -36,19 +37,19 @@ def __init__(self): # boxes ankor builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) - builder.add_box_visual(half_size=[0.02, 0.02, 0.06]) + builder.add_box_visual(half_size=[0.02, 0.02, 0.06], material=[1, 0, 0]) red_cube = builder.build(name="red_cube") red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06])) builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) - builder.add_box_visual(half_size=[0.02, 0.02, 0.04]) + builder.add_box_visual(half_size=[0.02, 0.02, 0.04], material=[0, 1, 0]) green_cube = builder.build(name="green_cube") green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04])) builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) - builder.add_box_visual(half_size=[0.02, 0.02, 0.07]) + builder.add_box_visual(half_size=[0.02, 0.02, 0.07], material=[0, 0, 1]) blue_cube = builder.build(name="blue_cube") blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07])) # boxes ankor end @@ -61,28 +62,28 @@ def demo(self): """ # target poses ankor poses = [ - [0.4, 0.3, 0.12, 0, 1, 0, 0], - [0.2, -0.3, 0.08, 0, 1, 0, 0], - [0.6, 0.1, 0.14, 0, 1, 0, 0], + Pose([0.4, 0.3, 0.12], [0, 1, 0, 0]), + Pose([0.2, -0.3, 0.08], [0, 1, 0, 0]), + Pose([0.6, 0.1, 0.14], [0, 1, 0, 0]), ] # target poses ankor end # execute motion ankor for i in range(3): pose = poses[i] - pose[2] += 0.2 + pose.p[2] += 0.2 self.move_to_pose(pose) self.open_gripper() - pose[2] -= 0.12 + pose.p[2] -= 0.12 self.move_to_pose(pose) self.close_gripper() - pose[2] += 0.12 + pose.p[2] += 0.12 self.move_to_pose(pose) - pose[0] += 0.1 + pose.p[0] += 0.1 self.move_to_pose(pose) - pose[2] -= 0.12 + pose.p[2] -= 0.12 self.move_to_pose(pose) self.open_gripper() - pose[2] += 0.12 + pose.p[2] += 0.12 self.move_to_pose(pose) # execute motion ankor end diff --git a/mplib/examples/demo_setup.py b/mplib/examples/demo_setup.py index cd178ac0..97df7bea 100644 --- a/mplib/examples/demo_setup.py +++ b/mplib/examples/demo_setup.py @@ -171,7 +171,6 @@ def move_to_pose_with_RRTConnect(self, pose: Pose): # result is a dictionary with keys 'status', 'time', 'position', 'velocity', # 'acceleration', 'duration' # plan_pose ankor - print("plan_pose") result = self.planner.plan_pose(pose, self.robot.get_qpos(), time_step=1 / 250) # plan_pose ankor end if result["status"] != "Success": diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py index 330c1655..e48e9486 100644 --- a/mplib/examples/detect_collision.py +++ b/mplib/examples/detect_collision.py @@ -82,7 +82,7 @@ def demo(self): print(status, path) print("\n----- no more env-collision after removing the floor -----") - self.planner.remove_normal_object("floor") + self.planner.remove_object("floor") self.print_collisions(self.planner.check_for_env_collision(env_collision_qpos)) # end ankor diff --git a/mplib/examples/moving_robot.py b/mplib/examples/moving_robot.py index 4d982b46..ca9e51d3 100644 --- a/mplib/examples/moving_robot.py +++ b/mplib/examples/moving_robot.py @@ -1,5 +1,6 @@ import sapien.core as sapien +from mplib import Pose from mplib.examples.demo_setup import DemoSetup @@ -26,7 +27,7 @@ def __init__(self): # We also need to tell the planner where the base is # since the sim and planner don't share info - self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0]) + self.planner.set_base_pose(Pose([1, 1, 0], [1, 0, 0, 0])) # Set initial joint positions init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] @@ -42,19 +43,19 @@ def __init__(self): # boxes builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) - builder.add_box_visual(half_size=[0.02, 0.02, 0.06]) + builder.add_box_visual(half_size=[0.02, 0.02, 0.06], material=[1, 0, 0]) red_cube = builder.build(name="red_cube") red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06])) builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) - builder.add_box_visual(half_size=[0.02, 0.02, 0.04]) + builder.add_box_visual(half_size=[0.02, 0.02, 0.04], material=[0, 1, 0]) green_cube = builder.build(name="green_cube") green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04])) builder = self.scene.create_actor_builder() builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) - builder.add_box_visual(half_size=[0.02, 0.02, 0.07]) + builder.add_box_visual(half_size=[0.02, 0.02, 0.07], material=[0, 0, 1]) blue_cube = builder.build(name="blue_cube") blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07])) @@ -67,32 +68,32 @@ def demo(self): the poses are specified with respect to the base of the robot. """ poses = [ - [1.4, 1.3, 0.12, 0, 1, 0, 0], - [1.2, 0.7, 0.08, 0, 1, 0, 0], - [1.6, 1.1, 0.14, 0, 1, 0, 0], + Pose([1.4, 1.3, 0.12], [0, 1, 0, 0]), + Pose([1.2, 0.7, 0.08], [0, 1, 0, 0]), + Pose([1.6, 1.1, 0.14], [0, 1, 0, 0]), ] for i in range(3): pose = poses[i] - pose[2] += 0.2 + pose.p[2] += 0.2 self.move_to_pose(pose) self.open_gripper() - pose[2] -= 0.12 + pose.p[2] -= 0.12 self.move_to_pose(pose) self.close_gripper() - pose[2] += 0.12 + pose.p[2] += 0.12 self.move_to_pose(pose) - pose[0] += 0.1 + pose.p[0] += 0.1 self.move_to_pose(pose) - pose[2] -= 0.12 + pose.p[2] -= 0.12 self.move_to_pose(pose) self.open_gripper() - pose[2] += 0.12 + pose.p[2] += 0.12 self.move_to_pose(pose) # convert the poses to be w.r.t. the base for pose in poses: - pose[0] -= 1 - pose[1] -= 1 + pose.p[0] -= 1 + pose.p[1] -= 1 # plan a path to the first pose result = self.planner.plan_pose( diff --git a/mplib/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py index e2f9dcb6..29510ac4 100644 --- a/mplib/examples/two_stage_motion.py +++ b/mplib/examples/two_stage_motion.py @@ -154,8 +154,8 @@ def demo(self): first by moving the base only and then the arm only """ # pickup ankor - pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] - delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] + pickup_pose = Pose([0.7, 0, 0.12], [0, 1, 0, 0]) + delivery_pose = Pose([0.4, 0.3, 0.13], [0, 1, 0, 0]) self.add_point_cloud() # also add the target as a collision object so we don't hit it @@ -164,7 +164,7 @@ def demo(self): self.planner.planning_world.add_object("target", collision_object) # go above the target - pickup_pose[2] += 0.2 + pickup_pose.p[2] += 0.2 self.move_in_two_stage(pickup_pose) # pickup ankor end self.open_gripper() @@ -172,7 +172,7 @@ def demo(self): self.planner.planning_world.remove_object( "target" ) # remove the object so we don't report collision - pickup_pose[2] -= 0.12 + pickup_pose.p[2] -= 0.12 result = self.plan_without_base(pickup_pose) self.follow_path(result) self.close_gripper() @@ -183,17 +183,17 @@ def demo(self): # add the attached box to the planning world self.planner.update_attached_box([0.04, 0.04, 0.12], Pose(p=[0, 0, 0.14])) - pickup_pose[2] += 0.12 + pickup_pose.p[2] += 0.12 result = self.plan_without_base(pickup_pose, has_attach=True) self.follow_path(result) - delivery_pose[2] += 0.2 + delivery_pose.p[2] += 0.2 # now go to the drop off in two stages self.move_in_two_stage(delivery_pose, has_attach=True) - delivery_pose[2] -= 0.12 + delivery_pose.p[2] -= 0.12 result = self.plan_without_base(delivery_pose, has_attach=True) self.follow_path(result) self.open_gripper() - delivery_pose[2] += 0.12 + delivery_pose.p[2] += 0.12 result = self.plan_without_base(delivery_pose) self.follow_path(result) diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 4635178a..94bbc401 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -420,7 +420,7 @@ class PlanningWorld: its current pose. If the object is currently attached, disallow collision between the object and previous touch_links. - Updates acm_ to allow collisions between attached object and touch_links. + Updates ``acm_`` to allow collisions between attached object and touch_links. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -437,10 +437,10 @@ class PlanningWorld: touch_links as the name of self links that collide with the object in the current state. - Updates acm_ to allow collisions between attached object and touch_links. + Updates ``acm_`` to allow collisions between attached object and touch_links. If the object is already attached, the touch_links of the attached object is - preserved and acm_ remains unchanged. + preserved and ``acm_`` remains unchanged. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -457,7 +457,7 @@ class PlanningWorld: given pose. If the object is currently attached, disallow collision between the object and previous touch_links. - Updates acm_ to allow collisions between attached object and touch_links. + Updates ``acm_`` to allow collisions between attached object and touch_links. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -475,10 +475,10 @@ class PlanningWorld: touch_links as the name of self links that collide with the object in the current state. - Updates acm_ to allow collisions between attached object and touch_links. + Updates ``acm_`` to allow collisions between attached object and touch_links. If the object is already attached, the touch_links of the attached object is - preserved and acm_ remains unchanged. + preserved and ``acm_`` remains unchanged. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -491,7 +491,7 @@ class PlanningWorld: def attach_object( self, name: str, - p_geom: collision_detection.fcl.CollisionGeometry, + obj_geom: collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, pose: Pose, @@ -500,10 +500,10 @@ class PlanningWorld: """ Attaches given object (w/ p_geom) to specified link of articulation at given pose. This is done by removing the object and then adding and attaching object. - As a result, all previous acm_ entries with the object are removed + As a result, all previous ``acm_`` entries with the object are removed :param name: name of the non-articulated object to attach - :param p_geom: pointer to a CollisionGeometry object + :param obj_geom: pointer to a CollisionGeometry object :param art_name: name of the planned articulation to attach to :param link_id: index of the link of the planned articulation to attach to :param pose: attached pose (relative pose from attached link to object) @@ -513,7 +513,7 @@ class PlanningWorld: def attach_object( self, name: str, - p_geom: collision_detection.fcl.CollisionGeometry, + obj_geom: collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, pose: Pose, @@ -521,12 +521,12 @@ class PlanningWorld: """ Attaches given object (w/ p_geom) to specified link of articulation at given pose. This is done by removing the object and then adding and attaching object. - As a result, all previous acm_ entries with the object are removed. + As a result, all previous ``acm_`` entries with the object are removed. Automatically sets touch_links as the name of self links that collide with the object in the current state (auto touch_links). :param name: name of the non-articulated object to attach - :param p_geom: pointer to a CollisionGeometry object + :param obj_geom: pointer to a CollisionGeometry object :param art_name: name of the planned articulation to attach to :param link_id: index of the link of the planned articulation to attach to :param pose: attached pose (relative pose from attached link to object) @@ -572,10 +572,18 @@ class PlanningWorld: :param request: collision request params. :return: List of ``WorldCollisionResult`` objects """ + def detach_all_objects(self, also_remove: bool = False) -> bool: + """ + Detaches all attached objects. Updates ``acm_`` to disallow collision between + the object and touch_links. + + :param also_remove: whether to also remove objects from world + :return: ``True`` if success, ``False`` if there are no attached objects + """ def detach_object(self, name: str, also_remove: bool = False) -> bool: """ - Detaches object with given name. Updates acm_ to disallow collision between the - object and touch_links. + Detaches object with given name. Updates ``acm_`` to disallow collision between + the object and touch_links. :param name: name of the non-articulated object to detach :param also_remove: whether to also remove object from world @@ -714,7 +722,7 @@ class PlanningWorld: """ def remove_articulation(self, name: str) -> bool: """ - Removes the articulation with given name if exists. Updates acm_ + Removes the articulation with given name if exists. Updates ``acm_`` :param name: name of the articulated model :return: ``True`` if success, ``False`` if articulation with given name does not @@ -723,7 +731,7 @@ class PlanningWorld: def remove_object(self, name: str) -> bool: """ Removes (and detaches) the collision object with given name if exists. Updates - acm_ + ``acm_`` :param name: name of the non-articulated collision object :return: ``True`` if success, ``False`` if the non-articulated object with given diff --git a/mplib/pymp/collision_detection/__init__.pyi b/mplib/pymp/collision_detection/__init__.pyi index 050cf1e9..073c4aba 100644 --- a/mplib/pymp/collision_detection/__init__.pyi +++ b/mplib/pymp/collision_detection/__init__.pyi @@ -77,7 +77,7 @@ class AllowedCollisionMatrix: """ def get_all_entry_names(self) -> list[str]: """ - Get sorted names of all existing elements (including default_entries_) + Get sorted names of all existing elements (including ``default_entries_``) """ def get_allowed_collision(self, name1: str, name2: str) -> AllowedCollision | None: """ diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index dab841fd..a3f5a23a 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -1,6 +1,7 @@ from __future__ import annotations -from typing import Optional, Sequence +import warnings +from typing import Literal, Optional, Sequence import numpy as np from sapien import Entity, Scene @@ -29,6 +30,7 @@ Box, BVHModel, Capsule, + CollisionGeometry, CollisionObject, Convex, Cylinder, @@ -44,6 +46,9 @@ from .srdf_exporter import export_srdf from .urdf_exporter import export_kinematic_chain_urdf +YELLOW_COLOR = "\033[93m" +RESET_COLOR = "\033[0m" + # TODO: link names? def convert_object_name(obj: PhysxArticulation | Entity) -> str: @@ -169,9 +174,11 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None ) > 0 ): - raise RuntimeError( - f"Entity {entity.name} not found in PlanningWorld! " - "The scene might have changed since last update." + warnings.warn( + YELLOW_COLOR + f"Entity {entity.name} not found in PlanningWorld! " + "The scene might have changed since last update. " + "Use PlanningWorld.add_object() to add the object." + RESET_COLOR, + stacklevel=2, ) def check_collision_between( @@ -355,6 +362,412 @@ def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: shape_poses, ) + # ----- Overloaded PlanningWorld methods to accept SAPIEN objects ----- # + def is_articulation_planned(self, articulation: PhysxArticulation | str) -> bool: # type: ignore + """ + Check whether the given articulation is being planned + + :param articulation: the articulation (or its name) to check + :return: ``True`` if the articulation is being planned, ``False`` otherwise. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.is_articulation_planned() + + method + .. automethod:: mplib.PlanningWorld.is_articulation_planned + :no-index: + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + return super().is_articulation_planned(articulation) + + def set_articulation_planned( # type: ignore + self, articulation: PhysxArticulation | str, planned: bool + ) -> None: + """ + Sets the given articulation as being planned or not + + :param articulation: the articulation (or its name) + :param planned: whether the articulation should be planned + + .. raw:: html + +
+ Overloaded + + PlanningWorld.set_articulation_planned() + + method + .. automethod:: mplib.PlanningWorld.set_articulation_planned + :no-index: + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + super().set_articulation_planned(articulation, planned) + + def has_object(self, obj: Entity | str) -> bool: + """ + Check whether the given non-articulated object exists. + + :param obj: the object (or its name) to check + + :return: ``True`` if the object exists, ``False`` otherwise. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.has_object() + + method + .. automethod:: mplib.PlanningWorld.has_object + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + return super().has_object(obj) + + def add_object(self, obj: FCLObject | Entity) -> None: + """ + Adds a non-articulated object to the PlanningWorld. + + :param obj: the non-articulated object to add + + .. raw:: html + +
+ Overloaded + + PlanningWorld.add_object() + + method + .. automethod:: mplib.PlanningWorld.add_object + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + component = obj.find_component_by_type(PhysxRigidBaseComponent) + assert component is not None, ( + f"No PhysxRigidBaseComponent found in {obj.name}: " f"{obj.components=}" + ) + + # Convert collision shapes at current global pose + if (fcl_obj := self.convert_physx_component(component)) is not None: # type: ignore + self.add_object(fcl_obj) + else: + super().add_object(obj) + + def remove_object(self, obj: Entity | str) -> None: + """ + Removes a non-articulated object from the PlanningWorld. + + :param obj: the non-articulated object (or its name) to remove + + .. raw:: html + +
+ Overloaded + + PlanningWorld.remove_object() + + method + .. automethod:: mplib.PlanningWorld.remove_object + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + super().remove_object(obj) + + def is_object_attached(self, obj: Entity | str) -> bool: # type: ignore + """ + Check whether the given non-articulated object is attached + + :param obj: the non-articulated object (or its name) to check + :return: ``True`` if the articulation is attached, ``False`` otherwise. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.is_object_attached() + + method + .. automethod:: mplib.PlanningWorld.is_object_attached + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + return super().is_object_attached(obj) + + def attach_object( # type: ignore + self, + obj: Entity | str, + articulation: PhysxArticulation | str, + link: PhysxArticulationLinkComponent | int, + pose: Optional[Pose] = None, + *, + touch_links: Optional[list[PhysxArticulationLinkComponent | str]] = None, + obj_geom: Optional[CollisionGeometry] = None, + ) -> None: + """ + Attaches given non-articulated object to the specified link of articulation. + + Updates ``acm_`` to allow collisions between attached object and touch_links. + + :param obj: the non-articulated object (or its name) to attach + :param articulation: the planned articulation (or its name) to attach to + :param link: the link of the planned articulation (or its index) to attach to + :param pose: attached pose (relative pose from attached link to object). + If ``None``, attach the object at its current pose. + :param touch_links: links (or their names) that the attached object touches. + When ``None``, + + * if the object is not currently attached, touch_links are set to the name + of articulation links that collide with the object in the current state. + + * if the object is already attached, touch_links of the attached object + is preserved and ``acm_`` remains unchanged. + :param obj_geom: a CollisionGeometry object representing the attached object. + If not ``None``, pose must be not ``None``. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_object() + + methods + .. automethod:: mplib.PlanningWorld.attach_object + :no-index: + .. raw:: html +
+ """ + kwargs = {"name": obj, "art_name": articulation, "link_id": link} + if pose is not None: + kwargs["pose"] = pose + if touch_links is not None: + kwargs["touch_links"] = [ + l.name if isinstance(l, PhysxArticulationLinkComponent) else l + for l in touch_links # noqa: E741 + ] + if obj_geom is not None: + kwargs["obj_geom"] = obj_geom + + if isinstance(obj, Entity): + kwargs["name"] = convert_object_name(obj) + if isinstance(articulation, PhysxArticulation): + kwargs["art_name"] = articulation = convert_object_name(articulation) + if isinstance(link, PhysxArticulationLinkComponent): + kwargs["link_id"] = ( + self.get_articulation(articulation) + .get_pinocchio_model() + .get_link_names() + .index(link.name) + ) + + super().attach_object(**kwargs) + + def detach_object(self, obj: Entity | str, also_remove: bool = False) -> bool: # type: ignore + """ + Detaches the given object. + + Updates ``acm_`` to disallow collision between the object and touch_links. + + :param obj: the non-articulated object (or its name) to detach + :param also_remove: whether to also remove the object from PlanningWorld + :return: ``True`` if success, ``False`` if the given object is not attached. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.detach_object() + + method + .. automethod:: mplib.PlanningWorld.detach_object + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + return super().detach_object(obj, also_remove=also_remove) + + def attach_sphere( # type: ignore + self, + radius: float, + articulation: PhysxArticulation | str, + link: PhysxArticulationLinkComponent | int, + pose: Pose, + ) -> None: + """ + Attaches given sphere to specified link of articulation (auto touch_links) + + :param radius: sphere radius + :param articulation: the planned articulation (or its name) to attach to + :param link: the link of the planned articulation (or its index) to attach to + :param pose: attached pose (relative pose from attached link to object) + + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_sphere() + + method + .. automethod:: mplib.PlanningWorld.attach_sphere + :no-index: + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + if isinstance(link, PhysxArticulationLinkComponent): + link = ( + self.get_articulation(articulation) + .get_pinocchio_model() + .get_link_names() + .index(link.name) + ) + super().attach_sphere(radius, articulation, link, pose) + + def attach_box( # type: ignore + self, + size: Sequence[float] + | np.ndarray[tuple[Literal[3], Literal[1]], np.dtype[np.floating]], + articulation: PhysxArticulation | str, + link: PhysxArticulationLinkComponent | int, + pose: Pose, + ) -> None: + """ + Attaches given box to specified link of articulation (auto touch_links) + + :param size: box side length + :param articulation: the planned articulation (or its name) to attach to + :param link: the link of the planned articulation (or its index) to attach to + :param pose: attached pose (relative pose from attached link to object) + + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_box() + + method + .. automethod:: mplib.PlanningWorld.attach_box + :no-index: + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + if isinstance(link, PhysxArticulationLinkComponent): + link = ( + self.get_articulation(articulation) + .get_pinocchio_model() + .get_link_names() + .index(link.name) + ) + super().attach_box(size, articulation, link, pose) # type: ignore + + def attach_mesh( # type: ignore + self, + mesh_path: str, + articulation: PhysxArticulation | str, + link: PhysxArticulationLinkComponent | int, + pose: Pose, + *, + convex: bool = False, + ) -> None: + """ + Attaches given mesh to specified link of articulation (auto touch_links) + + :param mesh_path: path to a mesh file + :param articulation: the planned articulation (or its name) to attach to + :param link: the link of the planned articulation (or its index) to attach to + :param pose: attached pose (relative pose from attached link to object) + :param convex: whether to load mesh as a convex mesh. Default: ``False``. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_mesh() + + method + .. automethod:: mplib.PlanningWorld.attach_mesh + :no-index: + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + if isinstance(link, PhysxArticulationLinkComponent): + link = ( + self.get_articulation(articulation) + .get_pinocchio_model() + .get_link_names() + .index(link.name) + ) + super().attach_mesh(mesh_path, articulation, link, pose, convex=convex) + + def set_allowed_collision( + self, + obj1: Entity | PhysxArticulationLinkComponent | str, + obj2: Entity | PhysxArticulationLinkComponent | str, + allowed: bool, + ) -> None: + """ + Set allowed collision between two objects. + + :param obj1: the first object (or its name) + :param obj2: the second object (or its name) + + .. raw:: html + +
+ Overloaded + + PlanningWorld.set_allowed_collision() + + method + .. automethod:: mplib.PlanningWorld.set_allowed_collision + :no-index: + .. raw:: html +
+ """ + if isinstance(obj1, Entity): + obj1 = convert_object_name(obj1) + elif isinstance(obj1, PhysxArticulationLinkComponent): + obj1 = obj1.name + if isinstance(obj2, Entity): + obj2 = convert_object_name(obj2) + elif isinstance(obj2, PhysxArticulationLinkComponent): + obj2 = obj2.name + super().set_allowed_collision(obj1, obj2, allowed) + class SapienPlanner(Planner): def __init__( @@ -407,6 +820,18 @@ def __init__( self.joint_acc_limits = joint_acc_limits self.move_group_link_id = self.link_name_2_idx[self.move_group] + # do a robot env collision check and warn if there is a collision + collisions = self.planning_world.check_robot_collision() + if len(collisions): + for collision in collisions: + warnings.warn( + YELLOW_COLOR + f"Robot's {collision.link_name1} collides with " + f"{collision.link_name2} of {collision.object_name2} in initial " + f"state. Use planner.planning_world.get_allowed_collision_matrix() " + f"to disable collisions if planning fails" + RESET_COLOR, + stacklevel=2, + ) + assert ( len(self.joint_vel_limits) == len(self.joint_acc_limits) diff --git a/pybind/docstring/collision_detection/collision_matrix.h b/pybind/docstring/collision_detection/collision_matrix.h index b6b4dc96..bf0c37ee 100644 --- a/pybind/docstring/collision_detection/collision_matrix.h +++ b/pybind/docstring/collision_detection/collision_matrix.h @@ -46,7 +46,7 @@ Clear all data in the allowed collision matrix)doc"; static const char *__doc_mplib_collision_detection_AllowedCollisionMatrix_getAllEntryNames = R"doc( -Get sorted names of all existing elements (including default_entries_))doc"; +Get sorted names of all existing elements (including ``default_entries_``))doc"; static const char *__doc_mplib_collision_detection_AllowedCollisionMatrix_getAllowedCollision = R"doc( diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index afb70104..10238055 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -97,7 +97,7 @@ Attaches existing non-articulated object to specified link of articulation at its current pose. If the object is currently attached, disallow collision between the object and previous touch_links. -Updates acm_ to allow collisions between attached object and touch_links. +Updates ``acm_`` to allow collisions between attached object and touch_links. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -113,10 +113,10 @@ its current pose. If the object is not currently attached, automatically sets touch_links as the name of self links that collide with the object in the current state. -Updates acm_ to allow collisions between attached object and touch_links. +Updates ``acm_`` to allow collisions between attached object and touch_links. If the object is already attached, the touch_links of the attached object is -preserved and acm_ remains unchanged. +preserved and ``acm_`` remains unchanged. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -130,7 +130,7 @@ Attaches existing non-articulated object to specified link of articulation at given pose. If the object is currently attached, disallow collision between the object and previous touch_links. -Updates acm_ to allow collisions between attached object and touch_links. +Updates ``acm_`` to allow collisions between attached object and touch_links. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -147,10 +147,10 @@ given pose. If the object is not currently attached, automatically sets touch_links as the name of self links that collide with the object in the current state. -Updates acm_ to allow collisions between attached object and touch_links. +Updates ``acm_`` to allow collisions between attached object and touch_links. If the object is already attached, the touch_links of the attached object is -preserved and acm_ remains unchanged. +preserved and ``acm_`` remains unchanged. :param name: name of the non-articulated object to attach :param art_name: name of the planned articulation to attach to @@ -163,10 +163,10 @@ static const char *__doc_mplib_PlanningWorldTpl_attachObject_5 = R"doc( Attaches given object (w/ p_geom) to specified link of articulation at given pose. This is done by removing the object and then adding and attaching object. -As a result, all previous acm_ entries with the object are removed +As a result, all previous ``acm_`` entries with the object are removed :param name: name of the non-articulated object to attach -:param p_geom: pointer to a CollisionGeometry object +:param obj_geom: pointer to a CollisionGeometry object :param art_name: name of the planned articulation to attach to :param link_id: index of the link of the planned articulation to attach to :param pose: attached pose (relative pose from attached link to object) @@ -176,12 +176,12 @@ static const char *__doc_mplib_PlanningWorldTpl_attachObject_6 = R"doc( Attaches given object (w/ p_geom) to specified link of articulation at given pose. This is done by removing the object and then adding and attaching object. -As a result, all previous acm_ entries with the object are removed. +As a result, all previous ``acm_`` entries with the object are removed. Automatically sets touch_links as the name of self links that collide with the object in the current state (auto touch_links). :param name: name of the non-articulated object to attach -:param p_geom: pointer to a CollisionGeometry object +:param obj_geom: pointer to a CollisionGeometry object :param art_name: name of the planned articulation to attach to :param link_id: index of the link of the planned articulation to attach to :param pose: attached pose (relative pose from attached link to object))doc"; @@ -219,10 +219,18 @@ articulation-attach collision, attach-attach collision) :param request: collision request params. :return: List of ``WorldCollisionResult`` objects)doc"; +static const char *__doc_mplib_PlanningWorldTpl_detachAllObjects = +R"doc( +Detaches all attached objects. Updates ``acm_`` to disallow collision between +the object and touch_links. + +:param also_remove: whether to also remove objects from world +:return: ``True`` if success, ``False`` if there are no attached objects)doc"; + static const char *__doc_mplib_PlanningWorldTpl_detachObject = R"doc( -Detaches object with given name. Updates acm_ to disallow collision between the -object and touch_links. +Detaches object with given name. Updates ``acm_`` to disallow collision between +the object and touch_links. :param name: name of the non-articulated object to detach :param also_remove: whether to also remove object from world @@ -279,6 +287,14 @@ static const char *__doc_mplib_PlanningWorldTpl_getAllowedCollisionMatrix = R"doc( Get the current allowed collision matrix)doc"; +static const char *__doc_mplib_PlanningWorldTpl_setAllowedCollision = +R"doc( +Set the allowed collision. For more comprehensive API, please get the +``AllowedCollisionMatrix`` object and use its methods. + +:param name1: name of the first object +:param name2: name of the second object)doc"; + static const char *__doc_mplib_PlanningWorldTpl_getArticulation = R"doc( Gets the articulation (ArticulatedModelPtr) with given name @@ -353,7 +369,7 @@ Prints global pose of all attached bodies)doc"; static const char *__doc_mplib_PlanningWorldTpl_removeArticulation = R"doc( -Removes the articulation with given name if exists. Updates acm_ +Removes the articulation with given name if exists. Updates ``acm_`` :param name: name of the articulated model :return: ``True`` if success, ``False`` if articulation with given name does not @@ -362,7 +378,7 @@ Removes the articulation with given name if exists. Updates acm_ static const char *__doc_mplib_PlanningWorldTpl_removeObject = R"doc( Removes (and detaches) the collision object with given name if exists. Updates -acm_ +``acm_`` :param name: name of the non-articulated collision object :return: ``True`` if success, ``False`` if the non-articulated object with given diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 589c1b0e..a0684b30 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -103,15 +103,16 @@ void build_pyplanning_world(py::module &pymp) { const std::string &, int, const Pose &, const std::vector &>( &PlanningWorld::attachObject), - py::arg("name"), py::arg("p_geom"), py::arg("art_name"), py::arg("link_id"), - py::arg("pose"), py::arg("touch_links"), + py::arg("name"), py::arg("obj_geom"), py::arg("art_name"), + py::arg("link_id"), py::arg("pose"), py::arg("touch_links"), DOC(mplib, PlanningWorldTpl, attachObject, 5)) .def("attach_object", py::overload_cast &>( &PlanningWorld::attachObject), - py::arg("name"), py::arg("p_geom"), py::arg("art_name"), py::arg("link_id"), - py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachObject, 6)) + py::arg("name"), py::arg("obj_geom"), py::arg("art_name"), + py::arg("link_id"), py::arg("pose"), + DOC(mplib, PlanningWorldTpl, attachObject, 6)) .def("attach_sphere", &PlanningWorld::attachSphere, py::arg("radius"), py::arg("art_name"), py::arg("link_id"), py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachSphere)) @@ -123,6 +124,9 @@ void build_pyplanning_world(py::module &pymp) { py::arg("convex") = false, DOC(mplib, PlanningWorldTpl, attachMesh)) .def("detach_object", &PlanningWorld::detachObject, py::arg("name"), py::arg("also_remove") = false, DOC(mplib, PlanningWorldTpl, detachObject)) + .def("detach_all_objects", &PlanningWorld::detachAllObjects, + py::arg("also_remove") = false, + DOC(mplib, PlanningWorldTpl, detachAllObjects)) .def("print_attached_body_pose", &PlanningWorld::printAttachedBodyPose, DOC(mplib, PlanningWorldTpl, printAttachedBodyPose)) @@ -133,6 +137,9 @@ void build_pyplanning_world(py::module &pymp) { .def("get_allowed_collision_matrix", &PlanningWorld::getAllowedCollisionMatrix, DOC(mplib, PlanningWorldTpl, getAllowedCollisionMatrix)) + .def("set_allowed_collision", &PlanningWorld::setAllowedCollision, + py::arg("name1"), py::arg("name2"), py::arg("allowed"), + DOC(mplib, PlanningWorldTpl, setAllowedCollision)) .def("is_state_colliding", &PlanningWorld::isStateColliding, DOC(mplib, PlanningWorldTpl, isStateColliding)) diff --git a/pybind/utils/pose.cpp b/pybind/utils/pose.cpp index 0a789b20..7b168491 100644 --- a/pybind/utils/pose.cpp +++ b/pybind/utils/pose.cpp @@ -75,7 +75,12 @@ void build_utils_pose(py::module &pymp) { }, DOC(mplib, Pose, to_transformation_matrix)) - .def_readwrite("p", &Pose::p, DOC(mplib, Pose, p)) + // def_readwrite is shorthand for def_property, but the default getter/setter + // functions have const args https://stackoverflow.com/a/57927899 + // note that another crucial step is to specify the return type of the lambda + .def_property( + "p", [](Pose &pose) -> Vector3 & { return pose.p; }, + [](Pose &pose, const Vector3 &p) { pose.p = p; }, DOC(mplib, Pose, p)) .def( "set_p", [](Pose &pose, const Vector3 &p) { pose.p = p; }, py::arg("p"), DOC(mplib, Pose, set_p)) diff --git a/pyproject.toml b/pyproject.toml index b33bd559..243036fb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -141,7 +141,7 @@ manylinux-x86_64-image = "kolinguo/mplib-build:latest" container-engine = { name = "docker", create-args = ["--rm"]} # only run tests on python3.10 test-requires = ["trimesh", "transforms3d"] -test-command = "python3.10 -m unittest discover -s {project}/tests -v" +test-command = "python3.10 -m pip install sapien==3.0.0.dev2 && python3.10 -m unittest discover -s {project}/tests -v" test-skip = ["cp37-*", "cp38-*", "cp39-*", "cp311-*", "cp312-*"] [tool.cibuildwheel.linux] diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index b4107bd3..b40f3f2e 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -71,6 +71,7 @@ std::unique_ptr> ArticulatedModelTpl::createFromURDFSt fcl_model->removeCollisionPairsFromSRDFString(srdf_string); articulation->current_qpos_ = VectorX::Constant(pinocchio_model->getModel().nv, 0); articulation->setMoveGroup(user_link_names); + articulation->setBasePose(Pose()); return articulation; } diff --git a/src/planning_world.cpp b/src/planning_world.cpp index b9f9fe1b..3b7283d9 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -164,8 +164,7 @@ void PlanningWorldTpl::attachObject(const std::string &name, attached_body_map_[name] = body; // Set touch_links to the name of self links colliding with object currently std::vector touch_links; - auto collisions = checkSelfCollision(); - for (const auto &collision : collisions) + for (const auto &collision : checkSelfCollision()) if (collision.link_name1 == name) touch_links.push_back(collision.link_name2); else if (collision.link_name2 == name) @@ -178,22 +177,22 @@ void PlanningWorldTpl::attachObject(const std::string &name, template void PlanningWorldTpl::attachObject(const std::string &name, - const CollisionGeometryPtr &p_geom, + const CollisionGeometryPtr &obj_geom, const std::string &art_name, int link_id, const Pose &pose, const std::vector &touch_links) { removeObject(name); - addObject(name, std::make_shared(p_geom)); + addObject(name, std::make_shared(obj_geom)); attachObject(name, art_name, link_id, pose, touch_links); } template void PlanningWorldTpl::attachObject(const std::string &name, - const CollisionGeometryPtr &p_geom, + const CollisionGeometryPtr &obj_geom, const std::string &art_name, int link_id, const Pose &pose) { removeObject(name); - addObject(name, std::make_shared(p_geom)); + addObject(name, std::make_shared(obj_geom)); attachObject(name, art_name, link_id, pose); } @@ -247,6 +246,15 @@ bool PlanningWorldTpl::detachObject(const std::string &name, bool also_remove return true; } +template +bool PlanningWorldTpl::detachAllObjects(bool also_remove) { + if (attached_body_map_.empty()) return false; + std::vector names; + for (const auto &pair : attached_body_map_) names.push_back(pair.first); + for (const auto &name : names) detachObject(name, also_remove); + return true; +} + template void PlanningWorldTpl::printAttachedBodyPose() const { for (const auto &[name, body] : attached_body_map_) diff --git a/tests/test_pinocchio.py b/tests/test_pinocchio.py index e42c4857..3f5592c6 100644 --- a/tests/test_pinocchio.py +++ b/tests/test_pinocchio.py @@ -154,7 +154,7 @@ def test_link_jacobian(self): analytical_jacobian = self.model.compute_single_link_jacobian(qpos, ee_idx) numerical_jacobian = self.get_numerical_jacobian(qpos, ee_idx) self.assertTrue( - np.allclose(analytical_jacobian, numerical_jacobian, atol=1e-3) + np.allclose(analytical_jacobian, numerical_jacobian, atol=2e-3) ) diff --git a/tests/test_sapien_conversion.py b/tests/test_sapien_conversion.py new file mode 100644 index 00000000..fb766466 --- /dev/null +++ b/tests/test_sapien_conversion.py @@ -0,0 +1,156 @@ +import os +import unittest + +import numpy as np +import sapien.core as sapien +from transforms3d.quaternions import mat2quat + +from mplib import Pose +from mplib.sapien_utils.conversion import ( + SapienPlanner, + SapienPlanningWorld, + convert_object_name, +) + +FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) + +PANDA_SPEC = { + "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", + "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", + "move_group": "panda_hand", +} + + +class TestSapienConversion(unittest.TestCase): + """ + + def setUp(self): + self.engine = sapien.Engine() + scene_config = sapien.SceneConfig() + self.scene = self.engine.create_scene(scene_config) + self.scene.set_timestep(1 / 240) + self.scene.add_ground(0) + self.scene.default_physical_material = self.scene.create_physical_material( + 1, 1, 0 + ) + + # robot + loader: sapien.URDFLoader = self.scene.create_urdf_loader() + loader.fix_root_link = True + self.robot: sapien.Articulation = loader.load(PANDA_SPEC["urdf"]) + self.robot.set_root_pose(sapien.Pose([0, 0, 0], [1, 0, 0, 0])) + self.robot.set_qpos([0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]) + + # red box is the target we want to grab + builder = self.scene.create_actor_builder() + builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) + builder.add_box_visual(half_size=[0.02, 0.02, 0.06], material=[1, 0, 0]) + self.red_cube = builder.build(name="red_cube") + self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) + + # green box is the landing pad on which we want to place the red box + builder = self.scene.create_actor_builder() + builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) + builder.add_box_visual(half_size=[0.04, 0.04, 0.005], material=[0, 1, 0]) + self.green_cube = builder.build(name="green_cube") + self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) + + # blue box is the obstacle we want to avoid + builder = self.scene.create_actor_builder() + builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) + builder.add_box_visual(half_size=[0.05, 0.2, 0.1], material=[0, 0, 1]) + self.blue_cube = builder.build(name="blue_cube") + self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) + + self.planning_world = SapienPlanningWorld(self.scene, [self.robot]) + self.planner = SapienPlanner(self.planning_world, "panda_hand") + # disable collision between the base and the ground + self.planning_world.get_allowed_collision_matrix().set_entry( + "panda_link0", "ground_1", True + ) + + self.target_pose = Pose([0.7, 0, 0.3], [0, 1, 0, 0]) + + def follow_path(self, result): + # for now just set the qpos directly + full_qpos = self.robot.get_qpos() + full_qpos[: len(self.planner.move_group_joint_indices)] = result["position"][-1] + self.robot.set_qpos(full_qpos) + + def test_articulation_planned(self): + self.assertTrue(self.planning_world.is_articulation_planned(self.robot)) + self.planning_world.set_articulation_planned(self.robot, False) + self.assertFalse(self.planning_world.is_articulation_planned(self.robot)) + + # after toggling back to True, we should be able to plan to a pose + self.planning_world.set_articulation_planned(self.robot, True) + result = self.planner.plan_pose(self.target_pose, self.robot.get_qpos()) + self.assertTrue(result["status"] == "Success") + + self.follow_path(result) + # check if the robot is at the target pose + self.planner.update_from_simulation() + ee_idx = self.planner.user_link_names.index("panda_hand") + ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx) + self.assertAlmostEqual(self.target_pose.distance(ee_pose), 0, places=3) + + def test_add_remove_obj(self): + self.assertTrue(self.planning_world.has_object(self.red_cube)) + self.planning_world.remove_object(self.red_cube) + self.assertFalse(self.planning_world.has_object(self.red_cube)) + + # after removing the blue cube, the plan should succeed + self.planning_world.remove_object(self.blue_cube) + result = self.planner.plan_screw(self.target_pose, self.robot.get_qpos()) + self.assertTrue(result["status"] == "Success") + # but after adding it back, the plan should fail + self.planning_world.add_object(self.blue_cube) + result = self.planner.plan_screw(self.target_pose, self.robot.get_qpos()) + self.assertFalse(result["status"] == "Success") + + def test_attach_detach_obj(self): + # the red cube should be at around [0.7, 0, 0.06] before picking up + self.assertTrue(np.allclose(self.red_cube.get_pose().p, [0.7, 0, 0.06])) + + result = self.planner.plan_pose(self.target_pose, self.robot.get_qpos()) + self.assertTrue(result["status"] == "Success") + self.follow_path(result) + + self.assertFalse(self.planning_world.is_object_attached(self.red_cube)) + ee_idx = self.planner.user_link_names.index("panda_hand") + self.planning_world.attach_object(self.red_cube, self.robot, ee_idx) + self.assertTrue(self.planning_world.is_object_attached(self.red_cube)) + + lift_pose = self.target_pose + lift_pose.p[2] += 0.2 + result = self.planner.plan_pose(lift_pose, self.robot.get_qpos()) + self.follow_path(result) + + # the red cube should be at around [0.7, 0, 0.26] after picking up + red_cube_name = convert_object_name(self.red_cube) + red_cube_attach = self.planning_world.get_object(red_cube_name) + expected_pose = Pose(p=[0.7, 0, 0.26]) + self.assertTrue(expected_pose.distance(red_cube_attach.pose) < 0.1) + + def test_allowed_collision(self): + # initially should fail + self.planning_world.add_object(self.blue_cube) + result = self.planner.plan_screw(self.target_pose, self.robot.get_qpos()) + self.assertFalse(result["status"] == "Success") + + # after allowing collision, should succeed + # get the link entity that is robot's end effector + + sapien_links = self.robot.get_links() + sapien_link_names = [link.get_name() for link in sapien_links] + sapien_eef = sapien_links[sapien_link_names.index("panda_hand")] + self.planning_world.set_allowed_collision(self.blue_cube, sapien_eef, True) + self.planning_world.remove_object(self.blue_cube) + result = self.planner.plan_screw(self.target_pose, self.robot.get_qpos()) + self.assertTrue(result["status"] == "Success") + + """ + + +if __name__ == "__main__": + unittest.main()