From 066bc61b3d9a2f44f6a565f7c1734880d9ff801b Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Wed, 17 Apr 2024 10:37:50 -0700 Subject: [PATCH 01/20] allows for mutable p inside a mplib.Pose --- pybind/utils/pose.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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)) From 4802b697c16a6f7313ba74fd1a098216fbbd1b76 Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Wed, 17 Apr 2024 10:40:01 -0700 Subject: [PATCH 02/20] get the simple demo working --- mplib/examples/demo.py | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) 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 From c9218efcc2995c324830d45fd39adafa1556f03f Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Thu, 18 Apr 2024 14:37:03 -0700 Subject: [PATCH 03/20] disable ground collision --- mplib/sapien_utils/conversion.py | 10 +++++++++- src/core/articulated_model.cpp | 1 + 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 54ab8415..0cc52d9f 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -128,7 +128,7 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None for articulation in self._sim_scene.get_all_articulations(): if art := self.get_articulation(convert_object_name(articulation)): # set_qpos to update poses - art.set_qpos(articulation.qpos) # type: ignore + art.set_qpos(articulation.qpos, full=True) # type: ignore else: raise RuntimeError( f"Articulation {articulation.name} not found in PlanningWorld! " @@ -402,6 +402,14 @@ def __init__( self.joint_acc_limits = joint_acc_limits self.move_group_link_id = self.link_name_2_idx[self.move_group] + # finally disable the collision between the root of the robot and ground_1 + root_link_name = self.user_link_names[0] + for other_link in self.planning_world.get_object_names(): + if other_link.startswith("ground_"): + self.planning_world.get_allowed_collision_matrix().set_entry( + other_link, root_link_name, True + ) + assert ( len(self.joint_vel_limits) == len(self.joint_acc_limits) 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; } From 91631bc9f97e1f145e74d518054ca8f3da9637e7 Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Thu, 18 Apr 2024 15:34:01 -0700 Subject: [PATCH 04/20] added convenience function to attach objects --- include/mplib/planning_world.h | 16 ++++++ mplib/examples/collision_avoidance.py | 76 +++++++++++++++------------ mplib/examples/detect_collision.py | 2 +- pybind/pybind_planning_world.cpp | 3 ++ src/planning_world.cpp | 32 ++++++++++- 5 files changed, 92 insertions(+), 37 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 7fdcaadd..997ce5eb 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -313,6 +313,13 @@ class PlanningWorldTpl { void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, const std::string &art_name, int link_id, const Pose &pose); + /** + * Attaches the object the articulation is currently touching + * + * @param art_name name of the planned articulation to attach to + */ + void attachCurrentlyTouchingObject(const std::string &art_name); + /** * Attaches given sphere to specified link of articulation (auto touch_links) * @@ -358,6 +365,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; diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py index eddf5b7f..655112d3 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,63 @@ 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]) + 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.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") - - 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): + # self.planner.IK( + # goal_pose=Pose([0.7, 0, 0.32], [0, 1, 0, 0]), + # start_qpos=np.array([0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]), + # verbose=True, + # ) + # # self.setup_planner( + # # urdf_path="./data/panda/panda.urdf", srdf_path="./data/panda/panda.srdf" + # # ) + # # self.planner.robot.set_qpos(init_qpos, full=True) + # # for obj in self.planner.robot.get_fcl_model().get_collision_objects(): + # # print(obj.name, obj.pose) + # # for i, link in enumerate( + # # self.planner.robot.get_pinocchio_model().get_link_names() + # # ): + # # print(link, self.planner.robot.get_pinocchio_model().get_link_pose(i)) + # # for obj in self.planner2.robot.get_fcl_model().get_collision_objects(): + # # print(obj.name, obj.pose) + # # for i, link in enumerate( + # # self.planner2.robot.get_pinocchio_model().get_link_names() + # # ): + # # print(link, self.planner2.robot.get_pinocchio_model().get_link_pose(i)) + # exit(0) + + 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 +103,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] - ) + art_name = self.planner.planning_world.get_articulation_names()[0] + self.planner.planning_world.attach_currently_touching_object(art_name) # 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(False) assert ret, "object is not attached" self.move_to_pose(delivery_pose, with_screw) @@ -126,4 +132,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/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/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 589c1b0e..77b31fda 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -112,6 +112,8 @@ void build_pyplanning_world(py::module &pymp) { &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)) + .def("attach_currently_touching_object", + &PlanningWorld::attachCurrentlyTouchingObject, py::arg("art_name")) .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 +125,7 @@ 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) .def("print_attached_body_pose", &PlanningWorld::printAttachedBodyPose, DOC(mplib, PlanningWorldTpl, printAttachedBodyPose)) diff --git a/src/planning_world.cpp b/src/planning_world.cpp index b9f9fe1b..f9793c39 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -165,11 +165,12 @@ void PlanningWorldTpl::attachObject(const std::string &name, // 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 : collisions) { if (collision.link_name1 == name) touch_links.push_back(collision.link_name2); else if (collision.link_name2 == name) touch_links.push_back(collision.link_name1); + } body->setTouchLinks(touch_links); // Update acm_ to allow collision between name and touch_links acm_->setEntry(name, touch_links, true); @@ -197,6 +198,25 @@ void PlanningWorldTpl::attachObject(const std::string &name, attachObject(name, art_name, link_id, pose); } +template +void PlanningWorldTpl::attachCurrentlyTouchingObject(const std::string &art_name) { + // do an environment collision + auto collisions = checkRobotCollision(CollisionRequest()); + const auto &link_names = + planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkNames(); + for (const auto &collision : collisions) { + if (collision.object_name1 == art_name) { + // get the index of the link inside link_names, which is a vector + int link_id = std::distance( + link_names.begin(), + std::find(link_names.begin(), link_names.end(), collision.link_name1)); + if (link_id == static_cast(link_names.size())) continue; + // attach the touching object + attachObject(collision.object_name2, art_name, link_id); + } + } +} + template void PlanningWorldTpl::attachSphere(S radius, const std::string &art_name, int link_id, const Pose &pose) { @@ -247,6 +267,16 @@ bool PlanningWorldTpl::detachObject(const std::string &name, bool also_remove return true; } +template +bool PlanningWorldTpl::detachAllObjects(bool also_remove) { + bool rtn = !attached_body_map_.empty(); + std::vector names; + for (auto it = attached_body_map_.begin(); it != attached_body_map_.end(); ++it) + names.push_back(it->first); + for (const auto &name : names) detachObject(name, also_remove); + return rtn; +} + template void PlanningWorldTpl::printAttachedBodyPose() const { for (const auto &[name, body] : attached_body_map_) From 3e84c4b38027af58fa7e13ffbc642039bcde095c Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Thu, 18 Apr 2024 17:23:06 -0700 Subject: [PATCH 05/20] modified all examples as well as added documentation to planning world --- mplib/examples/collision_avoidance.py | 2 +- mplib/examples/constrained_planning.py | 9 ++++---- mplib/examples/moving_robot.py | 31 +++++++++++++------------- mplib/examples/two_stage_motion.py | 16 ++++++------- pybind/pybind_planning_world.cpp | 7 ++++-- 5 files changed, 35 insertions(+), 30 deletions(-) diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py index 655112d3..1d2d05a4 100644 --- a/mplib/examples/collision_avoidance.py +++ b/mplib/examples/collision_avoidance.py @@ -117,7 +117,7 @@ def demo(self, with_screw=True, use_attach=True): self.open_gripper() delivery_pose.p[2] += 0.12 if use_attach: - ret = self.planner.planning_world.detach_all_objects(False) + ret = self.planner.planning_world.detach_all_objects() assert ret, "object is not attached" self.move_to_pose(delivery_pose, with_screw) 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/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/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 77b31fda..d0a68c52 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -113,7 +113,8 @@ void build_pyplanning_world(py::module &pymp) { py::arg("name"), py::arg("p_geom"), py::arg("art_name"), py::arg("link_id"), py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachObject, 6)) .def("attach_currently_touching_object", - &PlanningWorld::attachCurrentlyTouchingObject, py::arg("art_name")) + &PlanningWorld::attachCurrentlyTouchingObject, py::arg("art_name"), + DOC(mplib, PlanningWorldTpl, attachCurrentlyTouchingObject)) .def("attach_sphere", &PlanningWorld::attachSphere, py::arg("radius"), py::arg("art_name"), py::arg("link_id"), py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachSphere)) @@ -125,7 +126,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) + .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)) From c433597bcfe3c22a832fc46494e33ceb37a03ef6 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 14:29:54 -0700 Subject: [PATCH 06/20] Remove attachCurrentlyTouchingObject method --- include/mplib/planning_world.h | 11 ++--------- mplib/pymp/__init__.pyi | 8 ++++++++ pybind/docstring/planning_world.h | 8 ++++++++ pybind/pybind_planning_world.cpp | 3 --- src/planning_world.cpp | 30 ++++-------------------------- 5 files changed, 22 insertions(+), 38 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 997ce5eb..cd722239 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -313,13 +313,6 @@ class PlanningWorldTpl { void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, const std::string &art_name, int link_id, const Pose &pose); - /** - * Attaches the object the articulation is currently touching - * - * @param art_name name of the planned articulation to attach to - */ - void attachCurrentlyTouchingObject(const std::string &art_name); - /** * Attaches given sphere to specified link of articulation (auto touch_links) * @@ -366,8 +359,8 @@ 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. + * 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 diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 4635178a..823dab10 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -572,6 +572,14 @@ 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 diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index afb70104..2b3229bb 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -219,6 +219,14 @@ 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 diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index d0a68c52..531572c7 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -112,9 +112,6 @@ void build_pyplanning_world(py::module &pymp) { &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)) - .def("attach_currently_touching_object", - &PlanningWorld::attachCurrentlyTouchingObject, py::arg("art_name"), - DOC(mplib, PlanningWorldTpl, attachCurrentlyTouchingObject)) .def("attach_sphere", &PlanningWorld::attachSphere, py::arg("radius"), py::arg("art_name"), py::arg("link_id"), py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachSphere)) diff --git a/src/planning_world.cpp b/src/planning_world.cpp index f9793c39..4e88a7f5 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -164,13 +164,11 @@ 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) touch_links.push_back(collision.link_name1); - } body->setTouchLinks(touch_links); // Update acm_ to allow collision between name and touch_links acm_->setEntry(name, touch_links, true); @@ -198,25 +196,6 @@ void PlanningWorldTpl::attachObject(const std::string &name, attachObject(name, art_name, link_id, pose); } -template -void PlanningWorldTpl::attachCurrentlyTouchingObject(const std::string &art_name) { - // do an environment collision - auto collisions = checkRobotCollision(CollisionRequest()); - const auto &link_names = - planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkNames(); - for (const auto &collision : collisions) { - if (collision.object_name1 == art_name) { - // get the index of the link inside link_names, which is a vector - int link_id = std::distance( - link_names.begin(), - std::find(link_names.begin(), link_names.end(), collision.link_name1)); - if (link_id == static_cast(link_names.size())) continue; - // attach the touching object - attachObject(collision.object_name2, art_name, link_id); - } - } -} - template void PlanningWorldTpl::attachSphere(S radius, const std::string &art_name, int link_id, const Pose &pose) { @@ -269,12 +248,11 @@ bool PlanningWorldTpl::detachObject(const std::string &name, bool also_remove template bool PlanningWorldTpl::detachAllObjects(bool also_remove) { - bool rtn = !attached_body_map_.empty(); + if (attached_body_map_.empty()) return false; std::vector names; - for (auto it = attached_body_map_.begin(); it != attached_body_map_.end(); ++it) - names.push_back(it->first); + for (const auto &pair : attached_body_map_) names.push_back(pair.first); for (const auto &name : names) detachObject(name, also_remove); - return rtn; + return true; } template From 693745a44ac568604c445c99d01dfa4429bacfa6 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 15:59:53 -0700 Subject: [PATCH 07/20] Surround acm_/default_entries_ with backticks --- .../collision_detection/collision_matrix.h | 3 +- include/mplib/planning_world.h | 24 ++++++++-------- mplib/pymp/__init__.pyi | 28 +++++++++---------- mplib/pymp/collision_detection/__init__.pyi | 2 +- .../collision_detection/collision_matrix.h | 2 +- pybind/docstring/planning_world.h | 28 +++++++++---------- 6 files changed, 43 insertions(+), 44 deletions(-) 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 cd722239..af5365a9 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,7 +284,7 @@ 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 @@ -300,7 +300,7 @@ 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. * Automatically sets touch_links as the name of self links * that collide with the object in the current state (auto touch_links). * @@ -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 @@ -360,7 +360,7 @@ class PlanningWorldTpl { /** * Detaches all attached objects. - * Updates acm_ to disallow collision between the object and touch_links. + * 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 diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 823dab10..28ffa06d 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 @@ -500,7 +500,7 @@ 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 @@ -521,7 +521,7 @@ 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). @@ -574,16 +574,16 @@ class PlanningWorld: """ 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. + 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 @@ -722,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 @@ -731,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/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 2b3229bb..a864ecfc 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,7 +163,7 @@ 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 @@ -176,7 +176,7 @@ 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). @@ -221,16 +221,16 @@ articulation-attach collision, attach-attach collision) static const char *__doc_mplib_PlanningWorldTpl_detachAllObjects = R"doc( -Detaches all attached objects. Updates acm_ to disallow collision between the -object and touch_links. +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 @@ -361,7 +361,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 @@ -370,7 +370,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 From ab696c02b3173362d73026a98f34342fff465ad8 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 16:44:43 -0700 Subject: [PATCH 08/20] Rename p_geom to obj_geom in PlanningWorld::attachObject --- include/mplib/planning_world.h | 8 ++++---- mplib/pymp/__init__.pyi | 8 ++++---- pybind/docstring/planning_world.h | 4 ++-- pybind/pybind_planning_world.cpp | 9 +++++---- src/planning_world.cpp | 8 ++++---- 5 files changed, 19 insertions(+), 18 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index af5365a9..008f339a 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -287,13 +287,13 @@ class PlanningWorldTpl { * 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); @@ -305,12 +305,12 @@ class PlanningWorldTpl { * 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); /** diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 28ffa06d..94bbc401 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -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, @@ -503,7 +503,7 @@ class PlanningWorld: 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, @@ -526,7 +526,7 @@ class PlanningWorld: 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) diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index a864ecfc..9fdfb3b0 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -166,7 +166,7 @@ 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 :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) @@ -181,7 +181,7 @@ 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"; diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 531572c7..11c3ad92 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)) diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 4e88a7f5..3b7283d9 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -177,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); } From 4ac822308d0e22af8649402fd795f501a4498c91 Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Fri, 19 Apr 2024 17:18:04 -0700 Subject: [PATCH 09/20] moved up panda so it's not touching the ground --- data/panda/panda_on_rail.urdf | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 @@ - + From 814837010e877c576535a2dc3fe2d615aeff8000 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 17:02:35 -0700 Subject: [PATCH 10/20] Override SapienPlanningWorld.attach/detach_object to accept SAPIEN objects --- mplib/sapien_utils/conversion.py | 95 ++++++++++++++++++++++++++++++++ 1 file changed, 95 insertions(+) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 0cc52d9f..8f7bab02 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -29,6 +29,7 @@ Box, BVHModel, Capsule, + CollisionGeometry, CollisionObject, Convex, Cylinder, @@ -287,6 +288,100 @@ def _get_collision_obj( "(The scene might have changed since last update)" ) + 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 + .. 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 + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + return super().detach_object(obj, also_remove=also_remove) + @staticmethod def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: """ From 52adf0d91f4312d764f7bafa89b1b0c9b5cd3e8d Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Fri, 19 Apr 2024 17:20:03 -0700 Subject: [PATCH 11/20] remove commented out test code --- mplib/examples/collision_avoidance.py | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py index 1d2d05a4..695817ea 100644 --- a/mplib/examples/collision_avoidance.py +++ b/mplib/examples/collision_avoidance.py @@ -58,28 +58,6 @@ def __init__(self): planning_world = SapienPlanningWorld(self.scene, [self.robot]) self.planner = SapienPlanner(planning_world, "panda_hand") - # self.planner.IK( - # goal_pose=Pose([0.7, 0, 0.32], [0, 1, 0, 0]), - # start_qpos=np.array([0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]), - # verbose=True, - # ) - # # self.setup_planner( - # # urdf_path="./data/panda/panda.urdf", srdf_path="./data/panda/panda.srdf" - # # ) - # # self.planner.robot.set_qpos(init_qpos, full=True) - # # for obj in self.planner.robot.get_fcl_model().get_collision_objects(): - # # print(obj.name, obj.pose) - # # for i, link in enumerate( - # # self.planner.robot.get_pinocchio_model().get_link_names() - # # ): - # # print(link, self.planner.robot.get_pinocchio_model().get_link_pose(i)) - # # for obj in self.planner2.robot.get_fcl_model().get_collision_objects(): - # # print(obj.name, obj.pose) - # # for i, link in enumerate( - # # self.planner2.robot.get_pinocchio_model().get_link_names() - # # ): - # # print(link, self.planner2.robot.get_pinocchio_model().get_link_pose(i)) - # exit(0) def demo(self, with_screw=True, use_attach=True): """ From fd53ea6d7ad31e43d2f0f22a80af09aef7ba689c Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 17:42:38 -0700 Subject: [PATCH 12/20] Override SapienPlanningWorld.attach_sphere/box/mesh to accept SAPIEN objects --- mplib/sapien_utils/conversion.py | 235 +++++++++++++++++++++++-------- 1 file changed, 177 insertions(+), 58 deletions(-) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 8f7bab02..b58a3d84 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -1,6 +1,6 @@ from __future__ import annotations -from typing import Optional, Sequence +from typing import Literal, Optional, Sequence import numpy as np from sapien import Entity, Scene @@ -288,13 +288,77 @@ def _get_collision_obj( "(The scene might have changed since last update)" ) + @staticmethod + def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: + """ + Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCLObject. + All shapes in the returned FCLObject are already set at their world poses. + + :param comp: a SAPIEN physx.PhysxRigidBaseComponent. + :return: an FCLObject containing all collision shapes in the Physx component. + If the component has no collision shapes, return ``None``. + """ + shapes: list[CollisionObject] = [] + shape_poses: list[Pose] = [] + for shape in comp.collision_shapes: + shape_poses.append(shape.local_pose) # type: ignore + + if isinstance(shape, PhysxCollisionShapeBox): + c_geom = Box(side=shape.half_size * 2) + elif isinstance(shape, PhysxCollisionShapeCapsule): + c_geom = Capsule(radius=shape.radius, lz=shape.half_length * 2) + # NOTE: physx Capsule has x-axis along capsule height + # FCL Capsule has z-axis along capsule height + shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) + elif isinstance(shape, PhysxCollisionShapeConvexMesh): + assert np.allclose( + shape.scale, 1.0 + ), f"Not unit scale {shape.scale}, need to rescale vertices?" + c_geom = Convex(vertices=shape.vertices, faces=shape.triangles) + elif isinstance(shape, PhysxCollisionShapeCylinder): + c_geom = Cylinder(radius=shape.radius, lz=shape.half_length * 2) + # NOTE: physx Cylinder has x-axis along cylinder height + # FCL Cylinder has z-axis along cylinder height + shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) + elif isinstance(shape, PhysxCollisionShapePlane): + # PhysxCollisionShapePlane are actually a halfspace + # https://nvidia-omniverse.github.io/PhysX/physx/5.3.1/docs/Geometry.html#planes + # PxPlane's Pose determines its normal and offert (normal is +x) + n = shape_poses[-1].to_transformation_matrix()[:3, 0] + d = n.dot(shape_poses[-1].p) + c_geom = Halfspace(n=n, d=d) + shape_poses[-1] = Pose() + elif isinstance(shape, PhysxCollisionShapeSphere): + c_geom = Sphere(radius=shape.radius) + elif isinstance(shape, PhysxCollisionShapeTriangleMesh): + c_geom = BVHModel() + c_geom.begin_model() + c_geom.add_sub_model(vertices=shape.vertices, faces=shape.triangles) # type: ignore + c_geom.end_model() + else: + raise TypeError(f"Unknown shape type: {type(shape)}") + shapes.append(CollisionObject(c_geom)) + + if len(shapes) == 0: + return None + + return FCLObject( + comp.name + if isinstance(comp, PhysxArticulationLinkComponent) + else convert_object_name(comp.entity), + comp.entity.pose, # type: ignore + shapes, + shape_poses, + ) + + # ----- Overloaded PlanningWorld methods to accept SAPIEN objects ----- # 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: @@ -382,68 +446,123 @@ def detach_object(self, obj: Entity | str, also_remove: bool = False) -> bool: obj = convert_object_name(obj) return super().detach_object(obj, also_remove=also_remove) - @staticmethod - def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: + def attach_sphere( # type: ignore + self, + radius: float, + articulation: PhysxArticulation | str, + link: PhysxArticulationLinkComponent | int, + pose: Pose, + ) -> None: """ - Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCLObject. - All shapes in the returned FCLObject are already set at their world poses. + Attaches given sphere to specified link of articulation (auto touch_links) - :param comp: a SAPIEN physx.PhysxRigidBaseComponent. - :return: an FCLObject containing all collision shapes in the Physx component. - If the component has no collision shapes, return ``None``. + :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 + .. raw:: html +
""" - shapes: list[CollisionObject] = [] - shape_poses: list[Pose] = [] - for shape in comp.collision_shapes: - shape_poses.append(shape.local_pose) # type: ignore + 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) - if isinstance(shape, PhysxCollisionShapeBox): - c_geom = Box(side=shape.half_size * 2) - elif isinstance(shape, PhysxCollisionShapeCapsule): - c_geom = Capsule(radius=shape.radius, lz=shape.half_length * 2) - # NOTE: physx Capsule has x-axis along capsule height - # FCL Capsule has z-axis along capsule height - shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) - elif isinstance(shape, PhysxCollisionShapeConvexMesh): - assert np.allclose( - shape.scale, 1.0 - ), f"Not unit scale {shape.scale}, need to rescale vertices?" - c_geom = Convex(vertices=shape.vertices, faces=shape.triangles) - elif isinstance(shape, PhysxCollisionShapeCylinder): - c_geom = Cylinder(radius=shape.radius, lz=shape.half_length * 2) - # NOTE: physx Cylinder has x-axis along cylinder height - # FCL Cylinder has z-axis along cylinder height - shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) - elif isinstance(shape, PhysxCollisionShapePlane): - # PhysxCollisionShapePlane are actually a halfspace - # https://nvidia-omniverse.github.io/PhysX/physx/5.3.1/docs/Geometry.html#planes - # PxPlane's Pose determines its normal and offert (normal is +x) - n = shape_poses[-1].to_transformation_matrix()[:3, 0] - d = n.dot(shape_poses[-1].p) - c_geom = Halfspace(n=n, d=d) - shape_poses[-1] = Pose() - elif isinstance(shape, PhysxCollisionShapeSphere): - c_geom = Sphere(radius=shape.radius) - elif isinstance(shape, PhysxCollisionShapeTriangleMesh): - c_geom = BVHModel() - c_geom.begin_model() - c_geom.add_sub_model(vertices=shape.vertices, faces=shape.triangles) # type: ignore - c_geom.end_model() - else: - raise TypeError(f"Unknown shape type: {type(shape)}") - shapes.append(CollisionObject(c_geom)) + 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) - if len(shapes) == 0: - return None + :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) - return FCLObject( - comp.name - if isinstance(comp, PhysxArticulationLinkComponent) - else convert_object_name(comp.entity), - comp.entity.pose, # type: ignore - shapes, - shape_poses, - ) + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_box() + + method + .. automethod:: mplib.PlanningWorld.attach_box + .. 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 + .. 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) class SapienPlanner(Planner): From 382f663a69088b2a7a3bcfd9f8e958dbb88b9cf8 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 17:58:23 -0700 Subject: [PATCH 13/20] Override 3 more SapienPlanningWorld methods to accept SAPIEN objects * is_articulation_planned * set_articulation_planned * is_object_attached --- mplib/sapien_utils/conversion.py | 71 ++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index b58a3d84..bd4b6070 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -352,6 +352,77 @@ def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: ) # ----- 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 + .. 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 + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + super().set_articulation_planned(articulation, planned) + + 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 + .. 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, From d8ee50eb60d794d30f34300ed67846928c30075d Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Fri, 19 Apr 2024 18:05:05 -0700 Subject: [PATCH 14/20] output warning for initial colliding bodies during sapien conversion --- mplib/examples/collision_avoidance.py | 12 ++++++++---- mplib/examples/demo_setup.py | 1 - mplib/sapien_utils/conversion.py | 15 +++++++++------ 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py index 695817ea..c2622455 100644 --- a/mplib/examples/collision_avoidance.py +++ b/mplib/examples/collision_avoidance.py @@ -39,8 +39,8 @@ def __init__(self): 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]) - red_cube = builder.build(name="red_cube") - red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) + 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() @@ -58,6 +58,10 @@ def __init__(self): 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 demo(self, with_screw=True, use_attach=True): """ @@ -81,8 +85,8 @@ def demo(self, with_screw=True, use_attach=True): # use_attach ankor if use_attach: - art_name = self.planner.planning_world.get_articulation_names()[0] - self.planner.planning_world.attach_currently_touching_object(art_name) + 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 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/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 8f7bab02..8070c7b4 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -497,12 +497,15 @@ def __init__( self.joint_acc_limits = joint_acc_limits self.move_group_link_id = self.link_name_2_idx[self.move_group] - # finally disable the collision between the root of the robot and ground_1 - root_link_name = self.user_link_names[0] - for other_link in self.planning_world.get_object_names(): - if other_link.startswith("ground_"): - self.planning_world.get_allowed_collision_matrix().set_entry( - other_link, root_link_name, True + # 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: + print( + f"\033[93mRobot'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\033[0m" ) assert ( From 159897ec22de7cf03be9d98601deb5e3ed306e1c Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 18:12:31 -0700 Subject: [PATCH 15/20] Add no-index to all automethods --- mplib/sapien_utils/conversion.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 5550a2b2..840cb0d2 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -368,6 +368,7 @@ def is_articulation_planned(self, articulation: PhysxArticulation | str) -> bool method .. automethod:: mplib.PlanningWorld.is_articulation_planned + :no-index: .. raw:: html """ @@ -393,6 +394,7 @@ def set_articulation_planned( # type: ignore method .. automethod:: mplib.PlanningWorld.set_articulation_planned + :no-index: .. raw:: html """ @@ -416,6 +418,7 @@ def is_object_attached(self, obj: Entity | str) -> bool: # type: ignore method .. automethod:: mplib.PlanningWorld.is_object_attached + :no-index: .. raw:: html """ @@ -463,6 +466,7 @@ def attach_object( # type: ignore methods .. automethod:: mplib.PlanningWorld.attach_object + :no-index: .. raw:: html """ @@ -510,6 +514,7 @@ def detach_object(self, obj: Entity | str, also_remove: bool = False) -> bool: method .. automethod:: mplib.PlanningWorld.detach_object + :no-index: .. raw:: html """ @@ -541,6 +546,7 @@ def attach_sphere( # type: ignore method .. automethod:: mplib.PlanningWorld.attach_sphere + :no-index: .. raw:: html """ @@ -580,6 +586,7 @@ def attach_box( # type: ignore method .. automethod:: mplib.PlanningWorld.attach_box + :no-index: .. raw:: html """ @@ -621,6 +628,7 @@ def attach_mesh( # type: ignore method .. automethod:: mplib.PlanningWorld.attach_mesh + :no-index: .. raw:: html """ From cd58e01e885c1ca158fb52492fd24e83c43a737a Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Mon, 22 Apr 2024 10:38:04 -0700 Subject: [PATCH 16/20] more sapien overloaded functions --- include/mplib/planning_world.h | 12 ++++ mplib/sapien_utils/conversion.py | 107 ++++++++++++++++++++++++++++++- pybind/pybind_planning_world.cpp | 3 + 3 files changed, 120 insertions(+), 2 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 008f339a..ddf91496 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -384,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/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 840cb0d2..8e159b81 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -1,5 +1,6 @@ from __future__ import annotations +import warnings from typing import Literal, Optional, Sequence import numpy as np @@ -165,9 +166,11 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None ) > 0 ): - raise RuntimeError( + warnings.warn( f"Entity {entity.name} not found in PlanningWorld! " - "The scene might have changed since last update." + "The scene might have changed since last update. " + "Use PlanningWorld.add_object() to add the object.", + stacklevel=2, ) def check_collision_between( @@ -402,6 +405,78 @@ def set_articulation_planned( # type: ignore 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): + obj = self.convert_physx_component(obj) + if obj is not None: + 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 @@ -643,6 +718,34 @@ def attach_mesh( # type: ignore ) super().attach_mesh(mesh_path, articulation, link, pose, convex=convex) + def set_allowed_collision( + self, obj1: Entity | str, obj2: Entity | 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) + if isinstance(obj2, Entity): + obj2 = convert_object_name(obj2) + super().set_allowed_collision(obj1, obj2, allowed) + class SapienPlanner(Planner): def __init__( diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 11c3ad92..a0684b30 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -137,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)) From ab65bdb67a3978dd43c22126d9eb042f31bda9db Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Mon, 22 Apr 2024 10:49:43 -0700 Subject: [PATCH 17/20] warning color --- mplib/sapien_utils/conversion.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 8e159b81..9f7192fe 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -46,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: @@ -167,9 +170,9 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None > 0 ): warnings.warn( - f"Entity {entity.name} not found in PlanningWorld! " + 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.", + "Use PlanningWorld.add_object() to add the object." + RESET_COLOR, stacklevel=2, ) @@ -802,11 +805,12 @@ def __init__( collisions = self.planning_world.check_robot_collision() if len(collisions): for collision in collisions: - print( - f"\033[93mRobot's {collision.link_name1} collides with " + 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\033[0m" + f"to disable collisions if planning fails" + RESET_COLOR, + stacklevel=2, ) assert ( From 83a25f5dc4b7850dc6084e0f063ea4ad9751e1fb Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Mon, 22 Apr 2024 13:32:03 -0700 Subject: [PATCH 18/20] added tests and expanded the object types for set_allowed_collision --- mplib/sapien_utils/conversion.py | 20 +++- tests/test_pinocchio.py | 2 +- tests/test_sapien_conversion.py | 152 +++++++++++++++++++++++++++++++ 3 files changed, 170 insertions(+), 4 deletions(-) create mode 100644 tests/test_sapien_conversion.py diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 9f7192fe..30207c1e 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -453,8 +453,15 @@ def add_object(self, obj: FCLObject | Entity) -> None: """ if isinstance(obj, Entity): - obj = self.convert_physx_component(obj) - if obj is not None: + 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: @@ -722,7 +729,10 @@ def attach_mesh( # type: ignore super().attach_mesh(mesh_path, articulation, link, pose, convex=convex) def set_allowed_collision( - self, obj1: Entity | str, obj2: Entity | str, allowed: bool + self, + obj1: Entity | PhysxArticulationLinkComponent | str, + obj2: Entity | PhysxArticulationLinkComponent | str, + allowed: bool, ) -> None: """ Set allowed collision between two objects. @@ -745,8 +755,12 @@ def set_allowed_collision( """ 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) 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..adc1bb7a --- /dev/null +++ b/tests/test_sapien_conversion.py @@ -0,0 +1,152 @@ +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() From 8603aa9139827452d6eefb7a767a98a8dbf483d2 Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Mon, 22 Apr 2024 16:46:54 -0700 Subject: [PATCH 19/20] install sapien for test --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 397dcaf1..c05b5b22 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] From b23644145a9544c1c35bbba51f4cb3867c1c136e Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Tue, 23 Apr 2024 12:12:28 -0700 Subject: [PATCH 20/20] disable sapien tests for now. requires swiftshader in the docker container later --- pybind/docstring/planning_world.h | 8 ++++++++ tests/test_sapien_conversion.py | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 9fdfb3b0..10238055 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -287,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 diff --git a/tests/test_sapien_conversion.py b/tests/test_sapien_conversion.py index adc1bb7a..fb766466 100644 --- a/tests/test_sapien_conversion.py +++ b/tests/test_sapien_conversion.py @@ -22,6 +22,8 @@ class TestSapienConversion(unittest.TestCase): + """ + def setUp(self): self.engine = sapien.Engine() scene_config = sapien.SceneConfig() @@ -147,6 +149,8 @@ def test_allowed_collision(self): result = self.planner.plan_screw(self.target_pose, self.robot.get_qpos()) self.assertTrue(result["status"] == "Success") + """ + if __name__ == "__main__": unittest.main()