From 374bb377f7bcc6856353dafb1de4aebd47861560 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 12 Apr 2024 15:03:06 -0700 Subject: [PATCH 01/37] Remove name parameter from addNormalObject with FCLObject --- include/mplib/planning_world.h | 13 +++++-------- mplib/pymp/__init__.pyi | 11 ++++------- mplib/sapien_utils/conversion.py | 18 +++++++++++------- pybind/docstring/planning_world.h | 7 +++---- pybind/pybind_planning_world.cpp | 6 ++---- src/planning_world.cpp | 8 ++++---- 6 files changed, 29 insertions(+), 34 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 66bf05f7..6e714205 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -157,15 +157,13 @@ class PlanningWorldTpl { } /** - * Adds a normal object containing multiple collision objects (``FCLObjectPtr``) with - * given name to world + * Adds a normal object containing multiple collision objects (``FCLObjectPtr``) + * to world * - * @param name: name of the collision object - * @param collision_object: collision object to be added + * @param fcl_obj: FCLObject to be added */ - // TODO(merge): remove name - void addNormalObject(const std::string &name, const FCLObjectPtr &collision_object) { - normal_object_map_[name] = collision_object; + void addNormalObject(const FCLObjectPtr &fcl_obj) { + normal_object_map_[fcl_obj->name] = fcl_obj; } /** @@ -174,7 +172,6 @@ class PlanningWorldTpl { * @param name: name of the collision object * @param collision_object: collision object to be added */ - // TODO(merge): remove name void addNormalObject(const std::string &name, const CollisionObjectPtr &collision_object); diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 245adb0f..60f52f97 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -353,15 +353,12 @@ class PlanningWorld: :param planned: whether the articulation is being planned """ @typing.overload - def add_normal_object( - self, name: str, collision_object: collision_detection.fcl.FCLObject - ) -> None: + def add_normal_object(self, fcl_obj: collision_detection.fcl.FCLObject) -> None: """ - Adds a normal object containing multiple collision objects (``FCLObjectPtr``) - with given name to world + Adds a normal object containing multiple collision objects (``FCLObjectPtr``) to + world - :param name: name of the collision object - :param collision_object: collision object to be added + :param fcl_obj: FCLObject to be added """ @typing.overload def add_normal_object( diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index b3fdd22d..88a2d673 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -90,10 +90,8 @@ def __init__( ) # Convert collision shapes at current global pose - self.add_normal_object( - self.get_object_name(entity), - self.convert_physx_component(component), # type: ignore - ) + if (fcl_obj := self.convert_physx_component(component)) is not None: # type: ignore + self.add_normal_object(fcl_obj) def update_from_simulation(self, *, update_attached_object: bool = True) -> None: """ @@ -128,15 +126,21 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None elif fcl_obj := self.get_normal_object(object_name): # Overwrite the object self.add_normal_object( - object_name, FCLObject( object_name, entity.pose, # type: ignore fcl_obj.shapes, fcl_obj.shape_poses, - ), + ) ) - else: + elif ( + len( + entity.find_component_by_type( + PhysxRigidBaseComponent + ).collision_shapes # type: ignore + ) + > 0 + ): raise RuntimeError( f"Entity {entity.name} not found in PlanningWorld! " "The scene might have changed since last update." diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 24a893bf..25555d10 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -53,11 +53,10 @@ Adds an articulation (ArticulatedModelPtr) with given name to world static const char *__doc_mplib_PlanningWorldTpl_addNormalObject = R"doc( -Adds a normal object containing multiple collision objects (``FCLObjectPtr``) -with given name to world +Adds a normal object containing multiple collision objects (``FCLObjectPtr``) to +world -:param name: name of the collision object -:param collision_object: collision object to be added)doc"; +:param fcl_obj: FCLObject to be added)doc"; static const char *__doc_mplib_PlanningWorldTpl_addNormalObject_2 = R"doc( diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 525078a6..ed271faf 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -63,10 +63,8 @@ void build_pyplanning_world(py::module &pymp) { .def("has_normal_object", &PlanningWorld::hasNormalObject, py::arg("name"), DOC(mplib, PlanningWorldTpl, hasNormalObject)) .def("add_normal_object", - py::overload_cast( - &PlanningWorld::addNormalObject), - py::arg("name"), py::arg("collision_object"), - DOC(mplib, PlanningWorldTpl, addNormalObject)) + py::overload_cast(&PlanningWorld::addNormalObject), + py::arg("fcl_obj"), DOC(mplib, PlanningWorldTpl, addNormalObject)) .def("add_normal_object", py::overload_cast( &PlanningWorld::addNormalObject), diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 9c93ef85..a6a76e2b 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -92,10 +92,10 @@ std::vector PlanningWorldTpl::getNormalObjectNames() const { template void PlanningWorldTpl::addNormalObject(const std::string &name, const CollisionObjectPtr &collision_object) { - addNormalObject(name, std::make_shared( - name, Pose(collision_object->getTransform()), - std::vector {collision_object}, - std::vector> {Pose()})); + addNormalObject( + std::make_shared(name, Pose(collision_object->getTransform()), + std::vector {collision_object}, + std::vector> {Pose()})); } template From 92fd06473abd020106d3dfff6c0da0d7d296d5cc Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 12 Apr 2024 15:11:11 -0700 Subject: [PATCH 02/37] Remove name parameter from PlanningWorld::addArticulation --- include/mplib/planning_world.h | 7 ++----- mplib/pymp/__init__.pyi | 7 ++----- mplib/sapien_utils/conversion.py | 4 ++-- pybind/docstring/planning_world.h | 3 +-- pybind/pybind_planning_world.cpp | 5 ++--- src/planning_world.cpp | 8 +++----- 6 files changed, 12 insertions(+), 22 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 6e714205..ba6fa253 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -94,15 +94,12 @@ class PlanningWorldTpl { } /** - * Adds an articulation (ArticulatedModelPtr) with given name to world + * Adds an articulation (ArticulatedModelPtr) to world * - * @param name: name of the articulated model * @param model: articulated model to be added * @param planned: whether the articulation is being planned */ - // TODO(merge): remove name - void addArticulation(const std::string &name, const ArticulatedModelPtr &model, - bool planned = false); + void addArticulation(const ArticulatedModelPtr &model, bool planned = false); /** * Removes the articulation with given name if exists. Updates acm_ diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 60f52f97..a0b6a922 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -342,13 +342,10 @@ class PlanningWorld: :param normal_objects: list of collision objects that are not articulated :param normal_object_names: name of the normal objects """ - def add_articulation( - self, name: str, model: ArticulatedModel, planned: bool = False - ) -> None: + def add_articulation(self, model: ArticulatedModel, planned: bool = False) -> None: """ - Adds an articulation (ArticulatedModelPtr) with given name to world + Adds an articulation (ArticulatedModelPtr) to world - :param name: name of the articulated model :param model: articulated model to be added :param planned: whether the articulation is being planned """ diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 88a2d673..60dcb7f0 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -76,8 +76,8 @@ def __init__( verbose=False, ) articulated_model.set_qpos(articulation.qpos) # update qpos # type: ignore - - self.add_articulation(self.get_object_name(articulation), articulated_model) + articulated_model.name = self.get_object_name(articulation) + self.add_articulation(articulated_model) for articulation in planned_articulations: self.set_articulation_planned(self.get_object_name(articulation), True) diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 25555d10..3af04360 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -45,9 +45,8 @@ Constructs a PlanningWorld with given (planned) articulations and normal objects static const char *__doc_mplib_PlanningWorldTpl_addArticulation = R"doc( -Adds an articulation (ArticulatedModelPtr) with given name to world +Adds an articulation (ArticulatedModelPtr) to world -:param name: name of the articulated model :param model: articulated model to be added :param planned: whether the articulation is being planned)doc"; diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index ed271faf..f3a34264 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -45,9 +45,8 @@ void build_pyplanning_world(py::module &pymp) { DOC(mplib, PlanningWorldTpl, getArticulation)) .def("has_articulation", &PlanningWorld::hasArticulation, py::arg("name"), DOC(mplib, PlanningWorldTpl, hasArticulation)) - .def("add_articulation", &PlanningWorld::addArticulation, py::arg("name"), - py::arg("model"), py::arg("planned") = false, - DOC(mplib, PlanningWorldTpl, addArticulation)) + .def("add_articulation", &PlanningWorld::addArticulation, py::arg("model"), + py::arg("planned") = false, DOC(mplib, PlanningWorldTpl, addArticulation)) .def("remove_articulation", &PlanningWorld::removeArticulation, py::arg("name"), DOC(mplib, PlanningWorldTpl, removeArticulation)) .def("is_articulation_planned", &PlanningWorld::isArticulationPlanned, diff --git a/src/planning_world.cpp b/src/planning_world.cpp index a6a76e2b..aacc40b3 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -51,12 +51,10 @@ std::vector> PlanningWorldTpl::getPlannedArticulati } template -void PlanningWorldTpl::addArticulation(const std::string &name, - const ArticulatedModelPtr &model, +void PlanningWorldTpl::addArticulation(const ArticulatedModelPtr &model, bool planned) { - model->setName(name); - articulation_map_[name] = model; - setArticulationPlanned(name, planned); + articulation_map_[model->getName()] = model; + setArticulationPlanned(model->getName(), planned); } template From c83c5c954417fb8b43eb6a421ea639dcc279fb9d Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 12 Apr 2024 15:43:28 -0700 Subject: [PATCH 03/37] Use URDF robot name attribute to set ArticulatedModel name * Remove ArticulatedModel::setName * Add FCLModel::getName --- include/mplib/collision_detection/fcl/fcl_model.h | 9 +++++++++ include/mplib/core/articulated_model.h | 7 ------- mplib/pymp/__init__.pyi | 8 -------- mplib/pymp/collision_detection/fcl.pyi | 11 +++++++++++ pybind/collision_detection/fcl/pybind_fcl_model.cpp | 5 +++++ pybind/core/pybind_articulated_model.cpp | 6 ++---- pybind/docstring/collision_detection/fcl/fcl_model.h | 9 +++++++++ pybind/docstring/core/articulated_model.h | 6 ------ src/collision_detection/fcl/fcl_model.cpp | 1 + src/core/articulated_model.cpp | 2 ++ src/planning_world.cpp | 1 - 11 files changed, 39 insertions(+), 26 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 71759167..6997406b 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -65,6 +65,13 @@ class FCLModelTpl { const std::vector>> &collision_links, bool verbose = false); + /** + * Get name of the articulated model. + * + * @return: name of the articulated model + */ + const std::string &getName() const { return name_; } + /** * Get the collision objects of the FCL model. * @@ -153,6 +160,8 @@ class FCLModelTpl { void dfsParseTree(const urdf::LinkConstSharedPtr &link, const std::string &parent_link_name); + std::string name_; + urdf::ModelInterfaceSharedPtr urdf_model_; std::string package_dir_; bool use_convex_ {}; diff --git a/include/mplib/core/articulated_model.h b/include/mplib/core/articulated_model.h index a689d8df..e6a5a8ce 100644 --- a/include/mplib/core/articulated_model.h +++ b/include/mplib/core/articulated_model.h @@ -82,13 +82,6 @@ class ArticulatedModelTpl { */ const std::string &getName() const { return name_; } - /** - * Set name of the articulated model. - * - * @param: name of the articulated model - */ - void setName(const std::string &name) { name_ = name; } - /** * Get the underlying Pinocchio model. * diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index a0b6a922..c7b4e656 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -174,12 +174,6 @@ class ArticulatedModel: :param end_effectors: list of links extending to the end effector """ - def set_name(self, name: str) -> None: - """ - Set name of the articulated model. - - @param: name of the articulated model - """ def set_pose(self, pose: Pose) -> None: """ Set the base pose of the robot and update all collision links in the @@ -219,8 +213,6 @@ class ArticulatedModel: """ Name of the articulated model """ - @name.setter - def name(self, arg1: str) -> None: ... @property def pose(self) -> Pose: """ diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index c66336d9..c53f0911 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -611,6 +611,12 @@ class FCLModel: objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled collision pairs """ + def get_name(self) -> str: + """ + Get name of the articulated model. + + :return: name of the articulated model + """ def remove_collision_pairs_from_srdf(self, srdf_filename: str) -> None: """ Remove collision pairs from SRDF file. @@ -630,6 +636,11 @@ class FCLModel: :param link_poses: list of link poses in the order of the link order """ + @property + def name(self) -> str: + """ + Name of the fcl model + """ class FCLObject: """ diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index e08fa778..fff2d6b8 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -42,6 +42,11 @@ void build_pyfcl_model(py::module &m) { py::arg("verbose") = false, DOC(mplib, collision_detection, fcl, FCLModelTpl, createFromURDFString)) + .def_property_readonly("name", &FCLModel::getName, + DOC(mplib, collision_detection, fcl, FCLModelTpl, name)) + .def("get_name", &FCLModel::getName, + DOC(mplib, collision_detection, fcl, FCLModelTpl, getName)) + .def("get_collision_objects", &FCLModel::getCollisionObjects, DOC(mplib, collision_detection, fcl, FCLModelTpl, getCollisionObjects)) .def("get_collision_link_names", &FCLModel::getCollisionLinkNames, diff --git a/pybind/core/pybind_articulated_model.cpp b/pybind/core/pybind_articulated_model.cpp index e008fec3..ef997638 100644 --- a/pybind/core/pybind_articulated_model.cpp +++ b/pybind/core/pybind_articulated_model.cpp @@ -54,12 +54,10 @@ void build_pyarticulated_model(py::module &pymp) { py::arg("verbose") = false, DOC(mplib, ArticulatedModelTpl, createFromURDFString)) - .def_property("name", &ArticulatedModel::getName, &ArticulatedModel::setName, - DOC(mplib, ArticulatedModelTpl, name)) + .def_property_readonly("name", &ArticulatedModel::getName, + DOC(mplib, ArticulatedModelTpl, name)) .def("get_name", &ArticulatedModel::getName, DOC(mplib, ArticulatedModelTpl, getName)) - .def("set_name", &ArticulatedModel::setName, py::arg("name"), - DOC(mplib, ArticulatedModelTpl, setName)) .def("get_pinocchio_model", &ArticulatedModel::getPinocchioModel, DOC(mplib, ArticulatedModelTpl, getPinocchioModel)) diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index 2fd9f49f..65e5a88e 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -96,6 +96,12 @@ Get the collision pairs of the FCL model. objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled collision pairs)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_getName = +R"doc( +Get name of the articulated model. + +:return: name of the articulated model)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_getUserLinkNames = R"doc( )doc"; @@ -131,6 +137,9 @@ Update the collision objects of the FCL model. /* ----- Begin of custom docstring section ----- */ +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_name = +R"doc(Name of the fcl model)doc"; + /* ----- End of custom docstring section ----- */ #if defined(__GNUG__) diff --git a/pybind/docstring/core/articulated_model.h b/pybind/docstring/core/articulated_model.h index a71c3cec..eb89fe9f 100644 --- a/pybind/docstring/core/articulated_model.h +++ b/pybind/docstring/core/articulated_model.h @@ -149,12 +149,6 @@ EE1 --> EE2 --> ... --> EEn :param end_effectors: list of links extending to the end effector)doc"; -static const char *__doc_mplib_ArticulatedModelTpl_setName = -R"doc( -Set name of the articulated model. - -@param: name of the articulated model)doc"; - static const char *__doc_mplib_ArticulatedModelTpl_setQpos = R"doc( Set the current joint positions and update all collision links in the diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 194d8f28..2d8fec21 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -76,6 +76,7 @@ template void FCLModelTpl::init(const urdf::ModelInterfaceSharedPtr &urdf_model, const std::string &package_dir) { urdf_model_ = urdf_model; + name_ = urdf_model->getName(); package_dir_ = package_dir; if (not urdf_model_) throw std::invalid_argument("The XML stream does not contain a valid URDF model."); diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index a8a5129d..472165a9 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -26,6 +26,7 @@ ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, fcl_model_(std::make_shared>( urdf_filename, convex, verbose)), verbose_(verbose) { + name_ = fcl_model_->getName(); user_link_names_ = link_names.size() == 0 ? pinocchio_model_->getLinkNames(false) : link_names; user_joint_names_ = @@ -54,6 +55,7 @@ std::unique_ptr> ArticulatedModelTpl::createFromURDFSt auto fcl_model = articulation->fcl_model_ = collision_detection::FCLModelTpl::createFromURDFString( urdf_string, collision_links, verbose); + articulation->name_ = fcl_model->getName(); articulation->verbose_ = verbose; auto user_link_names = articulation->user_link_names_ = diff --git a/src/planning_world.cpp b/src/planning_world.cpp index aacc40b3..6025f3c2 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -27,7 +27,6 @@ PlanningWorldTpl::PlanningWorldTpl( "normal_objects and normal_object_names should have the same size"); // TODO(merge): remove articulation_names and normal_object_names for (size_t i = 0; i < articulations.size(); i++) { - articulations[i]->setName(articulation_names[i]); articulation_map_[articulation_names[i]] = articulations[i]; planned_articulation_map_[articulation_names[i]] = articulations[i]; } From 934490c57b361c9ac5289eefdf07758657880fd6 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 12 Apr 2024 15:53:45 -0700 Subject: [PATCH 04/37] Move convert_object_name as separate function --- mplib/sapien_utils/conversion.py | 42 ++++++++++++++--------------- mplib/sapien_utils/srdf_exporter.py | 4 ++- mplib/sapien_utils/urdf_exporter.py | 4 ++- 3 files changed, 27 insertions(+), 23 deletions(-) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 60dcb7f0..d51787b7 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -38,6 +38,23 @@ from .urdf_exporter import export_kinematic_chain_urdf +# TODO(merge): link names? +def convert_object_name(obj: PhysxArticulation | Entity) -> str: + """ + Constructs a unique name for the corresponding mplib object. + This is necessary because mplib objects assume unique names. + + :param obj: a SAPIEN object + :return: the unique mplib object name + """ + if isinstance(obj, PhysxArticulation): + return f"{obj.name}_{obj.root.entity.per_scene_id}" + elif isinstance(obj, Entity): + return f"{obj.name}_{obj.per_scene_id}" + else: + raise NotImplementedError(f"Unknown SAPIEN object type {type(obj)}") + + class SapienPlanningWorld(PlanningWorld): def __init__( self, @@ -76,11 +93,10 @@ def __init__( verbose=False, ) articulated_model.set_qpos(articulation.qpos) # update qpos # type: ignore - articulated_model.name = self.get_object_name(articulation) self.add_articulation(articulated_model) for articulation in planned_articulations: - self.set_articulation_planned(self.get_object_name(articulation), True) + self.set_articulation_planned(convert_object_name(articulation), True) for entity in actors: component = entity.find_component_by_type(PhysxRigidBaseComponent) @@ -103,7 +119,7 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None all attached objects """ for articulation in self._sim_scene.get_all_articulations(): - if art := self.get_articulation(self.get_object_name(articulation)): + if art := self.get_articulation(convert_object_name(articulation)): # set_qpos to update poses art.set_qpos(articulation.qpos) # type: ignore else: @@ -113,7 +129,7 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None ) for entity in self._sim_scene.get_all_actors(): - object_name = self.get_object_name(entity) + object_name = convert_object_name(entity) # If entity is an attached object if attached_body := self.get_attached_object(object_name): @@ -146,22 +162,6 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None "The scene might have changed since last update." ) - @staticmethod - def get_object_name(obj: PhysxArticulation | Entity) -> str: - """ - Constructs a unique name for the corresponding mplib object. - This is necessary because mplib objects assume unique names. - - :param obj: a SAPIEN object - :return: the unique mplib object name - """ - if isinstance(obj, PhysxArticulation): - return f"{obj.name}_{obj.root.entity.per_scene_id}" - elif isinstance(obj, Entity): - return f"{obj.name}_{obj.per_scene_id}" - else: - raise NotImplementedError(f"Unknown SAPIEN object type {type(obj)}") - @staticmethod def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: """ @@ -216,7 +216,7 @@ def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: return FCLObject( comp.name if isinstance(comp, PhysxArticulationLinkComponent) - else SapienPlanningWorld.get_object_name(comp.entity), + else convert_object_name(comp.entity), comp.entity.pose, # type: ignore shapes, shape_poses, diff --git a/mplib/sapien_utils/srdf_exporter.py b/mplib/sapien_utils/srdf_exporter.py index cd788431..64e515ff 100644 --- a/mplib/sapien_utils/srdf_exporter.py +++ b/mplib/sapien_utils/srdf_exporter.py @@ -43,7 +43,9 @@ def export_srdf_xml(articulation: PhysxArticulation) -> ET.Element: """ - elem_robot = ET.Element("robot", {"name": articulation.name}) + from .conversion import convert_object_name + + elem_robot = ET.Element("robot", {"name": convert_object_name(articulation)}) # Get all links with collision shapes colliding_links = [l for l in articulation.links if len(l.collision_shapes) > 0] diff --git a/mplib/sapien_utils/urdf_exporter.py b/mplib/sapien_utils/urdf_exporter.py index d67e35ba..3d10d139 100644 --- a/mplib/sapien_utils/urdf_exporter.py +++ b/mplib/sapien_utils/urdf_exporter.py @@ -160,7 +160,9 @@ def export_joint(joint: PhysxArticulationJoint) -> list[ET.Element]: def export_kinematic_chain_xml(articulation: PhysxArticulation) -> ET.Element: - elem_robot = ET.Element("robot", {"name": articulation.name}) + from .conversion import convert_object_name + + elem_robot = ET.Element("robot", {"name": convert_object_name(articulation)}) ET.SubElement(elem_robot, "link", {"name": "__world__"}) # root link for l in articulation.links: From 786767a171a9bb1573cfb66d3bc3d0bbab63f72e Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 12 Apr 2024 16:54:07 -0700 Subject: [PATCH 05/37] Remove articulation_names/normal_object_names from PlanningWorld constructor --- include/mplib/planning_world.h | 6 +----- mplib/planner.py | 2 -- mplib/pymp/__init__.pyi | 4 ---- mplib/sapien_utils/conversion.py | 2 +- pybind/docstring/planning_world.h | 4 +--- pybind/pybind_planning_world.cpp | 6 ++---- src/planning_world.cpp | 19 +++++-------------- 7 files changed, 10 insertions(+), 33 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index ba6fa253..23407438 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -57,14 +57,10 @@ class PlanningWorldTpl { * Constructs a PlanningWorld with given (planned) articulations and normal objects * * @param articulations: list of planned articulated models - * @param articulation_names: name of the articulated models * @param normal_objects: list of collision objects that are not articulated - * @param normal_object_names: name of the normal objects */ PlanningWorldTpl(const std::vector &articulations, - const std::vector &articulation_names, - const std::vector &normal_objects = {}, - const std::vector &normal_object_names = {}); + const std::vector &normal_objects = {}); /// @brief Gets names of all articulations in world (unordered) std::vector getArticulationNames() const; diff --git a/mplib/planner.py b/mplib/planner.py index bd4978c3..2ee6b989 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -125,9 +125,7 @@ def __init__( self.planning_world = PlanningWorld( [self.robot], - ["robot"], kwargs.get("normal_objects", []), - kwargs.get("normal_object_names", []), ) self.acm = self.planning_world.get_allowed_collision_matrix() diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index c7b4e656..4611253b 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -322,17 +322,13 @@ class PlanningWorld: def __init__( self, articulations: list[ArticulatedModel], - articulation_names: list[str], normal_objects: list[collision_detection.fcl.FCLObject] = [], - normal_object_names: list[str] = [], ) -> None: """ Constructs a PlanningWorld with given (planned) articulations and normal objects :param articulations: list of planned articulated models - :param articulation_names: name of the articulated models :param normal_objects: list of collision objects that are not articulated - :param normal_object_names: name of the normal objects """ def add_articulation(self, model: ArticulatedModel, planned: bool = False) -> None: """ diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index d51787b7..797fb42d 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -66,7 +66,7 @@ def __init__( :param planned_articulations: list of planned articulations. """ - super().__init__([], []) + super().__init__([]) self._sim_scene = sim_scene articulations: list[PhysxArticulation] = sim_scene.get_all_articulations() diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 3af04360..2911b6d0 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -39,9 +39,7 @@ R"doc( Constructs a PlanningWorld with given (planned) articulations and normal objects :param articulations: list of planned articulated models -:param articulation_names: name of the articulated models -:param normal_objects: list of collision objects that are not articulated -:param normal_object_names: name of the normal objects)doc"; +:param normal_objects: list of collision objects that are not articulated)doc"; static const char *__doc_mplib_PlanningWorldTpl_addArticulation = R"doc( diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index f3a34264..d67d6c33 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -30,11 +30,9 @@ void build_pyplanning_world(py::module &pymp) { PyPlanningWorld .def(py::init &, - const std::vector &, const std::vector &, - const std::vector &>(), - py::arg("articulations"), py::arg("articulation_names"), + const std::vector &>(), + py::arg("articulations"), py::arg("normal_objects") = std::vector(), - py::arg("normal_object_names") = std::vector(), DOC(mplib, PlanningWorldTpl, PlanningWorldTpl)) .def("get_articulation_names", &PlanningWorld::getArticulationNames, diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 6025f3c2..a48cf6db 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -17,21 +17,12 @@ DEFINE_TEMPLATE_PLANNING_WORLD(double); template PlanningWorldTpl::PlanningWorldTpl( const std::vector &articulations, - const std::vector &articulation_names, - const std::vector &normal_objects, - const std::vector &normal_object_names) + const std::vector &normal_objects) : acm_(std::make_shared()) { - ASSERT(articulations.size() == articulation_names.size(), - "articulations and articulation_names should have the same size"); - ASSERT(normal_objects.size() == normal_object_names.size(), - "normal_objects and normal_object_names should have the same size"); - // TODO(merge): remove articulation_names and normal_object_names - for (size_t i = 0; i < articulations.size(); i++) { - articulation_map_[articulation_names[i]] = articulations[i]; - planned_articulation_map_[articulation_names[i]] = articulations[i]; - } - for (size_t i = 0; i < normal_objects.size(); i++) - normal_object_map_[normal_object_names[i]] = normal_objects[i]; + for (const auto &art : articulations) + planned_articulation_map_[art->getName()] = articulation_map_[art->getName()] = art; + for (const auto &normal_object : normal_objects) + normal_object_map_[normal_object->name] = normal_object; } template From 198373f09851a051a721a848269f5c1b3017528c Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 14:05:33 -0700 Subject: [PATCH 06/37] Add a name argument to ArticulatedModel constructor/create to override URDF --- include/mplib/core/articulated_model.h | 5 +++++ mplib/pymp/__init__.pyi | 4 ++++ pybind/core/pybind_articulated_model.cpp | 27 ++++++++++++++++------- pybind/docstring/core/articulated_model.h | 2 ++ src/core/articulated_model.cpp | 8 ++++--- tests/test_articulated_model.cpp | 2 +- 6 files changed, 36 insertions(+), 12 deletions(-) diff --git a/include/mplib/core/articulated_model.h b/include/mplib/core/articulated_model.h index e6a5a8ce..9d024823 100644 --- a/include/mplib/core/articulated_model.h +++ b/include/mplib/core/articulated_model.h @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -33,6 +34,7 @@ class ArticulatedModelTpl { * @param urdf_filename: path to URDF file, can be relative to the current working * directory * @param srdf_filename: path to SRDF file, we use it to disable self-collisions + * @param name: name of the articulated model to override URDF robot name attribute * @param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis * @param link_names: list of links that are considered for planning * @param joint_names: list of joints that are considered for planning @@ -41,6 +43,7 @@ class ArticulatedModelTpl { */ ArticulatedModelTpl(const std::string &urdf_filename, const std::string &srdf_filename, + const std::string_view name = std::string_view {}, const Vector3 &gravity = Vector3 {0, 0, -9.81}, const std::vector &link_names = {}, const std::vector &joint_names = {}, @@ -60,6 +63,7 @@ class ArticulatedModelTpl { * @param collision_links: Vector of collision link names and FCLObjectPtr. * Format is: ``[(link_name, FCLObjectPtr), ...]``. * The collision objects are at the shape's local_pose. + * @param name: name of the articulated model to override URDF robot name attribute * @param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis * @param link_names: list of links that are considered for planning * @param joint_names: list of joints that are considered for planning @@ -71,6 +75,7 @@ class ArticulatedModelTpl { const std::string &urdf_string, const std::string &srdf_string, const std::vector>> &collision_links, + const std::string_view name = std::string_view {}, const Vector3 &gravity = Vector3 {0, 0, -9.81}, const std::vector &link_names = {}, const std::vector &joint_names = {}, bool verbose = false); diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 4611253b..8a9bdf2f 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -30,6 +30,7 @@ class ArticulatedModel: srdf_string: str, collision_links: list[tuple[str, collision_detection.fcl.FCLObject]], *, + name: str = None, gravity: numpy.ndarray[ tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] ] = ..., @@ -45,6 +46,7 @@ class ArticulatedModel: :param collision_links: Vector of collision link names and FCLObjectPtr. Format is: ``[(link_name, FCLObjectPtr), ...]``. The collision objects are at the shape's local_pose. + :param name: name of the articulated model to override URDF robot name attribute :param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis :param link_names: list of links that are considered for planning :param joint_names: list of joints that are considered for planning @@ -56,6 +58,7 @@ class ArticulatedModel: urdf_filename: str, srdf_filename: str, *, + name: str = None, gravity: numpy.ndarray[ tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] ] = ..., @@ -70,6 +73,7 @@ class ArticulatedModel: :param urdf_filename: path to URDF file, can be relative to the current working directory :param srdf_filename: path to SRDF file, we use it to disable self-collisions + :param name: name of the articulated model to override URDF robot name attribute :param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis :param link_names: list of links that are considered for planning :param joint_names: list of joints that are considered for planning diff --git a/pybind/core/pybind_articulated_model.cpp b/pybind/core/pybind_articulated_model.cpp index ef997638..e44bff80 100644 --- a/pybind/core/pybind_articulated_model.cpp +++ b/pybind/core/pybind_articulated_model.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -25,11 +26,18 @@ void build_pyarticulated_model(py::module &pymp) { pymp, "ArticulatedModel", DOC(mplib, ArticulatedModelTpl)); PyArticulatedModel - .def(py::init &, - const std::vector &, const std::vector &, - bool, bool>(), + .def(py::init([](const std::string &urdf_filename, + const std::string &srdf_filename, const char *name, + const Vector3 &gravity, + const std::vector &link_names, + const std::vector &joint_names, bool convex, + bool verbose) { + return ArticulatedModel(urdf_filename, srdf_filename, + name ? name : std::string_view {}, gravity, + link_names, joint_names, convex, verbose); + }), py::arg("urdf_filename"), py::arg("srdf_filename"), py::kw_only(), - py::arg("gravity") = Vector3 {0, 0, -9.81}, + py::arg("name") = nullptr, py::arg("gravity") = Vector3 {0, 0, -9.81}, py::arg("link_names") = std::vector(), py::arg("joint_names") = std::vector(), py::arg("convex") = false, py::arg("verbose") = false, @@ -39,16 +47,19 @@ void build_pyarticulated_model(py::module &pymp) { "create_from_urdf_string", [](const std::string &urdf_string, const std::string &srdf_string, const std::vector> &collision_links, - const Vector3 &gravity, const std::vector &link_names, + const char *name, const Vector3 &gravity, + const std::vector &link_names, const std::vector &joint_names, bool verbose) { std::shared_ptr articulation = ArticulatedModel::createFromURDFString( - urdf_string, srdf_string, collision_links, gravity, link_names, - joint_names, verbose); + urdf_string, srdf_string, collision_links, + name ? name : std::string_view {}, gravity, link_names, joint_names, + verbose); return articulation; }, py::arg("urdf_string"), py::arg("srdf_string"), py::arg("collision_links"), - py::kw_only(), py::arg("gravity") = Vector3 {0, 0, -9.81}, + py::kw_only(), py::arg("name") = nullptr, + py::arg("gravity") = Vector3 {0, 0, -9.81}, py::arg("link_names") = std::vector(), py::arg("joint_names") = std::vector(), py::arg("verbose") = false, diff --git a/pybind/docstring/core/articulated_model.h b/pybind/docstring/core/articulated_model.h index eb89fe9f..a0237661 100644 --- a/pybind/docstring/core/articulated_model.h +++ b/pybind/docstring/core/articulated_model.h @@ -35,6 +35,7 @@ Construct an articulated model from URDF and SRDF files. :param urdf_filename: path to URDF file, can be relative to the current working directory :param srdf_filename: path to SRDF file, we use it to disable self-collisions +:param name: name of the articulated model to override URDF robot name attribute :param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis :param link_names: list of links that are considered for planning :param joint_names: list of joints that are considered for planning @@ -56,6 +57,7 @@ Constructs an ArticulatedModel from URDF/SRDF strings and collision links :param collision_links: Vector of collision link names and FCLObjectPtr. Format is: ``[(link_name, FCLObjectPtr), ...]``. The collision objects are at the shape's local_pose. +:param name: name of the articulated model to override URDF robot name attribute :param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis :param link_names: list of links that are considered for planning :param joint_names: list of joints that are considered for planning diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index 472165a9..a1a54503 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -17,6 +17,7 @@ DEFINE_TEMPLATE_ARTICULATED_MODEL(double); template ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, const std::string &srdf_filename, + const std::string_view name, const Vector3 &gravity, const std::vector &link_names, const std::vector &joint_names, @@ -26,7 +27,7 @@ ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, fcl_model_(std::make_shared>( urdf_filename, convex, verbose)), verbose_(verbose) { - name_ = fcl_model_->getName(); + name_ = name.data() ? name : fcl_model_->getName(); user_link_names_ = link_names.size() == 0 ? pinocchio_model_->getLinkNames(false) : link_names; user_joint_names_ = @@ -45,7 +46,8 @@ std::unique_ptr> ArticulatedModelTpl::createFromURDFSt const std::string &urdf_string, const std::string &srdf_string, const std::vector>> &collision_links, - const Vector3 &gravity, const std::vector &link_names, + const std::string_view name, const Vector3 &gravity, + const std::vector &link_names, const std::vector &joint_names, bool verbose) { auto articulation = std::make_unique>(Secret()); @@ -55,7 +57,7 @@ std::unique_ptr> ArticulatedModelTpl::createFromURDFSt auto fcl_model = articulation->fcl_model_ = collision_detection::FCLModelTpl::createFromURDFString( urdf_string, collision_links, verbose); - articulation->name_ = fcl_model->getName(); + articulation->name_ = name.data() ? name : fcl_model->getName(); articulation->verbose_ = verbose; auto user_link_names = articulation->user_link_names_ = diff --git a/tests/test_articulated_model.cpp b/tests/test_articulated_model.cpp index c49f67d7..9c793c18 100644 --- a/tests/test_articulated_model.cpp +++ b/tests/test_articulated_model.cpp @@ -11,6 +11,6 @@ int main() { std::string urdf_filename = "../data/panda/panda.urdf"; std::string srdf_filename = "../data/panda/panda.srdf"; Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, -9.81); - ArticulatedModel articulated_model(urdf_filename, srdf_filename, gravity, {}, {}, + ArticulatedModel articulated_model(urdf_filename, srdf_filename, {}, gravity, {}, {}, false, false); } From 0c00745d0199f4d1d81f91a2614e1534c3195d5e Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 14:52:09 -0700 Subject: [PATCH 07/37] Add support for PhysxCollisionShapePlane conversion --- mplib/sapien_utils/conversion.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 797fb42d..edab8c59 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -195,10 +195,13 @@ def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: # FCL Cylinder has z-axis along cylinder height shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) elif isinstance(shape, PhysxCollisionShapePlane): - raise NotImplementedError( - "Support for Plane collision is not implemented yet in mplib, " - "need fcl::Plane" - ) + # 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): From 48b8b120c0ce5168b8df9d28d4b9ea6a5729a621 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 15:44:04 -0700 Subject: [PATCH 08/37] Remove pair from createFromURDFString; link_name is not needed --- include/mplib/collision_detection/fcl/fcl_model.h | 7 +++---- include/mplib/core/articulated_model.h | 9 +++------ mplib/pymp/__init__.pyi | 8 ++++---- mplib/pymp/collision_detection/fcl.pyi | 11 ++++------- mplib/sapien_utils/conversion.py | 2 +- pybind/collision_detection/fcl/pybind_fcl_model.cpp | 4 +--- pybind/core/pybind_articulated_model.cpp | 6 ++---- pybind/docstring/collision_detection/fcl/fcl_model.h | 6 +++--- pybind/docstring/core/articulated_model.h | 6 +++--- src/collision_detection/fcl/fcl_model.cpp | 7 +++---- src/core/articulated_model.cpp | 3 +-- 11 files changed, 28 insertions(+), 41 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 6997406b..855b4d28 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -54,16 +54,15 @@ class FCLModelTpl { * Constructs a FCLModel from URDF string and collision links * * @param urdf_string: URDF string (without visual/collision elements for links) - * @param collision_links: Vector of collision link names and FCLObjectPtr. - * Format is: ``[(link_name, FCLObjectPtr), ...]``. + * @param collision_links: Vector of collision links as FCLObjectPtr. + * Format is: ``[FCLObjectPtr, ...]``. * The collision objects are at the shape's local_pose. * @param verbose: print debug information. Default: ``false``. * @return: a unique_ptr to FCLModel */ static std::unique_ptr> createFromURDFString( const std::string &urdf_string, - const std::vector>> &collision_links, - bool verbose = false); + const std::vector> &collision_links, bool verbose = false); /** * Get name of the articulated model. diff --git a/include/mplib/core/articulated_model.h b/include/mplib/core/articulated_model.h index 9d024823..f816461b 100644 --- a/include/mplib/core/articulated_model.h +++ b/include/mplib/core/articulated_model.h @@ -3,7 +3,6 @@ #include #include #include -#include #include #include "mplib/collision_detection/types.h" @@ -60,8 +59,8 @@ class ArticulatedModelTpl { * * @param urdf_string: URDF string (without visual/collision elements for links) * @param srdf_string: SRDF string (only disable_collisions element) - * @param collision_links: Vector of collision link names and FCLObjectPtr. - * Format is: ``[(link_name, FCLObjectPtr), ...]``. + * @param collision_links: Vector of collision links as FCLObjectPtr. + * Format is: ``[FCLObjectPtr, ...]``. * The collision objects are at the shape's local_pose. * @param name: name of the articulated model to override URDF robot name attribute * @param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis @@ -70,11 +69,9 @@ class ArticulatedModelTpl { * @param verbose: print debug information. Default: ``false``. * @return: a unique_ptr to ArticulatedModel */ - // TODO(merge): remove pair (link name is not needed) static std::unique_ptr> createFromURDFString( const std::string &urdf_string, const std::string &srdf_string, - const std::vector>> - &collision_links, + const std::vector> &collision_links, const std::string_view name = std::string_view {}, const Vector3 &gravity = Vector3 {0, 0, -9.81}, const std::vector &link_names = {}, diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 8a9bdf2f..c355195e 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -28,7 +28,7 @@ class ArticulatedModel: def create_from_urdf_string( urdf_string: str, srdf_string: str, - collision_links: list[tuple[str, collision_detection.fcl.FCLObject]], + collision_links: list[collision_detection.fcl.FCLObject], *, name: str = None, gravity: numpy.ndarray[ @@ -43,9 +43,9 @@ class ArticulatedModel: :param urdf_string: URDF string (without visual/collision elements for links) :param srdf_string: SRDF string (only disable_collisions element) - :param collision_links: Vector of collision link names and FCLObjectPtr. Format - is: ``[(link_name, FCLObjectPtr), ...]``. The collision objects are at the - shape's local_pose. + :param collision_links: Vector of collision links as FCLObjectPtr. Format is: + ``[FCLObjectPtr, ...]``. The collision objects are at the shape's + local_pose. :param name: name of the articulated model to override URDF robot name attribute :param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis :param link_names: list of links that are considered for planning diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index c53f0911..20a4b5aa 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -555,18 +555,15 @@ class FCLModel: """ @staticmethod def create_from_urdf_string( - urdf_string: str, - collision_links: list[tuple[str, ...]], - *, - verbose: bool = False, + urdf_string: str, collision_links: list[...], *, verbose: bool = False ) -> FCLModel: """ Constructs a FCLModel from URDF string and collision links :param urdf_string: URDF string (without visual/collision elements for links) - :param collision_links: Vector of collision link names and FCLObjectPtr. Format - is: ``[(link_name, FCLObjectPtr), ...]``. The collision objects are at the - shape's local_pose. + :param collision_links: Vector of collision links as FCLObjectPtr. Format is: + ``[FCLObjectPtr, ...]``. The collision objects are at the shape's + local_pose. :param verbose: print debug information. Default: ``False``. :return: a unique_ptr to FCLModel """ diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index edab8c59..590c7976 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -78,7 +78,7 @@ def __init__( # Convert all links to FCLObject collision_links = [ - (link.name, fcl_obj) + fcl_obj for link in articulation.links if (fcl_obj := self.convert_physx_component(link)) is not None ] diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index fff2d6b8..24acfc44 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -31,9 +31,7 @@ void build_pyfcl_model(py::module &m) { .def_static( "create_from_urdf_string", [](const std::string &urdf_string, - const std::vector>> - &collision_links, - bool verbose) { + const std::vector> &collision_links, bool verbose) { std::shared_ptr fcl_model = FCLModel::createFromURDFString(urdf_string, collision_links, verbose); return fcl_model; diff --git a/pybind/core/pybind_articulated_model.cpp b/pybind/core/pybind_articulated_model.cpp index e44bff80..e5a061e9 100644 --- a/pybind/core/pybind_articulated_model.cpp +++ b/pybind/core/pybind_articulated_model.cpp @@ -1,7 +1,6 @@ #include #include #include -#include #include #include @@ -46,9 +45,8 @@ void build_pyarticulated_model(py::module &pymp) { .def_static( "create_from_urdf_string", [](const std::string &urdf_string, const std::string &srdf_string, - const std::vector> &collision_links, - const char *name, const Vector3 &gravity, - const std::vector &link_names, + const std::vector &collision_links, const char *name, + const Vector3 &gravity, const std::vector &link_names, const std::vector &joint_names, bool verbose) { std::shared_ptr articulation = ArticulatedModel::createFromURDFString( diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index 65e5a88e..5a245b20 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -68,9 +68,9 @@ R"doc( Constructs a FCLModel from URDF string and collision links :param urdf_string: URDF string (without visual/collision elements for links) -:param collision_links: Vector of collision link names and FCLObjectPtr. Format - is: ``[(link_name, FCLObjectPtr), ...]``. The collision objects are at the - shape's local_pose. +:param collision_links: Vector of collision links as FCLObjectPtr. Format is: + ``[FCLObjectPtr, ...]``. The collision objects are at the shape's + local_pose. :param verbose: print debug information. Default: ``False``. :return: a unique_ptr to FCLModel)doc"; diff --git a/pybind/docstring/core/articulated_model.h b/pybind/docstring/core/articulated_model.h index a0237661..6077e97c 100644 --- a/pybind/docstring/core/articulated_model.h +++ b/pybind/docstring/core/articulated_model.h @@ -54,9 +54,9 @@ Constructs an ArticulatedModel from URDF/SRDF strings and collision links :param urdf_string: URDF string (without visual/collision elements for links) :param srdf_string: SRDF string (only disable_collisions element) -:param collision_links: Vector of collision link names and FCLObjectPtr. Format - is: ``[(link_name, FCLObjectPtr), ...]``. The collision objects are at the - shape's local_pose. +:param collision_links: Vector of collision links as FCLObjectPtr. Format is: + ``[FCLObjectPtr, ...]``. The collision objects are at the shape's + local_pose. :param name: name of the articulated model to override URDF robot name attribute :param gravity: gravity vector, by default is ``[0, 0, -9.81]`` in -z axis :param link_names: list of links that are considered for planning diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 2d8fec21..c14453fa 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -42,16 +42,15 @@ FCLModelTpl::FCLModelTpl(const urdf::ModelInterfaceSharedPtr &urdf_model, template std::unique_ptr> FCLModelTpl::createFromURDFString( - const std::string &urdf_string, - const std::vector>> &collision_links, + const std::string &urdf_string, const std::vector> &collision_links, bool verbose) { auto urdf = urdf::parseURDF(urdf_string); // package_dir is not needed since urdf_string contains no visual/collision elements auto fcl_model = std::make_unique>(urdf, "", verbose, false); - for (const auto &[link_name, collision_obj] : collision_links) { + for (const auto &collision_obj : collision_links) { fcl_model->collision_objects_.push_back(collision_obj); - fcl_model->collision_link_names_.push_back(link_name); + fcl_model->collision_link_names_.push_back(collision_obj->name); } // TODO: this should not be needed after switching to FCLObject diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index a1a54503..edcf5fce 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -44,8 +44,7 @@ ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, template std::unique_ptr> ArticulatedModelTpl::createFromURDFString( const std::string &urdf_string, const std::string &srdf_string, - const std::vector>> - &collision_links, + const std::vector> &collision_links, const std::string_view name, const Vector3 &gravity, const std::vector &link_names, const std::vector &joint_names, bool verbose) { From 01e922b224833ffe9888f48afd1c5053fe36218c Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 16:06:38 -0700 Subject: [PATCH 09/37] Rename normal_object as just object See MoveIt2's collision_detection::World::addToObject() https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1World.html --- docs/source/tutorials/detect_collision.rst | 2 +- include/mplib/planning_world.h | 117 +++++++++--------- mplib/planner.py | 20 ++-- mplib/pymp/__init__.pyi | 132 +++++++++++---------- mplib/sapien_utils/conversion.py | 6 +- pybind/docstring/planning_world.h | 130 ++++++++++---------- pybind/pybind_planning_world.cpp | 35 +++--- src/planning_world.cpp | 45 ++++--- 8 files changed, 251 insertions(+), 236 deletions(-) diff --git a/docs/source/tutorials/detect_collision.rst b/docs/source/tutorials/detect_collision.rst index 8a4651e5..7e3a45c1 100644 --- a/docs/source/tutorials/detect_collision.rst +++ b/docs/source/tutorials/detect_collision.rst @@ -22,7 +22,7 @@ We will also create a floor as one of the collision objects to demonstrate the ` :start-after: # floor ankor :end-before: # floor ankor end -Note that we call floor a "normal" object because it is not an articulated object. The function to add a normal object to the planning world is `set_normal_object`. This can also be used to update the pose of the object or change it our entirely. +Note that we call floor an object because it is not an articulated object. The function to add an object to the planning world is `add_object`. This can also be used to update the pose of the object or change it our entirely. Collision Time! --------------- diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 23407438..1601cd9b 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -17,8 +17,6 @@ namespace mplib { // PlanningWorldTplPtr MPLIB_CLASS_TEMPLATE_FORWARD(PlanningWorldTpl); -// TODO(merge): rename "normal object" as just "object" - /** * Planning world for collision checking * @@ -54,13 +52,13 @@ class PlanningWorldTpl { public: /** - * Constructs a PlanningWorld with given (planned) articulations and normal objects + * Constructs a PlanningWorld with given (planned) articulations and objects * * @param articulations: list of planned articulated models - * @param normal_objects: list of collision objects that are not articulated + * @param objects: list of non-articulated collision objects */ PlanningWorldTpl(const std::vector &articulations, - const std::vector &normal_objects = {}); + const std::vector &objects = {}); /// @brief Gets names of all articulations in world (unordered) std::vector getArticulationNames() const; @@ -125,48 +123,45 @@ class PlanningWorldTpl { */ void setArticulationPlanned(const std::string &name, bool planned); - /// @brief Gets names of all normal objects in world (unordered) - std::vector getNormalObjectNames() const; + /// @brief Gets names of all objects in world (unordered) + std::vector getObjectNames() const; /** - * Gets the normal object (``FCLObjectPtr``) with given name + * Gets the non-articulated object (``FCLObjectPtr``) with given name * - * @param name: name of the normal object - * @return: the normal object with given name or ``nullptr`` if not found. + * @param name: name of the non-articulated object + * @return: the object with given name or ``nullptr`` if not found. */ - FCLObjectPtr getNormalObject(const std::string &name) const { - auto it = normal_object_map_.find(name); - return it != normal_object_map_.end() ? it->second : nullptr; + FCLObjectPtr getObject(const std::string &name) const { + auto it = object_map_.find(name); + return it != object_map_.end() ? it->second : nullptr; } /** - * Check whether the normal object with given name exists + * Check whether the non-articulated object with given name exists * - * @param name: name of the normal object + * @param name: name of the non-articulated object * @return: ``true`` if exists, ``false`` otherwise. */ - bool hasNormalObject(const std::string &name) const { - return normal_object_map_.find(name) != normal_object_map_.end(); + bool hasObject(const std::string &name) const { + return object_map_.find(name) != object_map_.end(); } /** - * Adds a normal object containing multiple collision objects (``FCLObjectPtr``) - * to world + * Adds an non-articulated object containing multiple collision objects + * (``FCLObjectPtr``) to world * * @param fcl_obj: FCLObject to be added */ - void addNormalObject(const FCLObjectPtr &fcl_obj) { - normal_object_map_[fcl_obj->name] = fcl_obj; - } + void addObject(const FCLObjectPtr &fcl_obj) { object_map_[fcl_obj->name] = fcl_obj; } /** - * Adds a normal object (``CollisionObjectPtr``) with given name to world + * Adds an non-articulated object (``CollisionObjectPtr``) with given name to world * * @param name: name of the collision object * @param collision_object: collision object to be added */ - void addNormalObject(const std::string &name, - const CollisionObjectPtr &collision_object); + void addObject(const std::string &name, const CollisionObjectPtr &collision_object); /** * Adds a point cloud as a collision object with given name to world @@ -183,18 +178,18 @@ class PlanningWorldTpl { * Updates acm_ * * @param name: name of the non-articulated collision object - * @return: ``true`` if success, ``false`` if normal object with given name does not - * exist + * @return: ``true`` if success, ``false`` if the non-articulated object + * with given name does not exist */ - bool removeNormalObject(const std::string &name); + bool removeObject(const std::string &name); /** - * Check whether normal object with given name is attached + * Check whether the non-articulated object with given name is attached * - * @param name: name of the normal object + * @param name: name of the non-articulated object * @return: ``true`` if it is attached, ``false`` otherwise. */ - bool isNormalObjectAttached(const std::string &name) const { + bool isObjectAttached(const std::string &name) const { return attached_body_map_.find(name) != attached_body_map_.end(); } @@ -210,67 +205,75 @@ class PlanningWorldTpl { } /** - * Attaches existing normal 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. + * 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. * - * @param name: normal object name to attach + * @param name: name of the non-articulated object to attach * @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 touch_links: link names that the attached object touches - * @throws std::out_of_range if normal object with given name does not exist + * @throws std::out_of_range if non-articulated object with given name does not exist * or if planned articulation with given name does not exist */ void attachObject(const std::string &name, const std::string &art_name, int link_id, const std::vector &touch_links); /** - * Attaches existing normal object to specified link of articulation at 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. + * Attaches existing non-articulated object to specified link of articulation + * at 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. + * * If the object is already attached, the touch_links of the attached object * is preserved and acm_ remains unchanged. * - * @param name: normal object name to attach + * @param name: name of the non-articulated object to attach * @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 - * @throws std::out_of_range if normal object with given name does not exist + * @throws std::out_of_range if non-articulated object with given name does not exist * or if planned articulation with given name does not exist */ void attachObject(const std::string &name, const std::string &art_name, int link_id); /** - * Attaches existing normal object to specified link of articulation at given pose. - * If the object is currently attached, disallow collision between the object - * and previous touch_links. + * 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. * - * @param name: normal object name to attach + * @param name: name of the non-articulated object to attach * @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 - * @throws std::out_of_range if normal object with given name does not exist + * @throws std::out_of_range if non-articulated object with given name does not exist * or if planned articulation with given name does not exist */ void attachObject(const std::string &name, const std::string &art_name, int link_id, const Pose &pose, const std::vector &touch_links); /** - * Attaches existing normal object to specified link of articulation at 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. + * Attaches existing non-articulated object to specified link of articulation + * at 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. + * * If the object is already attached, the touch_links of the attached object * is preserved and acm_ remains unchanged. * - * @param name: normal object name to attach + * @param name: name of the non-articulated object to attach * @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) - * @throws std::out_of_range if normal object with given name does not exist + * @throws std::out_of_range if non-articulated object with given name does not exist * or if planned articulation with given name does not exist */ void attachObject(const std::string &name, const std::string &art_name, int link_id, @@ -278,10 +281,10 @@ class PlanningWorldTpl { /** * Attaches given object (w/ p_geom) to specified link of articulation at given pose. - * This is done by removing normal object and then adding and attaching object. + * 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: normal object name to attach + * @param name: name of the non-articulated object to attach * @param p_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 @@ -294,12 +297,12 @@ class PlanningWorldTpl { /** * Attaches given object (w/ p_geom) to specified link of articulation at given pose. - * This is done by removing normal object and then adding and attaching object. + * 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. * Automatically sets touch_links as the name of self links * that collide with the object in the current state (auto touch_links). * - * @param name: normal object name to attach + * @param name: name of the non-articulated object to attach * @param p_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 @@ -345,7 +348,7 @@ class PlanningWorldTpl { * Detaches object with given name. * Updates acm_ to disallow collision between the object and touch_links. * - * @param name: normal object name to detach + * @param name: name of the non-articulated object to detach * @param also_remove: whether to also remove object from world * @return: ``true`` if success, ``false`` if the object with given name is not * attached @@ -447,7 +450,7 @@ class PlanningWorldTpl { private: std::unordered_map articulation_map_; - std::unordered_map normal_object_map_; + std::unordered_map object_map_; // TODO: can planned_articulations_ be unordered_map? (setQposAll) std::map planned_articulation_map_; diff --git a/mplib/planner.py b/mplib/planner.py index 2ee6b989..e4ac1df9 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -13,7 +13,7 @@ from mplib.pymp import ArticulatedModel, PlanningWorld, Pose from mplib.pymp.collision_detection import AllowedCollisionMatrix, WorldCollisionResult -from mplib.pymp.collision_detection.fcl import CollisionGeometry +from mplib.pymp.collision_detection.fcl import CollisionGeometry, FCLObject from mplib.pymp.planning import ompl @@ -35,8 +35,8 @@ def __init__( user_joint_names: Sequence[str] = [], joint_vel_limits: Optional[Sequence[float] | np.ndarray] = None, joint_acc_limits: Optional[Sequence[float] | np.ndarray] = None, + objects: list[FCLObject] = [], # noqa: B006 verbose: bool = False, - **kwargs, ): # constructor ankor end """ @@ -59,6 +59,7 @@ def __init__( which should have the same length as ``self.move_group_joint_indices`` :param joint_acc_limits: maximum joint accelerations for time parameterization, which should have the same length as ``self.move_group_joint_indices`` + :param objects: list of FCLObject as non-articulated collision objects :param verbose: if True, print verbose logs for debugging """ self.urdf = urdf @@ -123,10 +124,7 @@ def __init__( t.startswith("JointModelR") for t in self.joint_types ] & (self.joint_limits[:, 1] - self.joint_limits[:, 0] > 2 * np.pi) - self.planning_world = PlanningWorld( - [self.robot], - kwargs.get("normal_objects", []), - ) + self.planning_world = PlanningWorld([self.robot], objects) self.acm = self.planning_world.get_allowed_collision_matrix() if self.srdf == "": @@ -506,10 +504,10 @@ def remove_point_cloud(self, name="scene_pcd") -> bool: Removes the point cloud collision object with given name :param name: name of the point cloud collision object - :return: ``True`` if success, ``False`` if normal object with given name doesn't - exist + :return: ``True`` if success, ``False`` if the non-articulation object + with given name does not exist """ - return self.planning_world.remove_normal_object(name) + return self.planning_world.remove_object(name) def update_attached_object( self, @@ -627,9 +625,9 @@ def set_base_pose(self, pose: Pose): """ self.robot.set_base_pose(pose) - def remove_normal_object(self, name) -> bool: + def remove_object(self, name) -> bool: """returns true if the object was removed, false if it was not found""" - return self.planning_world.remove_normal_object(name) + return self.planning_world.remove_object(name) def plan_qpos_to_qpos( self, diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index c355195e..4e1ae40b 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -326,13 +326,13 @@ class PlanningWorld: def __init__( self, articulations: list[ArticulatedModel], - normal_objects: list[collision_detection.fcl.FCLObject] = [], + objects: list[collision_detection.fcl.FCLObject] = [], ) -> None: """ - Constructs a PlanningWorld with given (planned) articulations and normal objects + Constructs a PlanningWorld with given (planned) articulations and objects :param articulations: list of planned articulated models - :param normal_objects: list of collision objects that are not articulated + :param objects: list of non-articulated collision objects """ def add_articulation(self, model: ArticulatedModel, planned: bool = False) -> None: """ @@ -342,19 +342,19 @@ class PlanningWorld: :param planned: whether the articulation is being planned """ @typing.overload - def add_normal_object(self, fcl_obj: collision_detection.fcl.FCLObject) -> None: + def add_object(self, fcl_obj: collision_detection.fcl.FCLObject) -> None: """ - Adds a normal object containing multiple collision objects (``FCLObjectPtr``) to - world + Adds an non-articulated object containing multiple collision objects + (``FCLObjectPtr``) to world :param fcl_obj: FCLObject to be added """ @typing.overload - def add_normal_object( + def add_object( self, name: str, collision_object: collision_detection.fcl.CollisionObject ) -> None: """ - Adds a normal object (``CollisionObjectPtr``) with given name to world + Adds an non-articulated object (``CollisionObjectPtr``) with given name to world :param name: name of the collision object :param collision_object: collision object to be added @@ -407,68 +407,76 @@ class PlanningWorld: self, name: str, art_name: str, link_id: int, touch_links: list[str] ) -> None: """ - Attaches existing normal 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. + 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. - :param name: normal object name to attach + :param name: name of the non-articulated object to attach :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 touch_links: link names that the attached object touches - :raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist + :raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist """ @typing.overload def attach_object(self, name: str, art_name: str, link_id: int) -> None: """ - Attaches existing normal object to specified link of articulation at 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. If the - object is already attached, the touch_links of the attached object is preserved - and acm_ remains unchanged. + Attaches existing non-articulated object to specified link of articulation at + 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. + + If the object is already attached, the touch_links of the attached object is + preserved and acm_ remains unchanged. - :param name: normal object name to attach + :param name: name of the non-articulated object to attach :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 - :raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist + :raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist """ @typing.overload def attach_object( self, name: str, art_name: str, link_id: int, pose: Pose, touch_links: list[str] ) -> None: """ - Attaches existing normal 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. + 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. - :param name: normal object name to attach + 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 :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 - :raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist + :raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist """ @typing.overload def attach_object(self, name: str, art_name: str, link_id: int, pose: Pose) -> None: """ - Attaches existing normal object to specified link of articulation at 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. If the object - is already attached, the touch_links of the attached object is preserved and - acm_ remains unchanged. + Attaches existing non-articulated object to specified link of articulation at + 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. + + If the object is already attached, the touch_links of the attached object is + preserved and acm_ remains unchanged. - :param name: normal object name to attach + :param name: name of the non-articulated object to attach :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) - :raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist + :raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist """ @typing.overload def attach_object( @@ -482,10 +490,10 @@ class PlanningWorld: ) -> None: """ Attaches given object (w/ p_geom) to specified link of articulation at given - pose. This is done by removing normal object and then adding and attaching - object. As a result, all previous acm_ entries with the object are removed + 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: normal object name to attach + :param name: name of the non-articulated object to attach :param p_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 @@ -503,12 +511,12 @@ class PlanningWorld: ) -> None: """ Attaches given object (w/ p_geom) to specified link of articulation at given - pose. This is done by removing normal object and then adding and attaching - object. As a result, all previous acm_ entries with the object are removed. + 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. Automatically sets touch_links as the name of self links that collide with the object in the current state (auto touch_links). - :param name: normal object name to attach + :param name: name of the non-articulated object to attach :param p_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 @@ -556,7 +564,7 @@ class PlanningWorld: Detaches object with given name. Updates acm_ to disallow collision between the object and touch_links. - :param name: normal object name to detach + :param name: name of the non-articulated object to detach :param also_remove: whether to also remove object from world :return: ``True`` if success, ``False`` if the object with given name is not attached @@ -611,16 +619,16 @@ class PlanningWorld: :param name: name of the attached body :return: the attached body with given name or ``None`` if not found. """ - def get_normal_object(self, name: str) -> collision_detection.fcl.FCLObject: + def get_object(self, name: str) -> collision_detection.fcl.FCLObject: """ - Gets the normal object (``FCLObjectPtr``) with given name + Gets the non-articulated object (``FCLObjectPtr``) with given name - :param name: name of the normal object - :return: the normal object with given name or ``None`` if not found. + :param name: name of the non-articulated object + :return: the object with given name or ``None`` if not found. """ - def get_normal_object_names(self) -> list[str]: + def get_object_names(self) -> list[str]: """ - Gets names of all normal objects in world (unordered) + Gets names of all objects in world (unordered) """ def get_planned_articulations(self) -> list[ArticulatedModel]: """ @@ -633,11 +641,11 @@ class PlanningWorld: :param name: name of the articulated model :return: ``True`` if exists, ``False`` otherwise. """ - def has_normal_object(self, name: str) -> bool: + def has_object(self, name: str) -> bool: """ - Check whether the normal object with given name exists + Check whether the non-articulated object with given name exists - :param name: name of the normal object + :param name: name of the non-articulated object :return: ``True`` if exists, ``False`` otherwise. """ def is_articulation_planned(self, name: str) -> bool: @@ -647,11 +655,11 @@ class PlanningWorld: :param name: name of the articulated model :return: ``True`` if exists, ``False`` otherwise. """ - def is_normal_object_attached(self, name: str) -> bool: + def is_object_attached(self, name: str) -> bool: """ - Check whether normal object with given name is attached + Check whether the non-articulated object with given name is attached - :param name: name of the normal object + :param name: name of the non-articulated object :return: ``True`` if it is attached, ``False`` otherwise. """ def print_attached_body_pose(self) -> None: @@ -666,14 +674,14 @@ class PlanningWorld: :return: ``True`` if success, ``False`` if articulation with given name does not exist """ - def remove_normal_object(self, name: str) -> bool: + def remove_object(self, name: str) -> bool: """ Removes (and detaches) the collision object with given name if exists. Updates acm_ :param name: name of the non-articulated collision object - :return: ``True`` if success, ``False`` if normal object with given name does - not exist + :return: ``True`` if success, ``False`` if the non-articulated object with given + name does not exist """ def self_collide( self, request: collision_detection.fcl.CollisionRequest = ... diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 590c7976..89a34456 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -107,7 +107,7 @@ def __init__( # Convert collision shapes at current global pose if (fcl_obj := self.convert_physx_component(component)) is not None: # type: ignore - self.add_normal_object(fcl_obj) + self.add_object(fcl_obj) def update_from_simulation(self, *, update_attached_object: bool = True) -> None: """ @@ -139,9 +139,9 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None * entity.pose # type: ignore ) attached_body.update_pose() - elif fcl_obj := self.get_normal_object(object_name): + elif fcl_obj := self.get_object(object_name): # Overwrite the object - self.add_normal_object( + self.add_object( FCLObject( object_name, entity.pose, # type: ignore diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 2911b6d0..56ea367f 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -36,10 +36,10 @@ Mimicking MoveIt2's ``planning_scene::PlanningScene``, static const char *__doc_mplib_PlanningWorldTpl_PlanningWorldTpl = R"doc( -Constructs a PlanningWorld with given (planned) articulations and normal objects +Constructs a PlanningWorld with given (planned) articulations and objects :param articulations: list of planned articulated models -:param normal_objects: list of collision objects that are not articulated)doc"; +:param objects: list of non-articulated collision objects)doc"; static const char *__doc_mplib_PlanningWorldTpl_addArticulation = R"doc( @@ -48,16 +48,16 @@ Adds an articulation (ArticulatedModelPtr) to world :param model: articulated model to be added :param planned: whether the articulation is being planned)doc"; -static const char *__doc_mplib_PlanningWorldTpl_addNormalObject = +static const char *__doc_mplib_PlanningWorldTpl_addObject = R"doc( -Adds a normal object containing multiple collision objects (``FCLObjectPtr``) to -world +Adds an non-articulated object containing multiple collision objects +(``FCLObjectPtr``) to world :param fcl_obj: FCLObject to be added)doc"; -static const char *__doc_mplib_PlanningWorldTpl_addNormalObject_2 = +static const char *__doc_mplib_PlanningWorldTpl_addObject_2 = R"doc( -Adds a normal object (``CollisionObjectPtr``) with given name to world +Adds an non-articulated object (``CollisionObjectPtr``) with given name to world :param name: name of the collision object :param collision_object: collision object to be added)doc"; @@ -90,71 +90,79 @@ Attaches given mesh to specified link of articulation (auto touch_links) static const char *__doc_mplib_PlanningWorldTpl_attachObject = R"doc( -Attaches existing normal 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. +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. -:param name: normal object name to attach +:param name: name of the non-articulated object to attach :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 touch_links: link names that the attached object touches -:raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist)doc"; +:raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist)doc"; static const char *__doc_mplib_PlanningWorldTpl_attachObject_2 = R"doc( -Attaches existing normal object to specified link of articulation at 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. If the -object is already attached, the touch_links of the attached object is preserved -and acm_ remains unchanged. +Attaches existing non-articulated object to specified link of articulation at +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. + +If the object is already attached, the touch_links of the attached object is +preserved and acm_ remains unchanged. -:param name: normal object name to attach +:param name: name of the non-articulated object to attach :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 -:raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist)doc"; +:raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist)doc"; static const char *__doc_mplib_PlanningWorldTpl_attachObject_3 = R"doc( -Attaches existing normal 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. +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. -:param name: normal object name to attach +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 :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 -:raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist)doc"; +:raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist)doc"; static const char *__doc_mplib_PlanningWorldTpl_attachObject_4 = R"doc( -Attaches existing normal object to specified link of articulation at 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. If the object -is already attached, the touch_links of the attached object is preserved and -acm_ remains unchanged. +Attaches existing non-articulated object to specified link of articulation at +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. + +If the object is already attached, the touch_links of the attached object is +preserved and acm_ remains unchanged. -:param name: normal object name to attach +:param name: name of the non-articulated object to attach :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) -:raises ValueError: if normal object with given name does not exist or if - planned articulation with given name does not exist)doc"; +:raises ValueError: if non-articulated object with given name does not exist or + if planned articulation with given name does not exist)doc"; 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 normal object and then adding and attaching -object. As a result, all previous acm_ entries with the object are removed +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: normal object name to attach +:param name: name of the non-articulated object to attach :param p_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 @@ -164,12 +172,12 @@ object. As a result, all previous acm_ entries with the object are removed 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 normal object and then adding and attaching -object. As a result, all previous acm_ entries with the object are removed. +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. Automatically sets touch_links as the name of self links that collide with the object in the current state (auto touch_links). -:param name: normal object name to attach +:param name: name of the non-articulated object to attach :param p_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 @@ -211,7 +219,7 @@ R"doc( Detaches object with given name. Updates acm_ to disallow collision between the object and touch_links. -:param name: normal object name to detach +:param name: name of the non-articulated object to detach :param also_remove: whether to also remove object from world :return: ``True`` if success, ``False`` if the object with given name is not attached)doc"; @@ -267,16 +275,16 @@ Gets the attached body (AttachedBodyPtr) with given name :param name: name of the attached body :return: the attached body with given name or ``None`` if not found.)doc"; -static const char *__doc_mplib_PlanningWorldTpl_getNormalObject = +static const char *__doc_mplib_PlanningWorldTpl_getObject = R"doc( -Gets the normal object (``FCLObjectPtr``) with given name +Gets the non-articulated object (``FCLObjectPtr``) with given name -:param name: name of the normal object -:return: the normal object with given name or ``None`` if not found.)doc"; +:param name: name of the non-articulated object +:return: the object with given name or ``None`` if not found.)doc"; -static const char *__doc_mplib_PlanningWorldTpl_getNormalObjectNames = +static const char *__doc_mplib_PlanningWorldTpl_getObjectNames = R"doc( -Gets names of all normal objects in world (unordered))doc"; +Gets names of all objects in world (unordered))doc"; static const char *__doc_mplib_PlanningWorldTpl_getPlannedArticulations = R"doc( @@ -289,11 +297,11 @@ Check whether the articulation with given name exists :param name: name of the articulated model :return: ``True`` if exists, ``False`` otherwise.)doc"; -static const char *__doc_mplib_PlanningWorldTpl_hasNormalObject = +static const char *__doc_mplib_PlanningWorldTpl_hasObject = R"doc( -Check whether the normal object with given name exists +Check whether the non-articulated object with given name exists -:param name: name of the normal object +:param name: name of the non-articulated object :return: ``True`` if exists, ``False`` otherwise.)doc"; static const char *__doc_mplib_PlanningWorldTpl_isArticulationPlanned = @@ -303,11 +311,11 @@ Check whether the articulation with given name is being planned :param name: name of the articulated model :return: ``True`` if exists, ``False`` otherwise.)doc"; -static const char *__doc_mplib_PlanningWorldTpl_isNormalObjectAttached = +static const char *__doc_mplib_PlanningWorldTpl_isObjectAttached = R"doc( -Check whether normal object with given name is attached +Check whether the non-articulated object with given name is attached -:param name: name of the normal object +:param name: name of the non-articulated object :return: ``True`` if it is attached, ``False`` otherwise.)doc"; static const char *__doc_mplib_PlanningWorldTpl_printAttachedBodyPose = @@ -322,14 +330,14 @@ Removes the articulation with given name if exists. Updates acm_ :return: ``True`` if success, ``False`` if articulation with given name does not exist)doc"; -static const char *__doc_mplib_PlanningWorldTpl_removeNormalObject = +static const char *__doc_mplib_PlanningWorldTpl_removeObject = R"doc( Removes (and detaches) the collision object with given name if exists. Updates acm_ :param name: name of the non-articulated collision object -:return: ``True`` if success, ``False`` if normal object with given name does - not exist)doc"; +:return: ``True`` if success, ``False`` if the non-articulated object with given + name does not exist)doc"; static const char *__doc_mplib_PlanningWorldTpl_selfCollide = R"doc( diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index d67d6c33..0e4e2ef7 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -31,8 +31,7 @@ void build_pyplanning_world(py::module &pymp) { PyPlanningWorld .def(py::init &, const std::vector &>(), - py::arg("articulations"), - py::arg("normal_objects") = std::vector(), + py::arg("articulations"), py::arg("objects") = std::vector(), DOC(mplib, PlanningWorldTpl, PlanningWorldTpl)) .def("get_articulation_names", &PlanningWorld::getArticulationNames, @@ -53,28 +52,28 @@ void build_pyplanning_world(py::module &pymp) { py::arg("name"), py::arg("planned"), DOC(mplib, PlanningWorldTpl, setArticulationPlanned)) - .def("get_normal_object_names", &PlanningWorld::getNormalObjectNames, - DOC(mplib, PlanningWorldTpl, getNormalObjectNames)) - .def("get_normal_object", &PlanningWorld::getNormalObject, py::arg("name"), - DOC(mplib, PlanningWorldTpl, getNormalObject)) - .def("has_normal_object", &PlanningWorld::hasNormalObject, py::arg("name"), - DOC(mplib, PlanningWorldTpl, hasNormalObject)) - .def("add_normal_object", - py::overload_cast(&PlanningWorld::addNormalObject), - py::arg("fcl_obj"), DOC(mplib, PlanningWorldTpl, addNormalObject)) - .def("add_normal_object", + .def("get_object_names", &PlanningWorld::getObjectNames, + DOC(mplib, PlanningWorldTpl, getObjectNames)) + .def("get_object", &PlanningWorld::getObject, py::arg("name"), + DOC(mplib, PlanningWorldTpl, getObject)) + .def("has_object", &PlanningWorld::hasObject, py::arg("name"), + DOC(mplib, PlanningWorldTpl, hasObject)) + .def("add_object", + py::overload_cast(&PlanningWorld::addObject), + py::arg("fcl_obj"), DOC(mplib, PlanningWorldTpl, addObject)) + .def("add_object", py::overload_cast( - &PlanningWorld::addNormalObject), + &PlanningWorld::addObject), py::arg("name"), py::arg("collision_object"), - DOC(mplib, PlanningWorldTpl, addNormalObject, 2)) + DOC(mplib, PlanningWorldTpl, addObject, 2)) .def("add_point_cloud", &PlanningWorld::addPointCloud, py::arg("name"), py::arg("vertices"), py::arg("resolution") = 0.01, DOC(mplib, PlanningWorldTpl, addPointCloud)) - .def("remove_normal_object", &PlanningWorld::removeNormalObject, py::arg("name"), - DOC(mplib, PlanningWorldTpl, removeNormalObject)) + .def("remove_object", &PlanningWorld::removeObject, py::arg("name"), + DOC(mplib, PlanningWorldTpl, removeObject)) - .def("is_normal_object_attached", &PlanningWorld::isNormalObjectAttached, - py::arg("name"), DOC(mplib, PlanningWorldTpl, isNormalObjectAttached)) + .def("is_object_attached", &PlanningWorld::isObjectAttached, py::arg("name"), + DOC(mplib, PlanningWorldTpl, isObjectAttached)) .def("get_attached_object", &PlanningWorld::getAttachedObject, py::arg("name"), DOC(mplib, PlanningWorldTpl, getAttachedObject)) .def("attach_object", diff --git a/src/planning_world.cpp b/src/planning_world.cpp index a48cf6db..98f0155c 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -17,12 +17,11 @@ DEFINE_TEMPLATE_PLANNING_WORLD(double); template PlanningWorldTpl::PlanningWorldTpl( const std::vector &articulations, - const std::vector &normal_objects) + const std::vector &objects) : acm_(std::make_shared()) { for (const auto &art : articulations) planned_articulation_map_[art->getName()] = articulation_map_[art->getName()] = art; - for (const auto &normal_object : normal_objects) - normal_object_map_[normal_object->name] = normal_object; + for (const auto &object : objects) object_map_[object->name] = object; } template @@ -71,16 +70,16 @@ void PlanningWorldTpl::setArticulationPlanned(const std::string &name, } template -std::vector PlanningWorldTpl::getNormalObjectNames() const { +std::vector PlanningWorldTpl::getObjectNames() const { std::vector names; - for (const auto &pair : normal_object_map_) names.push_back(pair.first); + for (const auto &pair : object_map_) names.push_back(pair.first); return names; } template -void PlanningWorldTpl::addNormalObject(const std::string &name, - const CollisionObjectPtr &collision_object) { - addNormalObject( +void PlanningWorldTpl::addObject(const std::string &name, + const CollisionObjectPtr &collision_object) { + addObject( std::make_shared(name, Pose(collision_object->getTransform()), std::vector {collision_object}, std::vector> {Pose()})); @@ -93,13 +92,13 @@ void PlanningWorldTpl::addPointCloud(const std::string &name, auto tree = std::make_shared(resolution); for (const auto &row : vertices.rowwise()) tree->updateNode(octomap::point3d(row(0), row(1), row(2)), true); - addNormalObject( - name, std::make_shared(std::make_shared>(tree))); + addObject(name, + std::make_shared(std::make_shared>(tree))); } template -bool PlanningWorldTpl::removeNormalObject(const std::string &name) { - auto nh = normal_object_map_.extract(name); +bool PlanningWorldTpl::removeObject(const std::string &name) { + auto nh = object_map_.extract(name); if (nh.empty()) return false; attached_body_map_.erase(name); // Update acm_ @@ -112,7 +111,7 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, const std::vector &touch_links) { - const auto T_world_obj = normal_object_map_.at(name)->pose; + const auto T_world_obj = object_map_.at(name)->pose; const auto T_world_link = planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj), @@ -122,7 +121,7 @@ void PlanningWorldTpl::attachObject(const std::string &name, template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id) { - const auto T_world_obj = normal_object_map_.at(name)->pose; + const auto T_world_obj = object_map_.at(name)->pose; const auto T_world_link = planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj)); @@ -133,7 +132,7 @@ void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, const Pose &pose, const std::vector &touch_links) { - auto obj = normal_object_map_.at(name); + auto obj = object_map_.at(name); auto nh = attached_body_map_.extract(name); auto body = std::make_shared(name, obj, planned_articulation_map_.at(art_name), @@ -153,7 +152,7 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, const Pose &pose) { - auto obj = normal_object_map_.at(name); + auto obj = object_map_.at(name); auto nh = attached_body_map_.extract(name); auto body = std::make_shared( name, obj, planned_articulation_map_.at(art_name), link_id, pose.toIsometry()); @@ -183,8 +182,8 @@ void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id, const Pose &pose, const std::vector &touch_links) { - removeNormalObject(name); - addNormalObject(name, std::make_shared(p_geom)); + removeObject(name); + addObject(name, std::make_shared(p_geom)); attachObject(name, art_name, link_id, pose, touch_links); } @@ -193,8 +192,8 @@ void PlanningWorldTpl::attachObject(const std::string &name, const CollisionGeometryPtr &p_geom, const std::string &art_name, int link_id, const Pose &pose) { - removeNormalObject(name); - addNormalObject(name, std::make_shared(p_geom)); + removeObject(name); + addObject(name, std::make_shared(p_geom)); attachObject(name, art_name, link_id, pose); } @@ -229,7 +228,7 @@ void PlanningWorldTpl::attachMesh(const std::string &mesh_path, template bool PlanningWorldTpl::detachObject(const std::string &name, bool also_remove) { if (also_remove) { - normal_object_map_.erase(name); + object_map_.erase(name); // Update acm_ acm_->removeEntry(name); acm_->removeDefaultEntry(name); @@ -387,7 +386,7 @@ std::vector> PlanningWorldTpl::collideWithOthers( for (const auto &[name, art] : articulation_map_) if (planned_articulation_map_.find(name) == planned_articulation_map_.end()) unplanned_articulations.push_back(art); - for (const auto &[name, obj] : normal_object_map_) + for (const auto &[name, obj] : object_map_) if (attached_body_map_.find(name) == attached_body_map_.end()) scene_objects.push_back(obj); @@ -592,7 +591,7 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceOthers( for (const auto &[name, art] : articulation_map_) if (planned_articulation_map_.find(name) == planned_articulation_map_.end()) unplanned_articulations.push_back(art); - for (const auto &[name, obj] : normal_object_map_) + for (const auto &[name, obj] : object_map_) if (attached_body_map_.find(name) == attached_body_map_.end()) scene_objects.push_back(obj); From d4810546413a2d7d86e265c1374fe978183e5341 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 16:46:11 -0700 Subject: [PATCH 10/37] Rename collision/distance method names to align with MoveIt2 * PlanningWorld::collide() => PlanningWorld::isStateColliding() * PlanningWorld::selfCollide() => PlanningWorld::checkSelfCollision() * PlanningWorld::collideWithOthers() => PlanningWorld::checkRobotCollision() * PlanningWorld::collideFull() => PlanningWorld::checkCollision() * PlanningWorld::distance() => PlanningWorld::distanceToCollision() * PlanningWorld::distanceOthers() => PlanningWorld::distanceRobot() * PlanningWorld::distanceFull() => PlanningWorld::distance() * FCLModel::collideFull() => FCLModel::checkSelfCollision() * FCLModel::collide() => FCLModel::isStateColliding() * Change FCLModel::collideFull() default from GJKSolverType::GST_INDEP to GJKSolverType::GST_LIBCCD --- .../mplib/collision_detection/fcl/fcl_model.h | 9 +- .../mplib/planning/ompl/validity_checker.h | 2 +- include/mplib/planning_world.h | 78 +++++++++----- mplib/planner.py | 12 ++- mplib/pymp/__init__.pyi | 100 +++++++++++------- .../fcl/pybind_fcl_model.cpp | 8 +- .../collision_detection/fcl/fcl_model.h | 15 ++- pybind/docstring/planning_world.h | 86 +++++++++------ pybind/pybind_planning_world.cpp | 35 +++--- src/collision_detection/fcl/fcl_model.cpp | 5 +- src/planning_world.cpp | 20 ++-- 11 files changed, 216 insertions(+), 154 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 855b4d28..b3b34103 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -136,11 +136,9 @@ class FCLModelTpl { /** * Perform self-collision checking. * - * @param request: collision request * @return: ``true`` if any collision pair collides and ``false`` otherwise. */ - bool collide( - const fcl::CollisionRequest &request = fcl::CollisionRequest()) const; + bool isStateColliding() const; /** * Perform self-collision checking and returns all found collisions. @@ -148,9 +146,8 @@ class FCLModelTpl { * @param request: collision request * @return: list of CollisionResult for each collision pair */ - std::vector> collideFull( - const fcl::CollisionRequest &request = fcl::CollisionRequest( - 1, false, 1, false, true, fcl::GJKSolverType::GST_INDEP, 1e-6)) const; + std::vector> checkSelfCollision( + const fcl::CollisionRequest &request = fcl::CollisionRequest()) const; private: void init(const urdf::ModelInterfaceSharedPtr &urdf_model, diff --git a/include/mplib/planning/ompl/validity_checker.h b/include/mplib/planning/ompl/validity_checker.h index 7a2395e6..9f69fb96 100644 --- a/include/mplib/planning/ompl/validity_checker.h +++ b/include/mplib/planning/ompl/validity_checker.h @@ -25,7 +25,7 @@ class ValidityCheckerTpl : public ob::StateValidityChecker { bool _isValid(const VectorX &state) const { world_->setQposAll(addFixedJoints(fixed_joints_, state)); - return !world_->collide(); + return !world_->isStateColliding(); } bool isValid(const ob::State *state_raw) const { diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 1601cd9b..5cd4cc65 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -21,11 +21,13 @@ MPLIB_CLASS_TEMPLATE_FORWARD(PlanningWorldTpl); * Planning world for collision checking * * Mimicking MoveIt2's ``planning_scene::PlanningScene``, - * ``collision_detection::World``, ``moveit::core::RobotState`` + * ``collision_detection::World``, ``moveit::core::RobotState``, + * ``collision_detection::CollisionEnv`` * * https://moveit.picknik.ai/main/api/html/classplanning__scene_1_1PlanningScene.html * https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1World.html * https://moveit.picknik.ai/main/api/html/classmoveit_1_1core_1_1RobotState.html + * https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1CollisionEnv.html */ template class PlanningWorldTpl { @@ -372,80 +374,98 @@ class PlanningWorldTpl { /// @brief Get the current allowed collision matrix AllowedCollisionMatrixPtr getAllowedCollisionMatrix() const { return acm_; } - // TODO(merge): rename collision/distance function name to align with MoveIt2 /** - * Check full collision and return only a boolean indicating collision + * Check if the current state is in collision (with the environment or self + * collision). * - * @param request: collision request params. * @return: ``true`` if collision exists */ - bool collide(const CollisionRequest &request = CollisionRequest()) const { - return collideFull(request).size() > 0; + bool isStateColliding() const { + return checkCollision(CollisionRequest()).size() > 0; } /** - * Check self collision (including planned articulation self-collision, + * Check for self collision (including planned articulation self-collision, * planned articulation-attach collision, attach-attach collision) * * @param request: collision request params. - * @return: List of WorldCollisionResult objects + * @return: List of ``WorldCollisionResult`` objects */ - std::vector selfCollide( + std::vector checkSelfCollision( const CollisionRequest &request = CollisionRequest()) const; + /** - * Check collision with other scene bodies (planned articulations with + * Check collision with other scene bodies in the world (planned articulations with * attached objects collide against unplanned articulations and scene objects) * * @param request: collision request params. - * @return: List of WorldCollisionResult objects + * @return: List of ``WorldCollisionResult`` objects */ - std::vector collideWithOthers( + std::vector checkRobotCollision( const CollisionRequest &request = CollisionRequest()) const; /** - * Check full collision (calls selfCollide() and collideWithOthers()) + * Check full collision (calls ``checkSelfCollision()`` and ``checkRobotCollision()``) * * @param request: collision request params. - * @return: List of WorldCollisionResult objects + * @return: List of ``WorldCollisionResult`` objects */ - std::vector collideFull( + std::vector checkCollision( const CollisionRequest &request = CollisionRequest()) const; /** - * Returns the minimum distance-to-collision in current state + * The minimum distance to self-collision given the robot in current state. + * Calls ``distanceSelf()``. * - * @param request: distance request params. - * @return: minimum distance-to-collision + * @return: minimum distance-to-self-collision */ - S distance(const DistanceRequest &request = DistanceRequest()) const { - return distanceFull().min_distance; - } + S distanceToSelfCollision() const { return distanceSelf().min_distance; } /** - * Get the min distance to self-collision given the robot in current state + * Get the minimum distance to self-collision given the robot in current state * * @param request: distance request params. - * @return: a WorldDistanceResult object + * @return: a ``WorldDistanceResult`` object */ WorldDistanceResult distanceSelf( const DistanceRequest &request = DistanceRequest()) const; /** - * Compute the min distance between a robot and the world + * The distance between the robot model at current state to the nearest collision + * (ignoring self-collisions). Calls ``distanceRobot()``. + * + * @return: minimum distance-to-robot-collision + */ + S distanceToRobotCollision() const { return distanceRobot().min_distance; } + + /** + * Compute the minimum distance-to-collision between a robot and the world * * @param request: distance request params. - * @return: a WorldDistanceResult object + * @return: a ``WorldDistanceResult`` object */ - WorldDistanceResult distanceOthers( + WorldDistanceResult distanceRobot( const DistanceRequest &request = DistanceRequest()) const; /** - * Compute the min distance to collision (calls distanceSelf() and distanceOthers()) + * Compute the minimum distance-to-all-collision. Calls ``distance()``. + * + * Note that this is different from MoveIt2's + * ``planning_scene::PlanningScene::distanceToCollision()`` where self-collisions are + * ignored. + * + * @return: minimum distance-to-all-collision + */ + S distanceToCollision() const { return distance().min_distance; } + + /** + * Compute the minimum distance-to-all-collision (calls ``distanceSelf()`` and + * ``distanceRobot()``) * * @param request: distance request params. - * @return: a WorldDistanceResult object + * @return: a ``WorldDistanceResult`` object */ - WorldDistanceResult distanceFull( + WorldDistanceResult distance( const DistanceRequest &request = DistanceRequest()) const; private: diff --git a/mplib/planner.py b/mplib/planner.py index e4ac1df9..9a8e452a 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -333,7 +333,7 @@ def check_for_self_collision( :param state: all planned articulations qpos state. If None, use current qpos. :return: A list of collisions. """ - return self.check_for_collision(self.planning_world.self_collide, state) + return self.check_for_collision(self.planning_world.check_self_collision, state) def check_for_env_collision( self, @@ -345,7 +345,9 @@ def check_for_env_collision( :param state: all planned articulations qpos state. If None, use current qpos. :return: A list of collisions. """ - return self.check_for_collision(self.planning_world.collide_with_others, state) + return self.check_for_collision( + self.planning_world.check_robot_collision, state + ) def IK( self, @@ -399,7 +401,7 @@ def IK( if success: # check collision self.planning_world.set_qpos_all(ik_qpos[move_joint_idx]) - if len(collisions := self.planning_world.collide_full()) > 0: + if len(collisions := self.planning_world.check_collision()) > 0: success = False if verbose: for collision in collisions: @@ -677,7 +679,7 @@ def plan_qpos_to_qpos( current_qpos = self.pad_move_group_qpos(current_qpos) self.robot.set_qpos(current_qpos, True) - collisions = self.planning_world.collide_full() + collisions = self.planning_world.check_collision() if len(collisions) > 0: print("Invalid start state!") for collision in collisions: @@ -924,7 +926,7 @@ def check_joint_limit(q): within_joint_limit = check_joint_limit(current_qpos) self.planning_world.set_qpos_all(current_qpos[move_joint_idx]) - collide = self.planning_world.collide() + collide = self.planning_world.is_state_colliding() if np.linalg.norm(delta_twist) < 1e-4 or collide or not within_joint_limit: return {"status": "screw plan failed"} diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 4e1ae40b..ae226c5c 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -317,11 +317,13 @@ class PlanningWorld: Planning world for collision checking Mimicking MoveIt2's ``planning_scene::PlanningScene``, - ``collision_detection::World``, ``moveit::core::RobotState`` + ``collision_detection::World``, ``moveit::core::RobotState``, + ``collision_detection::CollisionEnv`` https://moveit.picknik.ai/main/api/html/classplanning__scene_1_1PlanningScene.html https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1World.html https://moveit.picknik.ai/main/api/html/classmoveit_1_1core_1_1RobotState.html + https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1CollisionEnv.html """ def __init__( self, @@ -533,31 +535,35 @@ class PlanningWorld: :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) """ - def collide(self, request: collision_detection.fcl.CollisionRequest = ...) -> bool: + def check_collision( + self, request: collision_detection.fcl.CollisionRequest = ... + ) -> list[collision_detection.WorldCollisionResult]: """ - Check full collision and return only a boolean indicating collision + Check full collision (calls ``checkSelfCollision()`` and + ``checkRobotCollision()``) :param request: collision request params. - :return: ``True`` if collision exists + :return: List of ``WorldCollisionResult`` objects """ - def collide_full( + def check_robot_collision( self, request: collision_detection.fcl.CollisionRequest = ... ) -> list[collision_detection.WorldCollisionResult]: """ - Check full collision (calls selfCollide() and collideWithOthers()) + Check collision with other scene bodies in the world (planned articulations with + attached objects collide against unplanned articulations and scene objects) :param request: collision request params. - :return: List of WorldCollisionResult objects + :return: List of ``WorldCollisionResult`` objects """ - def collide_with_others( + def check_self_collision( self, request: collision_detection.fcl.CollisionRequest = ... ) -> list[collision_detection.WorldCollisionResult]: """ - Check collision with other scene bodies (planned articulations with attached - objects collide against unplanned articulations and scene objects) + Check for self collision (including planned articulation self-collision, planned + articulation-attach collision, attach-attach collision) :param request: collision request params. - :return: List of WorldCollisionResult objects + :return: List of ``WorldCollisionResult`` objects """ def detach_object(self, name: str, also_remove: bool = False) -> bool: """ @@ -569,31 +575,57 @@ class PlanningWorld: :return: ``True`` if success, ``False`` if the object with given name is not attached """ - def distance(self, request: collision_detection.fcl.DistanceRequest = ...) -> float: + def distance( + self, request: collision_detection.fcl.DistanceRequest = ... + ) -> collision_detection.WorldDistanceResult: """ - Returns the minimum distance-to-collision in current state + Compute the minimum distance-to-all-collision (calls ``distanceSelf()`` and + ``distanceRobot()``) :param request: distance request params. - :return: minimum distance-to-collision + :return: a ``WorldDistanceResult`` object """ - def distance_full( + def distance_robot( self, request: collision_detection.fcl.DistanceRequest = ... ) -> collision_detection.WorldDistanceResult: """ - Compute the min distance to collision (calls distanceSelf() and - distanceOthers()) + Compute the minimum distance-to-collision between a robot and the world :param request: distance request params. - :return: a WorldDistanceResult object + :return: a ``WorldDistanceResult`` object """ - def distance_with_others( + def distance_self( self, request: collision_detection.fcl.DistanceRequest = ... ) -> collision_detection.WorldDistanceResult: """ - Compute the min distance between a robot and the world + Get the minimum distance to self-collision given the robot in current state :param request: distance request params. - :return: a WorldDistanceResult object + :return: a ``WorldDistanceResult`` object + """ + def distance_to_collision(self) -> float: + """ + Compute the minimum distance-to-all-collision. Calls ``distance()``. + + Note that this is different from MoveIt2's + ``planning_scene::PlanningScene::distanceToCollision()`` where self-collisions + are ignored. + + :return: minimum distance-to-collision + """ + def distance_to_robot_collision(self) -> float: + """ + The distance between the robot model at current state to the nearest collision + (ignoring self-collisions). Calls ``distanceRobot()``. + + :return: minimum distance-to-collision + """ + def distance_to_self_collision(self) -> float: + """ + The minimum distance to self-collision given the robot in current state. Calls + ``distanceSelf()``. + + :return: minimum distance-to-collision """ def get_allowed_collision_matrix( self, @@ -662,6 +694,13 @@ class PlanningWorld: :param name: name of the non-articulated object :return: ``True`` if it is attached, ``False`` otherwise. """ + def is_state_colliding(self) -> bool: + """ + Check if the current state is in collision (with the environment or self + collision). + + :return: ``True`` if collision exists + """ def print_attached_body_pose(self) -> None: """ Prints global pose of all attached bodies @@ -683,25 +722,6 @@ class PlanningWorld: :return: ``True`` if success, ``False`` if the non-articulated object with given name does not exist """ - def self_collide( - self, request: collision_detection.fcl.CollisionRequest = ... - ) -> list[collision_detection.WorldCollisionResult]: - """ - Check self collision (including planned articulation self-collision, planned - articulation-attach collision, attach-attach collision) - - :param request: collision request params. - :return: List of WorldCollisionResult objects - """ - def self_distance( - self, request: collision_detection.fcl.DistanceRequest = ... - ) -> collision_detection.WorldDistanceResult: - """ - Get the min distance to self-collision given the robot in current state - - :param request: distance request params. - :return: a WorldDistanceResult object - """ def set_articulation_planned(self, name: str, planned: bool) -> None: """ Sets articulation with given name as being planned diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index 24acfc44..06bd2681 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -64,11 +64,11 @@ void build_pyfcl_model(py::module &m) { py::arg("link_poses"), DOC(mplib, collision_detection, fcl, FCLModelTpl, updateCollisionObjects)) - .def("collide", &FCLModel::collide, py::arg("request") = CollisionRequest(), - DOC(mplib, collision_detection, fcl, FCLModelTpl, collide)) - .def("collide_full", &FCLModel::collideFull, + .def("is_state_colliding", &FCLModel::isStateColliding, + DOC(mplib, collision_detection, fcl, FCLModelTpl, isStateColliding)) + .def("check_self_collision", &FCLModel::checkSelfCollision, py::arg("request") = CollisionRequest(), - DOC(mplib, collision_detection, fcl, FCLModelTpl, collideFull)); + DOC(mplib, collision_detection, fcl, FCLModelTpl, checkSelfCollision)); } } // namespace mplib::collision_detection::fcl diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index 5a245b20..ddd7984b 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -49,14 +49,7 @@ Construct an FCL model from URDF and SRDF files. ``False``. :param verbose: print debug information. Default: ``False``.)doc"; -static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_collide = -R"doc( -Perform self-collision checking. - -:param request: collision request -:return: ``True`` if any collision pair collides and ``False`` otherwise.)doc"; - -static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_collideFull = +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkSelfCollision = R"doc( Perform self-collision checking and returns all found collisions. @@ -106,6 +99,12 @@ static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_getUserLinkNa R"doc( )doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_isStateColliding = +R"doc( +Perform self-collision checking. + +:return: ``True`` if any collision pair collides and ``False`` otherwise.)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_printCollisionPairs = R"doc( Print all collision pairs)doc"; diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 56ea367f..3f3c4ae6 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -28,11 +28,13 @@ static const char *__doc_mplib_PlanningWorldTpl = R"doc(Planning world for collision checking Mimicking MoveIt2's ``planning_scene::PlanningScene``, -``collision_detection::World``, ``moveit::core::RobotState`` +``collision_detection::World``, ``moveit::core::RobotState``, +``collision_detection::CollisionEnv`` https://moveit.picknik.ai/main/api/html/classplanning__scene_1_1PlanningScene.html https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1World.html -https://moveit.picknik.ai/main/api/html/classmoveit_1_1core_1_1RobotState.html)doc"; +https://moveit.picknik.ai/main/api/html/classmoveit_1_1core_1_1RobotState.html +https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1CollisionEnv.html)doc"; static const char *__doc_mplib_PlanningWorldTpl_PlanningWorldTpl = R"doc( @@ -192,27 +194,29 @@ Attaches given sphere to specified link of articulation (auto touch_links) :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"; -static const char *__doc_mplib_PlanningWorldTpl_collide = +static const char *__doc_mplib_PlanningWorldTpl_checkCollision = R"doc( -Check full collision and return only a boolean indicating collision +Check full collision (calls ``checkSelfCollision()`` and +``checkRobotCollision()``) :param request: collision request params. -:return: ``True`` if collision exists)doc"; +:return: List of ``WorldCollisionResult`` objects)doc"; -static const char *__doc_mplib_PlanningWorldTpl_collideFull = +static const char *__doc_mplib_PlanningWorldTpl_checkRobotCollision = R"doc( -Check full collision (calls selfCollide() and collideWithOthers()) +Check collision with other scene bodies in the world (planned articulations with +attached objects collide against unplanned articulations and scene objects) :param request: collision request params. -:return: List of WorldCollisionResult objects)doc"; +:return: List of ``WorldCollisionResult`` objects)doc"; -static const char *__doc_mplib_PlanningWorldTpl_collideWithOthers = +static const char *__doc_mplib_PlanningWorldTpl_checkSelfCollision = R"doc( -Check collision with other scene bodies (planned articulations with attached -objects collide against unplanned articulations and scene objects) +Check for self collision (including planned articulation self-collision, planned +articulation-attach collision, attach-attach collision) :param request: collision request params. -:return: List of WorldCollisionResult objects)doc"; +:return: List of ``WorldCollisionResult`` objects)doc"; static const char *__doc_mplib_PlanningWorldTpl_detachObject = R"doc( @@ -226,32 +230,49 @@ object and touch_links. static const char *__doc_mplib_PlanningWorldTpl_distance = R"doc( -Returns the minimum distance-to-collision in current state +Compute the minimum distance-to-all-collision (calls ``distanceSelf()`` and +``distanceRobot()``) :param request: distance request params. -:return: minimum distance-to-collision)doc"; +:return: a ``WorldDistanceResult`` object)doc"; -static const char *__doc_mplib_PlanningWorldTpl_distanceFull = +static const char *__doc_mplib_PlanningWorldTpl_distanceRobot = R"doc( -Compute the min distance to collision (calls distanceSelf() and -distanceOthers()) +Compute the minimum distance-to-collision between a robot and the world :param request: distance request params. -:return: a WorldDistanceResult object)doc"; +:return: a ``WorldDistanceResult`` object)doc"; -static const char *__doc_mplib_PlanningWorldTpl_distanceOthers = +static const char *__doc_mplib_PlanningWorldTpl_distanceSelf = R"doc( -Compute the min distance between a robot and the world +Get the minimum distance to self-collision given the robot in current state :param request: distance request params. -:return: a WorldDistanceResult object)doc"; +:return: a ``WorldDistanceResult`` object)doc"; -static const char *__doc_mplib_PlanningWorldTpl_distanceSelf = +static const char *__doc_mplib_PlanningWorldTpl_distanceToCollision = R"doc( -Get the min distance to self-collision given the robot in current state +Compute the minimum distance-to-all-collision. Calls ``distance()``. -:param request: distance request params. -:return: a WorldDistanceResult object)doc"; +Note that this is different from MoveIt2's +``planning_scene::PlanningScene::distanceToCollision()`` where self-collisions +are ignored. + +:return: minimum distance-to-collision)doc"; + +static const char *__doc_mplib_PlanningWorldTpl_distanceToRobotCollision = +R"doc( +The distance between the robot model at current state to the nearest collision +(ignoring self-collisions). Calls ``distanceRobot()``. + +:return: minimum distance-to-collision)doc"; + +static const char *__doc_mplib_PlanningWorldTpl_distanceToSelfCollision = +R"doc( +The minimum distance to self-collision given the robot in current state. Calls +``distanceSelf()``. + +:return: minimum distance-to-collision)doc"; static const char *__doc_mplib_PlanningWorldTpl_getAllowedCollisionMatrix = R"doc( @@ -318,6 +339,13 @@ Check whether the non-articulated object with given name is attached :param name: name of the non-articulated object :return: ``True`` if it is attached, ``False`` otherwise.)doc"; +static const char *__doc_mplib_PlanningWorldTpl_isStateColliding = +R"doc( +Check if the current state is in collision (with the environment or self +collision). + +:return: ``True`` if collision exists)doc"; + static const char *__doc_mplib_PlanningWorldTpl_printAttachedBodyPose = R"doc( Prints global pose of all attached bodies)doc"; @@ -339,14 +367,6 @@ acm_ :return: ``True`` if success, ``False`` if the non-articulated object with given name does not exist)doc"; -static const char *__doc_mplib_PlanningWorldTpl_selfCollide = -R"doc( -Check self collision (including planned articulation self-collision, planned -articulation-attach collision, attach-attach collision) - -:param request: collision request params. -:return: List of WorldCollisionResult objects)doc"; - static const char *__doc_mplib_PlanningWorldTpl_setArticulationPlanned = R"doc( Sets articulation with given name as being planned diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 0e4e2ef7..13b5029b 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -134,29 +134,32 @@ void build_pyplanning_world(py::module &pymp) { .def("get_allowed_collision_matrix", &PlanningWorld::getAllowedCollisionMatrix, DOC(mplib, PlanningWorldTpl, getAllowedCollisionMatrix)) - .def("collide", &PlanningWorld::collide, py::arg("request") = CollisionRequest(), - DOC(mplib, PlanningWorldTpl, collide)) - .def("self_collide", &PlanningWorld::selfCollide, + .def("is_state_colliding", &PlanningWorld::isStateColliding, + DOC(mplib, PlanningWorldTpl, isStateColliding)) + .def("check_self_collision", &PlanningWorld::checkSelfCollision, py::arg("request") = CollisionRequest(), - DOC(mplib, PlanningWorldTpl, selfCollide)) - .def("collide_with_others", &PlanningWorld::collideWithOthers, + DOC(mplib, PlanningWorldTpl, checkSelfCollision)) + .def("check_robot_collision", &PlanningWorld::checkRobotCollision, py::arg("request") = CollisionRequest(), - DOC(mplib, PlanningWorldTpl, collideWithOthers)) - .def("collide_full", &PlanningWorld::collideFull, + DOC(mplib, PlanningWorldTpl, checkRobotCollision)) + .def("check_collision", &PlanningWorld::checkCollision, py::arg("request") = CollisionRequest(), - DOC(mplib, PlanningWorldTpl, collideFull)) + DOC(mplib, PlanningWorldTpl, checkCollision)) - .def("distance", &PlanningWorld::distance, py::arg("request") = DistanceRequest(), - DOC(mplib, PlanningWorldTpl, distance)) - .def("self_distance", &PlanningWorld::distanceSelf, + .def("distance_to_self_collision", &PlanningWorld::distanceToSelfCollision, + DOC(mplib, PlanningWorldTpl, distanceToSelfCollision)) + .def("distance_self", &PlanningWorld::distanceSelf, py::arg("request") = DistanceRequest(), DOC(mplib, PlanningWorldTpl, distanceSelf)) - .def("distance_with_others", &PlanningWorld::distanceOthers, - py::arg("request") = DistanceRequest(), - DOC(mplib, PlanningWorldTpl, distanceOthers)) - .def("distance_full", &PlanningWorld::distanceFull, + .def("distance_to_robot_collision", &PlanningWorld::distanceToRobotCollision, + DOC(mplib, PlanningWorldTpl, distanceToRobotCollision)) + .def("distance_robot", &PlanningWorld::distanceRobot, py::arg("request") = DistanceRequest(), - DOC(mplib, PlanningWorldTpl, distanceFull)); + DOC(mplib, PlanningWorldTpl, distanceRobot)) + .def("distance_to_collision", &PlanningWorld::distanceToCollision, + DOC(mplib, PlanningWorldTpl, distanceToCollision)) + .def("distance", &PlanningWorld::distance, py::arg("request") = DistanceRequest(), + DOC(mplib, PlanningWorldTpl, distance)); } } // namespace mplib diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index c14453fa..4b8aafc1 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -236,7 +236,8 @@ void FCLModelTpl::updateCollisionObjects( } template -bool FCLModelTpl::collide(const fcl::CollisionRequest &request) const { +bool FCLModelTpl::isStateColliding() const { + fcl::CollisionRequest request = fcl::CollisionRequest(); fcl::CollisionResult result; for (const auto &[i, j] : collision_pairs_) { collision_detection::fcl::collide(collision_objects_[i], collision_objects_[j], @@ -247,7 +248,7 @@ bool FCLModelTpl::collide(const fcl::CollisionRequest &request) const { } template -std::vector> FCLModelTpl::collideFull( +std::vector> FCLModelTpl::checkSelfCollision( const fcl::CollisionRequest &request) const { // TODO(merge): return only CollisionResult in collision? // Result will be returned via the collision result structure diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 98f0155c..42d0a353 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -164,7 +164,7 @@ void PlanningWorldTpl::attachObject(const std::string &name, attached_body_map_[name] = body; // Set touch_links to the name of self links colliding with object currently std::vector touch_links; - auto collisions = selfCollide(); + auto collisions = checkSelfCollision(); for (const auto &collision : collisions) if (collision.link_name1 == name) touch_links.push_back(collision.link_name2); @@ -282,7 +282,7 @@ std::vector> PlanningWorldTpl::filterCollisions( } template -std::vector> PlanningWorldTpl::selfCollide( +std::vector> PlanningWorldTpl::checkSelfCollision( const CollisionRequest &request) const { std::vector ret; CollisionResult result; @@ -297,7 +297,7 @@ std::vector> PlanningWorldTpl::selfCollide( auto col_pairs = fcl_model->getCollisionPairs(); // Articulation self-collision - auto results = fcl_model->collideFull(request); + auto results = fcl_model->checkSelfCollision(request); for (size_t i = 0; i < results.size(); i++) if (results[i].isCollision()) { WorldCollisionResult tmp; @@ -373,7 +373,7 @@ std::vector> PlanningWorldTpl::selfCollide( } template -std::vector> PlanningWorldTpl::collideWithOthers( +std::vector> PlanningWorldTpl::checkRobotCollision( const CollisionRequest &request) const { std::vector ret; CollisionResult result; @@ -471,10 +471,10 @@ std::vector> PlanningWorldTpl::collideWithOthers( } template -std::vector> PlanningWorldTpl::collideFull( +std::vector> PlanningWorldTpl::checkCollision( const CollisionRequest &request) const { - auto ret1 = selfCollide(request); - const auto ret2 = collideWithOthers(request); + auto ret1 = checkSelfCollision(request); + const auto ret2 = checkRobotCollision(request); ret1.insert(ret1.end(), ret2.begin(), ret2.end()); return ret1; } @@ -578,7 +578,7 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( } template -WorldDistanceResultTpl PlanningWorldTpl::distanceOthers( +WorldDistanceResultTpl PlanningWorldTpl::distanceRobot( const DistanceRequest &request) const { WorldDistanceResult ret; DistanceResult result; @@ -680,10 +680,10 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceOthers( } template -WorldDistanceResultTpl PlanningWorldTpl::distanceFull( +WorldDistanceResultTpl PlanningWorldTpl::distance( const DistanceRequest &request) const { const auto ret1 = distanceSelf(request); - const auto ret2 = distanceOthers(request); + const auto ret2 = distanceRobot(request); return ret1.min_distance < ret2.min_distance ? ret1 : ret2; } From c97506ba7569c12703b7ce8cea331fe4be8ed549 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 19:28:06 -0700 Subject: [PATCH 11/37] Update FCLModel::checkSelfCollision to return WorldCollisionResult * Update fcl_model name when articulated_model's name is overridden --- .../mplib/collision_detection/fcl/fcl_model.h | 23 +++++++++++--- mplib/pymp/collision_detection/fcl.pyi | 22 +++++++------ .../collision_detection/fcl/fcl_model.h | 12 +++++-- src/collision_detection/fcl/fcl_model.cpp | 31 +++++++++---------- src/core/articulated_model.cpp | 2 ++ src/planning_world.cpp | 17 ++-------- 6 files changed, 57 insertions(+), 50 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index b3b34103..10fcc6df 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -10,6 +10,7 @@ #include #include +#include "mplib/collision_detection/collision_common.h" #include "mplib/collision_detection/fcl/collision_common.h" #include "mplib/macros/class_forward.h" #include "mplib/utils/pose.h" @@ -26,6 +27,9 @@ MPLIB_CLASS_TEMPLATE_FORWARD(FCLModelTpl); */ template class FCLModelTpl { + // Common type alias + using WorldCollisionResult = WorldCollisionResultTpl; + public: /** * Construct an FCL model from URDF and SRDF files. @@ -71,6 +75,13 @@ class FCLModelTpl { */ const std::string &getName() const { return name_; } + /** + * Set name of the articulated model. + * + * @param name: name of the articulated model + */ + void setName(const std::string &name) { name_ = name; } + /** * Get the collision objects of the FCL model. * @@ -134,19 +145,21 @@ class FCLModelTpl { void updateCollisionObjects(const std::vector> &link_poses) const; /** - * Perform self-collision checking. + * Check if the current state is in self-collision * * @return: ``true`` if any collision pair collides and ``false`` otherwise. */ - bool isStateColliding() const; + bool isStateColliding() const { + return checkSelfCollision(fcl::CollisionRequest()).size() > 0; + } /** - * Perform self-collision checking and returns all found collisions. + * Check for self-collision in the current state and returns all found collisions. * * @param request: collision request - * @return: list of CollisionResult for each collision pair + * @return: List of ``WorldCollisionResult`` objects. If empty, no self-collision. */ - std::vector> checkSelfCollision( + std::vector checkSelfCollision( const fcl::CollisionRequest &request = fcl::CollisionRequest()) const; private: diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index 20a4b5aa..85a0d71e 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -7,6 +7,7 @@ import typing import numpy import mplib.pymp +import mplib.pymp.collision_detection __all__ = [ "BVHModel", @@ -579,19 +580,14 @@ class FCLModel: ``False``. :param verbose: print debug information. Default: ``False``. """ - def collide(self, request: CollisionRequest = ...) -> bool: + def check_self_collision( + self, request: CollisionRequest = ... + ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: """ - Perform self-collision checking. + Check for self-collision in the current state and returns all found collisions. :param request: collision request - :return: ``True`` if any collision pair collides and ``False`` otherwise. - """ - def collide_full(self, request: CollisionRequest = ...) -> list[CollisionResult]: - """ - Perform self-collision checking and returns all found collisions. - - :param request: collision request - :return: list of CollisionResult for each collision pair + :return: List of ``WorldCollisionResult`` objects. If empty, no self-collision. """ def get_collision_link_names(self) -> list[str]: ... def get_collision_objects(self) -> list[...]: @@ -614,6 +610,12 @@ class FCLModel: :return: name of the articulated model """ + def is_state_colliding(self) -> bool: + """ + Check if the current state is in self-collision + + :return: ``True`` if any collision pair collides and ``False`` otherwise. + """ def remove_collision_pairs_from_srdf(self, srdf_filename: str) -> None: """ Remove collision pairs from SRDF file. diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index ddd7984b..9be98714 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -51,10 +51,10 @@ Construct an FCL model from URDF and SRDF files. static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkSelfCollision = R"doc( -Perform self-collision checking and returns all found collisions. +Check for self-collision in the current state and returns all found collisions. :param request: collision request -:return: list of CollisionResult for each collision pair)doc"; +:return: List of ``WorldCollisionResult`` objects. If empty, no self-collision.)doc"; static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_createFromURDFString = R"doc( @@ -101,7 +101,7 @@ R"doc( static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_isStateColliding = R"doc( -Perform self-collision checking. +Check if the current state is in self-collision :return: ``True`` if any collision pair collides and ``False`` otherwise.)doc"; @@ -128,6 +128,12 @@ Set the link order of the FCL model. :param names: list of link names in the order that you want to set.)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_setName = +R"doc( +Set name of the articulated model. + +:param name: name of the articulated model)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_updateCollisionObjects = R"doc( Update the collision objects of the FCL model. diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 4b8aafc1..2b4af4c6 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -236,28 +236,25 @@ void FCLModelTpl::updateCollisionObjects( } template -bool FCLModelTpl::isStateColliding() const { - fcl::CollisionRequest request = fcl::CollisionRequest(); +std::vector> FCLModelTpl::checkSelfCollision( + const fcl::CollisionRequest &request) const { + std::vector ret; fcl::CollisionResult result; - for (const auto &[i, j] : collision_pairs_) { - collision_detection::fcl::collide(collision_objects_[i], collision_objects_[j], - request, result); - if (result.isCollision()) return true; - } - return false; -} -template -std::vector> FCLModelTpl::checkSelfCollision( - const fcl::CollisionRequest &request) const { - // TODO(merge): return only CollisionResult in collision? - // Result will be returned via the collision result structure - std::vector> ret; for (const auto &[i, j] : collision_pairs_) { - fcl::CollisionResult result; + result.clear(); collision_detection::fcl::collide(collision_objects_[i], collision_objects_[j], request, result); - ret.push_back(result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "self"; + tmp.object_name1 = name_; + tmp.object_name2 = name_; + tmp.link_name1 = collision_link_names_[i]; + tmp.link_name2 = collision_link_names_[j]; + ret.push_back(tmp); + } } return ret; } diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index edcf5fce..b4107bd3 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -28,6 +28,7 @@ ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, urdf_filename, convex, verbose)), verbose_(verbose) { name_ = name.data() ? name : fcl_model_->getName(); + fcl_model_->setName(name_); user_link_names_ = link_names.size() == 0 ? pinocchio_model_->getLinkNames(false) : link_names; user_joint_names_ = @@ -57,6 +58,7 @@ std::unique_ptr> ArticulatedModelTpl::createFromURDFSt collision_detection::FCLModelTpl::createFromURDFString( urdf_string, collision_links, verbose); articulation->name_ = name.data() ? name : fcl_model->getName(); + fcl_model->setName(articulation->name_); articulation->verbose_ = verbose; auto user_link_names = articulation->user_link_names_ = diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 42d0a353..75b019c3 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -293,23 +293,10 @@ std::vector> PlanningWorldTpl::checkSelfCollision( for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); auto col_objs = fcl_model->getCollisionObjects(); - auto col_link_names = fcl_model->getCollisionLinkNames(); - auto col_pairs = fcl_model->getCollisionPairs(); // Articulation self-collision - auto results = fcl_model->checkSelfCollision(request); - for (size_t i = 0; i < results.size(); i++) - if (results[i].isCollision()) { - WorldCollisionResult tmp; - auto x = col_pairs[i].first, y = col_pairs[i].second; - tmp.res = results[i]; - tmp.collision_type = "self"; - tmp.object_name1 = art_name; - tmp.object_name2 = art_name; - tmp.link_name1 = col_link_names[x]; - tmp.link_name2 = col_link_names[y]; - ret.push_back(tmp); - } + const auto results = fcl_model->checkSelfCollision(request); + ret.insert(ret.end(), results.begin(), results.end()); // Collision among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { From 1014f5dd6f0d60f74a405df64d4a816fc79e938c Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 19:57:26 -0700 Subject: [PATCH 12/37] Add FCLModel::checkCollisionWith another FCLModelPtr --- .../mplib/collision_detection/fcl/fcl_model.h | 11 +++++ mplib/pymp/collision_detection/fcl.pyi | 10 +++++ .../fcl/pybind_fcl_model.cpp | 5 ++- .../collision_detection/fcl/fcl_model.h | 8 ++++ src/collision_detection/fcl/fcl_model.cpp | 24 +++++++++++ src/planning_world.cpp | 41 ++++--------------- 6 files changed, 65 insertions(+), 34 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 10fcc6df..37ca824d 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -162,6 +162,17 @@ class FCLModelTpl { std::vector checkSelfCollision( const fcl::CollisionRequest &request = fcl::CollisionRequest()) const; + /** + * Check for collision in the current state with another ``FCLModel``. + * + * @param other: another ``FCLModel`` to check collision with + * @param request: collision request + * @return: List of ``WorldCollisionResult`` objects. If empty, no collision. + */ + std::vector checkCollisionWith( + const FCLModelTplPtr &other, + const fcl::CollisionRequest &request = fcl::CollisionRequest()) const; + private: void init(const urdf::ModelInterfaceSharedPtr &urdf_model, const std::string &package_dir_); diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index 85a0d71e..dedf8496 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -580,6 +580,16 @@ class FCLModel: ``False``. :param verbose: print debug information. Default: ``False``. """ + def check_collision_with( + self, other: FCLModel, request: CollisionRequest = ... + ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: + """ + Check for collision in the current state with another ``FCLModel``. + + :param other: another ``FCLModel`` to check collision with + :param request: collision request + :return: List of ``WorldCollisionResult`` objects. If empty, no collision. + """ def check_self_collision( self, request: CollisionRequest = ... ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index 06bd2681..28408256 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -68,7 +68,10 @@ void build_pyfcl_model(py::module &m) { DOC(mplib, collision_detection, fcl, FCLModelTpl, isStateColliding)) .def("check_self_collision", &FCLModel::checkSelfCollision, py::arg("request") = CollisionRequest(), - DOC(mplib, collision_detection, fcl, FCLModelTpl, checkSelfCollision)); + DOC(mplib, collision_detection, fcl, FCLModelTpl, checkSelfCollision)) + .def("check_collision_with", &FCLModel::checkCollisionWith, py::arg("other"), + py::arg("request") = CollisionRequest(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith)); } } // namespace mplib::collision_detection::fcl diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index 9be98714..e22491c3 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -49,6 +49,14 @@ Construct an FCL model from URDF and SRDF files. ``False``. :param verbose: print debug information. Default: ``False``.)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkCollisionWith = +R"doc( +Check for collision in the current state with another ``FCLModel``. + +:param other: another ``FCLModel`` to check collision with +:param request: collision request +:return: List of ``WorldCollisionResult`` objects. If empty, no collision.)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkSelfCollision = R"doc( Check for self-collision in the current state and returns all found collisions. diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 2b4af4c6..43b46ee6 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -259,4 +259,28 @@ std::vector> FCLModelTpl::checkSelfCollision( return ret; } +template +std::vector> FCLModelTpl::checkCollisionWith( + const FCLModelTplPtr &other, const fcl::CollisionRequest &request) const { + std::vector ret; + fcl::CollisionResult result; + + for (const auto &col_obj : collision_objects_) + for (const auto &col_obj2 : other->collision_objects_) { + result.clear(); + collision_detection::fcl::collide(col_obj, col_obj2, request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "self_articulation"; + tmp.object_name1 = name_; + tmp.object_name2 = other->name_; + tmp.link_name1 = col_obj->name; + tmp.link_name2 = col_obj2->name; + ret.push_back(tmp); + } + } + return ret; +} + } // namespace mplib::collision_detection::fcl diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 75b019c3..bb64d11f 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -300,22 +300,9 @@ std::vector> PlanningWorldTpl::checkSelfCollision( // Collision among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { - if (art_name == art_name2) break; - for (const auto &col_obj : col_objs) - for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) { - result.clear(); - collision_detection::fcl::collide(col_obj, col_obj2, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "self_articulation"; - tmp.object_name1 = art_name; - tmp.object_name2 = art_name2; - tmp.link_name1 = col_obj->name; - tmp.link_name2 = col_obj2->name; - ret.push_back(tmp); - } - } + if (art_name == art_name2) continue; + const auto results = fcl_model->checkCollisionWith(art2->getFCLModel(), request); + ret.insert(ret.end(), results.begin(), results.end()); } // Articulation collide with attached_body_map_ @@ -383,22 +370,10 @@ std::vector> PlanningWorldTpl::checkRobotCollision auto col_objs = fcl_model->getCollisionObjects(); // Collision with unplanned articulation - for (const auto &art2 : unplanned_articulations) - for (const auto &col_obj : col_objs) - for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) { - result.clear(); - collision_detection::fcl::collide(col_obj, col_obj2, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "articulation_articulation"; - tmp.object_name1 = art_name; - tmp.object_name2 = art2->getName(); - tmp.link_name1 = col_obj->name; - tmp.link_name2 = col_obj2->name; - ret.push_back(tmp); - } - } + for (const auto &art2 : unplanned_articulations) { + const auto results = fcl_model->checkCollisionWith(art2->getFCLModel(), request); + ret.insert(ret.end(), results.begin(), results.end()); + } // Collision with scene objects for (const auto &scene_obj : scene_objects) @@ -501,7 +476,7 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( // Minimum distance among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { - if (art_name == art_name2) break; + if (art_name == art_name2) continue; for (const auto &col_obj : col_objs) for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) if (auto type = acm_->getAllowedCollision(col_obj->name, col_obj2->name); From 302e7b5929669acd44bb1316edfd8874628f32e4 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 20:30:31 -0700 Subject: [PATCH 13/37] Add FCLModel::checkCollisionWith an FCLObject --- .../mplib/collision_detection/fcl/fcl_model.h | 19 +++++- mplib/pymp/collision_detection/fcl.pyi | 12 ++++ .../fcl/pybind_fcl_model.cpp | 15 +++-- .../collision_detection/fcl/fcl_model.h | 8 +++ src/collision_detection/fcl/fcl_model.cpp | 31 ++++++++-- src/planning_world.cpp | 62 +++++-------------- 6 files changed, 88 insertions(+), 59 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 37ca824d..dc9a8b3c 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -28,6 +28,8 @@ MPLIB_CLASS_TEMPLATE_FORWARD(FCLModelTpl); template class FCLModelTpl { // Common type alias + using CollisionRequest = fcl::CollisionRequest; + using CollisionResult = fcl::CollisionResult; using WorldCollisionResult = WorldCollisionResultTpl; public: @@ -150,7 +152,7 @@ class FCLModelTpl { * @return: ``true`` if any collision pair collides and ``false`` otherwise. */ bool isStateColliding() const { - return checkSelfCollision(fcl::CollisionRequest()).size() > 0; + return checkSelfCollision(CollisionRequest()).size() > 0; } /** @@ -160,7 +162,7 @@ class FCLModelTpl { * @return: List of ``WorldCollisionResult`` objects. If empty, no self-collision. */ std::vector checkSelfCollision( - const fcl::CollisionRequest &request = fcl::CollisionRequest()) const; + const CollisionRequest &request = CollisionRequest()) const; /** * Check for collision in the current state with another ``FCLModel``. @@ -171,7 +173,18 @@ class FCLModelTpl { */ std::vector checkCollisionWith( const FCLModelTplPtr &other, - const fcl::CollisionRequest &request = fcl::CollisionRequest()) const; + const CollisionRequest &request = CollisionRequest()) const; + + /** + * Check for collision in the current state with an ``FCLObject``. + * + * @param object: an ``FCLObject`` to check collision with + * @param request: collision request + * @return: List of ``WorldCollisionResult`` objects. If empty, no collision. + */ + std::vector checkCollisionWith( + const FCLObjectPtr &object, + const CollisionRequest &request = CollisionRequest()) const; private: void init(const urdf::ModelInterfaceSharedPtr &urdf_model, diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index dedf8496..d7499cb2 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -580,6 +580,7 @@ class FCLModel: ``False``. :param verbose: print debug information. Default: ``False``. """ + @typing.overload def check_collision_with( self, other: FCLModel, request: CollisionRequest = ... ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: @@ -590,6 +591,17 @@ class FCLModel: :param request: collision request :return: List of ``WorldCollisionResult`` objects. If empty, no collision. """ + @typing.overload + def check_collision_with( + self, object: ..., request: CollisionRequest = ... + ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: + """ + Check for collision in the current state with an ``FCLObject``. + + :param object: an ``FCLObject`` to check collision with + :param request: collision request + :return: List of ``WorldCollisionResult`` objects. If empty, no collision. + """ def check_self_collision( self, request: CollisionRequest = ... ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index 28408256..9c18de27 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -1,6 +1,5 @@ #include #include -#include #include #include @@ -17,6 +16,7 @@ namespace py = pybind11; namespace mplib::collision_detection::fcl { using FCLModel = FCLModelTpl; +using FCLModelPtr = FCLModelTplPtr; using CollisionRequest = fcl::CollisionRequest; @@ -69,9 +69,16 @@ void build_pyfcl_model(py::module &m) { .def("check_self_collision", &FCLModel::checkSelfCollision, py::arg("request") = CollisionRequest(), DOC(mplib, collision_detection, fcl, FCLModelTpl, checkSelfCollision)) - .def("check_collision_with", &FCLModel::checkCollisionWith, py::arg("other"), - py::arg("request") = CollisionRequest(), - DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith)); + .def("check_collision_with", + py::overload_cast( + &FCLModel::checkCollisionWith, py::const_), + py::arg("other"), py::arg("request") = CollisionRequest(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith)) + .def("check_collision_with", + py::overload_cast &, const CollisionRequest &>( + &FCLModel::checkCollisionWith, py::const_), + py::arg("object"), py::arg("request") = CollisionRequest(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith, 2)); } } // namespace mplib::collision_detection::fcl diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index e22491c3..669af428 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -57,6 +57,14 @@ Check for collision in the current state with another ``FCLModel``. :param request: collision request :return: List of ``WorldCollisionResult`` objects. If empty, no collision.)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkCollisionWith_2 = +R"doc( +Check for collision in the current state with an ``FCLObject``. + +:param object: an ``FCLObject`` to check collision with +:param request: collision request +:return: List of ``WorldCollisionResult`` objects. If empty, no collision.)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkSelfCollision = R"doc( Check for self-collision in the current state and returns all found collisions. diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 43b46ee6..5d59e84c 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -237,9 +237,9 @@ void FCLModelTpl::updateCollisionObjects( template std::vector> FCLModelTpl::checkSelfCollision( - const fcl::CollisionRequest &request) const { + const CollisionRequest &request) const { std::vector ret; - fcl::CollisionResult result; + CollisionResult result; for (const auto &[i, j] : collision_pairs_) { result.clear(); @@ -261,9 +261,9 @@ std::vector> FCLModelTpl::checkSelfCollision( template std::vector> FCLModelTpl::checkCollisionWith( - const FCLModelTplPtr &other, const fcl::CollisionRequest &request) const { + const FCLModelTplPtr &other, const CollisionRequest &request) const { std::vector ret; - fcl::CollisionResult result; + CollisionResult result; for (const auto &col_obj : collision_objects_) for (const auto &col_obj2 : other->collision_objects_) { @@ -283,4 +283,27 @@ std::vector> FCLModelTpl::checkCollisionWith( return ret; } +template +std::vector> FCLModelTpl::checkCollisionWith( + const FCLObjectPtr &object, const CollisionRequest &request) const { + std::vector ret; + CollisionResult result; + + for (const auto &col_obj : collision_objects_) { + result.clear(); + collision_detection::fcl::collide(col_obj, object, request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "self_object"; + tmp.object_name1 = name_; + tmp.object_name2 = object->name; + tmp.link_name1 = col_obj->name; + tmp.link_name2 = object->name; + ret.push_back(tmp); + } + } + return ret; +} + } // namespace mplib::collision_detection::fcl diff --git a/src/planning_world.cpp b/src/planning_world.cpp index bb64d11f..69f6600d 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -292,8 +292,6 @@ std::vector> PlanningWorldTpl::checkSelfCollision( // Collision involving planned articulation for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); - auto col_objs = fcl_model->getCollisionObjects(); - // Articulation self-collision const auto results = fcl_model->checkSelfCollision(request); ret.insert(ret.end(), results.begin(), results.end()); @@ -306,23 +304,12 @@ std::vector> PlanningWorldTpl::checkSelfCollision( } // Articulation collide with attached_body_map_ - for (const auto &[attached_body_name, attached_body] : attached_body_map_) { - const auto attached_obj = attached_body->getObject(); - for (const auto &col_obj : col_objs) { - result.clear(); - collision_detection::fcl::collide(attached_obj, col_obj, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "self_attach"; - tmp.object_name1 = art_name; - tmp.object_name2 = attached_body_name; - tmp.link_name1 = col_obj->name; - tmp.link_name2 = attached_body_name; - ret.push_back(tmp); - } + for (const auto &[attached_body_name, attached_body] : attached_body_map_) + for (auto &result : + fcl_model->checkCollisionWith(attached_body->getObject(), request)) { + result.collision_type = "self_attached"; + ret.push_back(result); } - } } // Collision among attached_body_map_ @@ -335,7 +322,7 @@ std::vector> PlanningWorldTpl::checkSelfCollision( auto name1 = it->first, name2 = it2->first; WorldCollisionResult tmp; tmp.res = result; - tmp.collision_type = "attach_attach"; + tmp.collision_type = "attached_attached"; tmp.object_name1 = name1; tmp.object_name2 = name2; tmp.link_name1 = name1; @@ -367,8 +354,6 @@ std::vector> PlanningWorldTpl::checkRobotCollision // Collision involving planned articulation for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); - auto col_objs = fcl_model->getCollisionObjects(); - // Collision with unplanned articulation for (const auto &art2 : unplanned_articulations) { const auto results = fcl_model->checkCollisionWith(art2->getFCLModel(), request); @@ -377,19 +362,9 @@ std::vector> PlanningWorldTpl::checkRobotCollision // Collision with scene objects for (const auto &scene_obj : scene_objects) - for (const auto &col_obj : col_objs) { - result.clear(); - collision_detection::fcl::collide(col_obj, scene_obj, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "articulation_sceneobject"; - tmp.object_name1 = art_name; - tmp.object_name2 = scene_obj->name; - tmp.link_name1 = col_obj->name; - tmp.link_name2 = scene_obj->name; - ret.push_back(tmp); - } + for (auto &result : fcl_model->checkCollisionWith(scene_obj, request)) { + result.collision_type = "self_sceneobject"; + ret.push_back(result); } } @@ -398,19 +373,10 @@ std::vector> PlanningWorldTpl::checkRobotCollision const auto attached_obj = attached_body->getObject(); // Collision with unplanned articulation for (const auto &art2 : unplanned_articulations) - for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) { - result.clear(); - collision_detection::fcl::collide(attached_obj, col_obj2, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "attach_articulation"; - tmp.object_name1 = attached_body_name; - tmp.object_name2 = art2->getName(); - tmp.link_name1 = attached_body_name; - tmp.link_name2 = col_obj2->name; - ret.push_back(tmp); - } + for (auto &result : + art2->getFCLModel()->checkCollisionWith(attached_obj, request)) { + result.collision_type = "attached_articulation"; + ret.push_back(result); } // Collision with scene objects @@ -420,7 +386,7 @@ std::vector> PlanningWorldTpl::checkRobotCollision if (result.isCollision()) { WorldCollisionResult tmp; tmp.res = result; - tmp.collision_type = "attach_sceneobject"; + tmp.collision_type = "attached_sceneobject"; tmp.object_name1 = attached_body_name; tmp.object_name2 = scene_obj->name; tmp.link_name1 = attached_body_name; From 92aa7ca96ab5c456d136e24df7dd671a0db59ebb Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Apr 2024 21:36:35 -0700 Subject: [PATCH 14/37] Add FCLModel::distanceSelf method --- .../mplib/collision_detection/fcl/fcl_model.h | 31 +++++++++++++++++++ mplib/pymp/__init__.pyi | 6 ++-- mplib/pymp/collision_detection/fcl.pyi | 25 +++++++++++++++ .../fcl/pybind_fcl_model.cpp | 11 ++++++- .../collision_detection/fcl/fcl_model.h | 19 ++++++++++++ pybind/docstring/planning_world.h | 6 ++-- src/collision_detection/fcl/fcl_model.cpp | 26 ++++++++++++++++ src/planning_world.cpp | 21 ++----------- 8 files changed, 120 insertions(+), 25 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index dc9a8b3c..d3e36c87 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -11,6 +11,7 @@ #include #include "mplib/collision_detection/collision_common.h" +#include "mplib/collision_detection/collision_matrix.h" #include "mplib/collision_detection/fcl/collision_common.h" #include "mplib/macros/class_forward.h" #include "mplib/utils/pose.h" @@ -30,7 +31,10 @@ class FCLModelTpl { // Common type alias using CollisionRequest = fcl::CollisionRequest; using CollisionResult = fcl::CollisionResult; + using DistanceRequest = fcl::DistanceRequest; + using DistanceResult = fcl::DistanceResult; using WorldCollisionResult = WorldCollisionResultTpl; + using WorldDistanceResult = WorldDistanceResultTpl; public: /** @@ -186,6 +190,33 @@ class FCLModelTpl { const FCLObjectPtr &object, const CollisionRequest &request = CollisionRequest()) const; + /** + * The minimum distance to self-collision given the robot in current state, + * ignoring the distances between links that are allowed to always collide (as + * specified by acm). Calls ``distanceSelf()``. + * + * @param acm: allowed collision matrix. + * @return: minimum distance-to-self-collision + */ + S distanceToSelfCollision(const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const { + return distanceSelf(DistanceRequest(), acm).min_distance; + } + + /** + * Get the minimum distance to self-collision given the robot in current state, + * ignoring the distances between links that are allowed to always collide (as + * specified by acm). + * + * @param request: distance request. + * @param acm: allowed collision matrix. + * @return: a ``WorldDistanceResult`` object + */ + WorldDistanceResult distanceSelf( + const DistanceRequest &request = DistanceRequest(), + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const; + private: void init(const urdf::ModelInterfaceSharedPtr &urdf_model, const std::string &package_dir_); diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index ae226c5c..60b12e2a 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -611,21 +611,21 @@ class PlanningWorld: ``planning_scene::PlanningScene::distanceToCollision()`` where self-collisions are ignored. - :return: minimum distance-to-collision + :return: minimum distance-to-all-collision """ def distance_to_robot_collision(self) -> float: """ The distance between the robot model at current state to the nearest collision (ignoring self-collisions). Calls ``distanceRobot()``. - :return: minimum distance-to-collision + :return: minimum distance-to-robot-collision """ def distance_to_self_collision(self) -> float: """ The minimum distance to self-collision given the robot in current state. Calls ``distanceSelf()``. - :return: minimum distance-to-collision + :return: minimum distance-to-self-collision """ def get_allowed_collision_matrix( self, diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index d7499cb2..7c85b1aa 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -611,6 +611,31 @@ class FCLModel: :param request: collision request :return: List of ``WorldCollisionResult`` objects. If empty, no self-collision. """ + def distance_self( + self, + request: DistanceRequest = ..., + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., + ) -> mplib.pymp.collision_detection.WorldDistanceResult: + """ + Get the minimum distance to self-collision given the robot in current state, + ignoring the distances between links that are allowed to always collide (as + specified by acm). + + :param request: distance request. + :param acm: allowed collision matrix. + :return: a ``WorldDistanceResult`` object + """ + def distance_to_self_collision( + self, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... + ) -> float: + """ + The minimum distance to self-collision given the robot in current state, + ignoring the distances between links that are allowed to always collide (as + specified by acm). Calls ``distanceSelf()``. + + :param acm: allowed collision matrix. + :return: minimum distance-to-self-collision + """ def get_collision_link_names(self) -> list[str]: ... def get_collision_objects(self) -> list[...]: """ diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index 9c18de27..fce76b2c 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -19,6 +19,7 @@ using FCLModel = FCLModelTpl; using FCLModelPtr = FCLModelTplPtr; using CollisionRequest = fcl::CollisionRequest; +using DistanceRequest = fcl::DistanceRequest; void build_pyfcl_model(py::module &m) { auto PyFCLModel = py::class_>( @@ -78,7 +79,15 @@ void build_pyfcl_model(py::module &m) { py::overload_cast &, const CollisionRequest &>( &FCLModel::checkCollisionWith, py::const_), py::arg("object"), py::arg("request") = CollisionRequest(), - DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith, 2)); + DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith, 2)) + + .def("distance_to_self_collision", &FCLModel::distanceToSelfCollision, + py::arg("acm") = std::make_shared(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceToSelfCollision)) + .def("distance_self", &FCLModel::distanceSelf, + py::arg("request") = DistanceRequest(), + py::arg("acm") = std::make_shared(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceSelf)); } } // namespace mplib::collision_detection::fcl diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index 669af428..72291a45 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -83,6 +83,25 @@ Constructs a FCLModel from URDF string and collision links :param verbose: print debug information. Default: ``False``. :return: a unique_ptr to FCLModel)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceSelf = +R"doc( +Get the minimum distance to self-collision given the robot in current state, +ignoring the distances between links that are allowed to always collide (as +specified by acm). + +:param request: distance request. +:param acm: allowed collision matrix. +:return: a ``WorldDistanceResult`` object)doc"; + +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceToSelfCollision = +R"doc( +The minimum distance to self-collision given the robot in current state, +ignoring the distances between links that are allowed to always collide (as +specified by acm). Calls ``distanceSelf()``. + +:param acm: allowed collision matrix. +:return: minimum distance-to-self-collision)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_getCollisionLinkNames = R"doc( )doc"; diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 3f3c4ae6..da6286b7 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -258,21 +258,21 @@ Note that this is different from MoveIt2's ``planning_scene::PlanningScene::distanceToCollision()`` where self-collisions are ignored. -:return: minimum distance-to-collision)doc"; +:return: minimum distance-to-all-collision)doc"; static const char *__doc_mplib_PlanningWorldTpl_distanceToRobotCollision = R"doc( The distance between the robot model at current state to the nearest collision (ignoring self-collisions). Calls ``distanceRobot()``. -:return: minimum distance-to-collision)doc"; +:return: minimum distance-to-robot-collision)doc"; static const char *__doc_mplib_PlanningWorldTpl_distanceToSelfCollision = R"doc( The minimum distance to self-collision given the robot in current state. Calls ``distanceSelf()``. -:return: minimum distance-to-collision)doc"; +:return: minimum distance-to-self-collision)doc"; static const char *__doc_mplib_PlanningWorldTpl_getAllowedCollisionMatrix = R"doc( diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 5d59e84c..a25da1b0 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -306,4 +306,30 @@ std::vector> FCLModelTpl::checkCollisionWith( return ret; } +template +WorldDistanceResultTpl FCLModelTpl::distanceSelf( + const DistanceRequest &request, const AllowedCollisionMatrixPtr &acm) const { + WorldDistanceResult ret; + DistanceResult result; + + for (const auto &[i, j] : collision_pairs_) + if (auto type = acm->getAllowedCollision(collision_link_names_[i], + collision_link_names_[j]); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::distance(collision_objects_[i], collision_objects_[j], + request, result); + if (result.min_distance < ret.min_distance) { + ret.res = result; + ret.min_distance = result.min_distance; + ret.distance_type = "self"; + ret.object_name1 = name_; + ret.object_name2 = name_; + ret.link_name1 = collision_link_names_[i]; + ret.link_name2 = collision_link_names_[j]; + } + } + return ret; +} + } // namespace mplib::collision_detection::fcl diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 69f6600d..5d50924e 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -419,26 +419,11 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); auto col_objs = fcl_model->getCollisionObjects(); - auto col_link_names = fcl_model->getCollisionLinkNames(); - auto col_pairs = fcl_model->getCollisionPairs(); - // TODO(merge): add FCLModel::distanceSelf() method // Articulation minimum distance to self-collision - for (const auto &[x, y] : col_pairs) - if (auto type = acm_->getAllowedCollision(col_link_names[x], col_link_names[y]); - !type || type == collision_detection::AllowedCollision::NEVER) { - result.clear(); - collision_detection::fcl::distance(col_objs[x], col_objs[y], request, result); - if (result.min_distance < ret.min_distance) { - ret.res = result; - ret.min_distance = result.min_distance; - ret.distance_type = "self"; - ret.object_name1 = art_name; - ret.object_name2 = art_name; - ret.link_name1 = col_link_names[x]; - ret.link_name2 = col_link_names[y]; - } - } + if (const auto &tmp = fcl_model->distanceSelf(request, acm_); + tmp.min_distance < ret.min_distance) + ret = tmp; // Minimum distance among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { From 338ba91b1d1b725c998b9c0456c501d927e68fd3 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 11:21:56 -0700 Subject: [PATCH 15/37] Add FCLModel::distanceWith another FCLModelPtr --- .../mplib/collision_detection/fcl/fcl_model.h | 31 +++++++++++++++ mplib/pymp/collision_detection/fcl.pyi | 30 +++++++++++++++ .../fcl/pybind_fcl_model.cpp | 10 ++++- .../collision_detection/fcl/fcl_model.h | 21 ++++++++++ src/collision_detection/fcl/fcl_model.cpp | 26 +++++++++++++ src/planning_world.cpp | 38 +++---------------- 6 files changed, 123 insertions(+), 33 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index d3e36c87..2da6baf3 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -217,6 +217,37 @@ class FCLModelTpl { const AllowedCollisionMatrixPtr &acm = std::make_shared()) const; + /** + * The minimum distance to collision with another ``FCLModel`` given the robot in + * current state, ignoring the distances between links that are allowed to always + * collide (as specified by acm). + * + * @param other: another ``FCLModel`` to get minimum distance-to-collision with + * @param acm: allowed collision matrix. + * @return: minimum distance-to-collision with the other ``FCLModel`` + */ + S distanceToCollisionWith(const FCLModelTplPtr &other, + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const { + return distanceWith(other, DistanceRequest(), acm).min_distance; + } + + /** + * Get the minimum distance to collision with another ``FCLModel`` given the robot in + * current state, ignoring the distances between links that are allowed to always + * collide (as specified by acm). + * + * @param other: another ``FCLModel`` to get minimum distance-to-collision with + * @param request: distance request. + * @param acm: allowed collision matrix. + * @return: a ``WorldDistanceResult`` object + */ + WorldDistanceResult distanceWith( + const FCLModelTplPtr &other, + const DistanceRequest &request = DistanceRequest(), + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const; + private: void init(const urdf::ModelInterfaceSharedPtr &urdf_model, const std::string &package_dir_); diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index 7c85b1aa..99942315 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -625,6 +625,20 @@ class FCLModel: :param acm: allowed collision matrix. :return: a ``WorldDistanceResult`` object """ + def distance_to_collision_with( + self, + other: FCLModel, + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., + ) -> float: + """ + The minimum distance to collision with another ``FCLModel`` given the robot in + current state, ignoring the distances between links that are allowed to always + collide (as specified by acm). + + :param other: another ``FCLModel`` to get minimum distance-to-collision with + :param acm: allowed collision matrix. + :return: minimum distance-to-collision with the other ``FCLModel`` + """ def distance_to_self_collision( self, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... ) -> float: @@ -636,6 +650,22 @@ class FCLModel: :param acm: allowed collision matrix. :return: minimum distance-to-self-collision """ + def distance_with( + self, + other: FCLModel, + request: DistanceRequest = ..., + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., + ) -> mplib.pymp.collision_detection.WorldDistanceResult: + """ + Get the minimum distance to collision with another ``FCLModel`` given the robot + in current state, ignoring the distances between links that are allowed to + always collide (as specified by acm). + + :param other: another ``FCLModel`` to get minimum distance-to-collision with + :param request: distance request. + :param acm: allowed collision matrix. + :return: a ``WorldDistanceResult`` object + """ def get_collision_link_names(self) -> list[str]: ... def get_collision_objects(self) -> list[...]: """ diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index fce76b2c..a9d49269 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -87,7 +87,15 @@ void build_pyfcl_model(py::module &m) { .def("distance_self", &FCLModel::distanceSelf, py::arg("request") = DistanceRequest(), py::arg("acm") = std::make_shared(), - DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceSelf)); + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceSelf)) + .def("distance_to_collision_with", &FCLModel::distanceToCollisionWith, + py::arg("other"), + py::arg("acm") = std::make_shared(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceToCollisionWith)) + .def("distance_with", &FCLModel::distanceWith, py::arg("other"), + py::arg("request") = DistanceRequest(), + py::arg("acm") = std::make_shared(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceWith)); } } // namespace mplib::collision_detection::fcl diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index 72291a45..c2f6d234 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -93,6 +93,16 @@ specified by acm). :param acm: allowed collision matrix. :return: a ``WorldDistanceResult`` object)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceToCollisionWith = +R"doc( +The minimum distance to collision with another ``FCLModel`` given the robot in +current state, ignoring the distances between links that are allowed to always +collide (as specified by acm). + +:param other: another ``FCLModel`` to get minimum distance-to-collision with +:param acm: allowed collision matrix. +:return: minimum distance-to-collision with the other ``FCLModel``)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceToSelfCollision = R"doc( The minimum distance to self-collision given the robot in current state, @@ -102,6 +112,17 @@ specified by acm). Calls ``distanceSelf()``. :param acm: allowed collision matrix. :return: minimum distance-to-self-collision)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceWith = +R"doc( +Get the minimum distance to collision with another ``FCLModel`` given the robot +in current state, ignoring the distances between links that are allowed to +always collide (as specified by acm). + +:param other: another ``FCLModel`` to get minimum distance-to-collision with +:param request: distance request. +:param acm: allowed collision matrix. +:return: a ``WorldDistanceResult`` object)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_getCollisionLinkNames = R"doc( )doc"; diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index a25da1b0..dbd11124 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -332,4 +332,30 @@ WorldDistanceResultTpl FCLModelTpl::distanceSelf( return ret; } +template +WorldDistanceResultTpl FCLModelTpl::distanceWith( + const FCLModelTplPtr &other, const DistanceRequest &request, + const AllowedCollisionMatrixPtr &acm) const { + WorldDistanceResult ret; + DistanceResult result; + + for (const auto &col_obj : collision_objects_) + for (const auto &col_obj2 : other->collision_objects_) + if (auto type = acm->getAllowedCollision(col_obj->name, col_obj2->name); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::distance(col_obj, col_obj2, request, result); + if (result.min_distance < ret.min_distance) { + ret.res = result; + ret.min_distance = result.min_distance; + ret.distance_type = "self_articulation"; + ret.object_name1 = name_; + ret.object_name2 = other->name_; + ret.link_name1 = col_obj->name; + ret.link_name2 = col_obj2->name; + } + } + return ret; +} + } // namespace mplib::collision_detection::fcl diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 5d50924e..32e65e76 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -428,22 +428,9 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( // Minimum distance among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { if (art_name == art_name2) continue; - for (const auto &col_obj : col_objs) - for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) - if (auto type = acm_->getAllowedCollision(col_obj->name, col_obj2->name); - !type || type == collision_detection::AllowedCollision::NEVER) { - result.clear(); - collision_detection::fcl::distance(col_obj, col_obj2, request, result); - if (result.min_distance < ret.min_distance) { - ret.res = result; - ret.min_distance = result.min_distance; - ret.distance_type = "self_articulation"; - ret.object_name1 = art_name; - ret.object_name2 = art_name2; - ret.link_name1 = col_obj->name; - ret.link_name2 = col_obj2->name; - } - } + if (const auto &tmp = fcl_model->distanceWith(art2->getFCLModel(), request, acm_); + tmp.min_distance < ret.min_distance) + ret = tmp; } // Articulation minimum distance to attached_body_map_ @@ -515,22 +502,9 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceRobot( // Minimum distance to unplanned articulation for (const auto &art2 : unplanned_articulations) - for (const auto &col_obj : col_objs) - for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) - if (auto type = acm_->getAllowedCollision(col_obj->name, col_obj2->name); - !type || type == collision_detection::AllowedCollision::NEVER) { - result.clear(); - collision_detection::fcl::distance(col_obj, col_obj2, request, result); - if (result.min_distance < ret.min_distance) { - ret.res = result; - ret.min_distance = result.min_distance; - ret.distance_type = "articulation_articulation"; - ret.object_name1 = art_name; - ret.object_name2 = art2->getName(); - ret.link_name1 = col_obj->name; - ret.link_name2 = col_obj2->name; - } - } + if (const auto &tmp = fcl_model->distanceWith(art2->getFCLModel(), request, acm_); + tmp.min_distance < ret.min_distance) + ret = tmp; // Minimum distance to scene objects for (const auto &scene_obj : scene_objects) From d41ed3e256ab8f84b21e8897362bd2224a4a1408 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 11:38:21 -0700 Subject: [PATCH 16/37] Add FCLModel::distanceWith an FCLObject --- .../mplib/collision_detection/fcl/fcl_model.h | 30 ++++++++ mplib/pymp/collision_detection/fcl.pyi | 34 +++++++++ .../fcl/pybind_fcl_model.cpp | 27 ++++++- .../collision_detection/fcl/fcl_model.h | 21 ++++++ src/collision_detection/fcl/fcl_model.cpp | 25 +++++++ src/planning_world.cpp | 74 +++++-------------- 6 files changed, 153 insertions(+), 58 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index 2da6baf3..b3abe71a 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -248,6 +248,36 @@ class FCLModelTpl { const AllowedCollisionMatrixPtr &acm = std::make_shared()) const; + /** + * The minimum distance to collision with an ``FCLObject`` given the robot in + * current state, ignoring the distances between objects that are allowed to always + * collide (as specified by acm). + * + * @param object: an ``FCLObject`` to get minimum distance-to-collision with + * @param acm: allowed collision matrix. + * @return: minimum distance-to-collision with the ``FCLObject`` + */ + S distanceToCollisionWith(const FCLObjectPtr &object, + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const { + return distanceWith(object, DistanceRequest(), acm).min_distance; + } + + /** + * Get the minimum distance to collision with an ``FCLObject`` given the robot in + * current state, ignoring the distances between objects that are allowed to always + * collide (as specified by acm). + * + * @param object: an ``FCLObject`` to get minimum distance-to-collision with + * @param request: distance request. + * @param acm: allowed collision matrix. + * @return: a ``WorldDistanceResult`` object + */ + WorldDistanceResult distanceWith( + const FCLObjectPtr &object, const DistanceRequest &request = DistanceRequest(), + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const; + private: void init(const urdf::ModelInterfaceSharedPtr &urdf_model, const std::string &package_dir_); diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index 99942315..72b08e49 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -625,6 +625,7 @@ class FCLModel: :param acm: allowed collision matrix. :return: a ``WorldDistanceResult`` object """ + @typing.overload def distance_to_collision_with( self, other: FCLModel, @@ -639,6 +640,21 @@ class FCLModel: :param acm: allowed collision matrix. :return: minimum distance-to-collision with the other ``FCLModel`` """ + @typing.overload + def distance_to_collision_with( + self, + object: ..., + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., + ) -> float: + """ + The minimum distance to collision with an ``FCLObject`` given the robot in + current state, ignoring the distances between objects that are allowed to always + collide (as specified by acm). + + :param object: an ``FCLObject`` to get minimum distance-to-collision with + :param acm: allowed collision matrix. + :return: minimum distance-to-collision with the ``FCLObject`` + """ def distance_to_self_collision( self, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... ) -> float: @@ -650,6 +666,7 @@ class FCLModel: :param acm: allowed collision matrix. :return: minimum distance-to-self-collision """ + @typing.overload def distance_with( self, other: FCLModel, @@ -666,6 +683,23 @@ class FCLModel: :param acm: allowed collision matrix. :return: a ``WorldDistanceResult`` object """ + @typing.overload + def distance_with( + self, + object: ..., + request: DistanceRequest = ..., + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., + ) -> mplib.pymp.collision_detection.WorldDistanceResult: + """ + Get the minimum distance to collision with an ``FCLObject`` given the robot in + current state, ignoring the distances between objects that are allowed to always + collide (as specified by acm). + + :param object: an ``FCLObject`` to get minimum distance-to-collision with + :param request: distance request. + :param acm: allowed collision matrix. + :return: a ``WorldDistanceResult`` object + """ def get_collision_link_names(self) -> list[str]: ... def get_collision_objects(self) -> list[...]: """ diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index a9d49269..e1cfc617 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -88,14 +88,33 @@ void build_pyfcl_model(py::module &m) { py::arg("request") = DistanceRequest(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceSelf)) - .def("distance_to_collision_with", &FCLModel::distanceToCollisionWith, + .def("distance_to_collision_with", + py::overload_cast( + &FCLModel::distanceToCollisionWith, py::const_), py::arg("other"), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceToCollisionWith)) - .def("distance_with", &FCLModel::distanceWith, py::arg("other"), - py::arg("request") = DistanceRequest(), + .def("distance_with", + py::overload_cast(&FCLModel::distanceWith, + py::const_), + py::arg("other"), py::arg("request") = DistanceRequest(), + py::arg("acm") = std::make_shared(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceWith)) + .def( + "distance_to_collision_with", + py::overload_cast &, const AllowedCollisionMatrixPtr &>( + &FCLModel::distanceToCollisionWith, py::const_), + py::arg("object"), + py::arg("acm") = std::make_shared(), + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceToCollisionWith, 2)) + .def("distance_with", + py::overload_cast &, const DistanceRequest &, + const AllowedCollisionMatrixPtr &>(&FCLModel::distanceWith, + py::const_), + py::arg("object"), py::arg("request") = DistanceRequest(), py::arg("acm") = std::make_shared(), - DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceWith)); + DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceWith, 2)); } } // namespace mplib::collision_detection::fcl diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index c2f6d234..e14f37f3 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -103,6 +103,16 @@ collide (as specified by acm). :param acm: allowed collision matrix. :return: minimum distance-to-collision with the other ``FCLModel``)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceToCollisionWith_2 = +R"doc( +The minimum distance to collision with an ``FCLObject`` given the robot in +current state, ignoring the distances between objects that are allowed to always +collide (as specified by acm). + +:param object: an ``FCLObject`` to get minimum distance-to-collision with +:param acm: allowed collision matrix. +:return: minimum distance-to-collision with the ``FCLObject``)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceToSelfCollision = R"doc( The minimum distance to self-collision given the robot in current state, @@ -123,6 +133,17 @@ always collide (as specified by acm). :param acm: allowed collision matrix. :return: a ``WorldDistanceResult`` object)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_distanceWith_2 = +R"doc( +Get the minimum distance to collision with an ``FCLObject`` given the robot in +current state, ignoring the distances between objects that are allowed to always +collide (as specified by acm). + +:param object: an ``FCLObject`` to get minimum distance-to-collision with +:param request: distance request. +:param acm: allowed collision matrix. +:return: a ``WorldDistanceResult`` object)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_getCollisionLinkNames = R"doc( )doc"; diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index dbd11124..c7d0c318 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -358,4 +358,29 @@ WorldDistanceResultTpl FCLModelTpl::distanceWith( return ret; } +template +WorldDistanceResultTpl FCLModelTpl::distanceWith( + const FCLObjectPtr &object, const DistanceRequest &request, + const AllowedCollisionMatrixPtr &acm) const { + WorldDistanceResult ret; + DistanceResult result; + + for (const auto &col_obj : collision_objects_) + if (auto type = acm->getAllowedCollision(col_obj->name, object->name); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::distance(col_obj, object, request, result); + if (result.min_distance < ret.min_distance) { + ret.res = result; + ret.min_distance = result.min_distance; + ret.distance_type = "self_object"; + ret.object_name1 = name_; + ret.object_name2 = object->name; + ret.link_name1 = col_obj->name; + ret.link_name2 = object->name; + } + } + return ret; +} + } // namespace mplib::collision_detection::fcl diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 32e65e76..1c7fffc3 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -418,8 +418,6 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( // Minimum distance involving planned articulation for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); - auto col_objs = fcl_model->getCollisionObjects(); - // Articulation minimum distance to self-collision if (const auto &tmp = fcl_model->distanceSelf(request, acm_); tmp.min_distance < ret.min_distance) @@ -434,24 +432,13 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( } // Articulation minimum distance to attached_body_map_ - for (const auto &[attached_body_name, attached_body] : attached_body_map_) { - auto attached_obj = attached_body->getObject(); - for (const auto &col_obj : col_objs) - if (auto type = acm_->getAllowedCollision(col_obj->name, attached_body_name); - !type || type == collision_detection::AllowedCollision::NEVER) { - result.clear(); - collision_detection::fcl::distance(attached_obj, col_obj, request, result); - if (result.min_distance < ret.min_distance) { - ret.res = result; - ret.min_distance = result.min_distance; - ret.distance_type = "self_attach"; - ret.object_name1 = art_name; - ret.object_name2 = attached_body_name; - ret.link_name1 = col_obj->name; - ret.link_name2 = attached_body_name; - } - } - } + for (const auto &[attached_body_name, attached_body] : attached_body_map_) + if (const auto &tmp = + fcl_model->distanceWith(attached_body->getObject(), request, acm_); + tmp.min_distance < ret.min_distance) { + ret = tmp; + ret.distance_type = "self_attached"; + } } // Minimum distance among attached_body_map_ @@ -466,7 +453,7 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; - ret.distance_type = "attach_attach"; + ret.distance_type = "attached_attached"; ret.object_name1 = name1; ret.object_name2 = name2; ret.link_name1 = name1; @@ -498,8 +485,6 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceRobot( // Minimum distance involving planned articulation for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); - auto col_objs = fcl_model->getCollisionObjects(); - // Minimum distance to unplanned articulation for (const auto &art2 : unplanned_articulations) if (const auto &tmp = fcl_model->distanceWith(art2->getFCLModel(), request, acm_); @@ -508,21 +493,11 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceRobot( // Minimum distance to scene objects for (const auto &scene_obj : scene_objects) - for (const auto &col_obj : col_objs) - if (auto type = acm_->getAllowedCollision(col_obj->name, scene_obj->name); - !type || type == collision_detection::AllowedCollision::NEVER) { - result.clear(); - collision_detection::fcl::distance(col_obj, scene_obj, request, result); - if (result.min_distance < ret.min_distance) { - ret.res = result; - ret.min_distance = result.min_distance; - ret.distance_type = "articulation_sceneobject"; - ret.object_name1 = art_name; - ret.object_name2 = scene_obj->name; - ret.link_name1 = col_obj->name; - ret.link_name2 = scene_obj->name; - } - } + if (const auto &tmp = fcl_model->distanceWith(scene_obj, request, acm_); + tmp.min_distance < ret.min_distance) { + ret = tmp; + ret.distance_type = "self_sceneobject"; + } } // Minimum distance involving attached_body_map_ @@ -530,21 +505,12 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceRobot( const auto attached_obj = attached_body->getObject(); // Minimum distance to unplanned articulation for (const auto &art2 : unplanned_articulations) - for (const auto &col_obj2 : art2->getFCLModel()->getCollisionObjects()) - if (auto type = acm_->getAllowedCollision(attached_body_name, col_obj2->name); - !type || type == collision_detection::AllowedCollision::NEVER) { - result.clear(); - collision_detection::fcl::distance(attached_obj, col_obj2, request, result); - if (result.min_distance < ret.min_distance) { - ret.res = result; - ret.min_distance = result.min_distance; - ret.distance_type = "attach_articulation"; - ret.object_name1 = attached_body_name; - ret.object_name2 = art2->getName(); - ret.link_name1 = attached_body_name; - ret.link_name2 = col_obj2->name; - } - } + if (const auto &tmp = + art2->getFCLModel()->distanceWith(attached_obj, request, acm_); + tmp.min_distance < ret.min_distance) { + ret = tmp; + ret.distance_type = "attached_articulation"; + } // Minimum distance to scene objects for (const auto &scene_obj : scene_objects) @@ -555,7 +521,7 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceRobot( if (result.min_distance < ret.min_distance) { ret.res = result; ret.min_distance = result.min_distance; - ret.distance_type = "attach_sceneobject"; + ret.distance_type = "attached_sceneobject"; ret.object_name1 = attached_body_name; ret.object_name2 = scene_obj->name; ret.link_name1 = attached_body_name; From 303943ad68afe2c92f0de1aea10db88cb75a3b41 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 11:52:30 -0700 Subject: [PATCH 17/37] Rename collision/distance_type of planned_art with planned_art as self_self --- src/planning_world.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 1c7fffc3..7d916124 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -299,8 +299,10 @@ std::vector> PlanningWorldTpl::checkSelfCollision( // Collision among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { if (art_name == art_name2) continue; - const auto results = fcl_model->checkCollisionWith(art2->getFCLModel(), request); - ret.insert(ret.end(), results.begin(), results.end()); + for (auto &result : fcl_model->checkCollisionWith(art2->getFCLModel(), request)) { + result.collision_type = "self_self"; + ret.push_back(result); + } } // Articulation collide with attached_body_map_ @@ -427,8 +429,10 @@ WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( for (const auto &[art_name2, art2] : planned_articulation_map_) { if (art_name == art_name2) continue; if (const auto &tmp = fcl_model->distanceWith(art2->getFCLModel(), request, acm_); - tmp.min_distance < ret.min_distance) + tmp.min_distance < ret.min_distance) { ret = tmp; + ret.distance_type = "self_self"; + } } // Articulation minimum distance to attached_body_map_ From 5324aafe5af30cad0314cb623f2b2ec95862c4c8 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 12:48:47 -0700 Subject: [PATCH 18/37] Add acm support for FCLModel collision methods --- .../mplib/collision_detection/fcl/fcl_model.h | 37 ++++++-- include/mplib/planning_world.h | 4 - mplib/pymp/collision_detection/fcl.pyi | 37 ++++++-- .../fcl/pybind_fcl_model.cpp | 10 +- .../collision_detection/fcl/fcl_model.h | 19 +++- src/collision_detection/fcl/fcl_model.cpp | 95 ++++++++++--------- src/planning_world.cpp | 87 ++++++++--------- 7 files changed, 173 insertions(+), 116 deletions(-) diff --git a/include/mplib/collision_detection/fcl/fcl_model.h b/include/mplib/collision_detection/fcl/fcl_model.h index b3abe71a..0b568401 100644 --- a/include/mplib/collision_detection/fcl/fcl_model.h +++ b/include/mplib/collision_detection/fcl/fcl_model.h @@ -151,44 +151,63 @@ class FCLModelTpl { void updateCollisionObjects(const std::vector> &link_poses) const; /** - * Check if the current state is in self-collision + * Check if the current state is in self-collision, + * ignoring the distances between links that are allowed to always collide (as + * specified by acm). * + * @param acm: allowed collision matrix. * @return: ``true`` if any collision pair collides and ``false`` otherwise. */ - bool isStateColliding() const { - return checkSelfCollision(CollisionRequest()).size() > 0; + bool isStateColliding(const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const { + return checkSelfCollision(CollisionRequest(), acm).size() > 0; } /** - * Check for self-collision in the current state and returns all found collisions. + * Check for self-collision in the current state and returns all found collisions, + * ignoring the distances between links that are allowed to always collide (as + * specified by acm). * * @param request: collision request + * @param acm: allowed collision matrix. * @return: List of ``WorldCollisionResult`` objects. If empty, no self-collision. */ std::vector checkSelfCollision( - const CollisionRequest &request = CollisionRequest()) const; + const CollisionRequest &request = CollisionRequest(), + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const; /** - * Check for collision in the current state with another ``FCLModel``. + * Check for collision in the current state with another ``FCLModel``, + * ignoring the distances between links that are allowed to always + * collide (as specified by acm). * * @param other: another ``FCLModel`` to check collision with + * @param acm: allowed collision matrix. * @param request: collision request * @return: List of ``WorldCollisionResult`` objects. If empty, no collision. */ std::vector checkCollisionWith( const FCLModelTplPtr &other, - const CollisionRequest &request = CollisionRequest()) const; + const CollisionRequest &request = CollisionRequest(), + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const; /** - * Check for collision in the current state with an ``FCLObject``. + * Check for collision in the current state with an ``FCLObject``, + * ignoring the distances between objects that are allowed to always + * collide (as specified by acm). * * @param object: an ``FCLObject`` to check collision with + * @param acm: allowed collision matrix. * @param request: collision request * @return: List of ``WorldCollisionResult`` objects. If empty, no collision. */ std::vector checkCollisionWith( const FCLObjectPtr &object, - const CollisionRequest &request = CollisionRequest()) const; + const CollisionRequest &request = CollisionRequest(), + const AllowedCollisionMatrixPtr &acm = + std::make_shared()) const; /** * The minimum distance to self-collision given the robot in current state, diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 5cd4cc65..8422ae13 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -486,10 +486,6 @@ class PlanningWorldTpl { for (const auto &[name, attached_body] : attached_body_map_) attached_body->updatePose(); } - - /// @brief Filter collisions using acm_ - std::vector filterCollisions( - const std::vector &collisions) const; }; // Common Type Alias =================================================================== diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index 72b08e49..eec866b4 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -582,33 +582,50 @@ class FCLModel: """ @typing.overload def check_collision_with( - self, other: FCLModel, request: CollisionRequest = ... + self, + other: FCLModel, + request: CollisionRequest = ..., + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: """ - Check for collision in the current state with another ``FCLModel``. + Check for collision in the current state with another ``FCLModel``, ignoring the + distances between links that are allowed to always collide (as specified by + acm). :param other: another ``FCLModel`` to check collision with + :param acm: allowed collision matrix. :param request: collision request :return: List of ``WorldCollisionResult`` objects. If empty, no collision. """ @typing.overload def check_collision_with( - self, object: ..., request: CollisionRequest = ... + self, + object: ..., + request: CollisionRequest = ..., + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: """ - Check for collision in the current state with an ``FCLObject``. + Check for collision in the current state with an ``FCLObject``, ignoring the + distances between objects that are allowed to always collide (as specified by + acm). :param object: an ``FCLObject`` to check collision with + :param acm: allowed collision matrix. :param request: collision request :return: List of ``WorldCollisionResult`` objects. If empty, no collision. """ def check_self_collision( - self, request: CollisionRequest = ... + self, + request: CollisionRequest = ..., + acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: """ - Check for self-collision in the current state and returns all found collisions. + Check for self-collision in the current state and returns all found collisions, + ignoring the distances between links that are allowed to always collide (as + specified by acm). :param request: collision request + :param acm: allowed collision matrix. :return: List of ``WorldCollisionResult`` objects. If empty, no self-collision. """ def distance_self( @@ -721,10 +738,14 @@ class FCLModel: :return: name of the articulated model """ - def is_state_colliding(self) -> bool: + def is_state_colliding( + self, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... + ) -> bool: """ - Check if the current state is in self-collision + Check if the current state is in self-collision, ignoring the distances between + links that are allowed to always collide (as specified by acm). + :param acm: allowed collision matrix. :return: ``True`` if any collision pair collides and ``False`` otherwise. """ def remove_collision_pairs_from_srdf(self, srdf_filename: str) -> None: diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index e1cfc617..38f145c7 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -66,19 +66,25 @@ void build_pyfcl_model(py::module &m) { DOC(mplib, collision_detection, fcl, FCLModelTpl, updateCollisionObjects)) .def("is_state_colliding", &FCLModel::isStateColliding, + py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, isStateColliding)) .def("check_self_collision", &FCLModel::checkSelfCollision, py::arg("request") = CollisionRequest(), + py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, checkSelfCollision)) .def("check_collision_with", - py::overload_cast( + py::overload_cast( &FCLModel::checkCollisionWith, py::const_), py::arg("other"), py::arg("request") = CollisionRequest(), + py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith)) .def("check_collision_with", - py::overload_cast &, const CollisionRequest &>( + py::overload_cast &, const CollisionRequest &, + const AllowedCollisionMatrixPtr &>( &FCLModel::checkCollisionWith, py::const_), py::arg("object"), py::arg("request") = CollisionRequest(), + py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith, 2)) .def("distance_to_self_collision", &FCLModel::distanceToSelfCollision, diff --git a/pybind/docstring/collision_detection/fcl/fcl_model.h b/pybind/docstring/collision_detection/fcl/fcl_model.h index e14f37f3..b24e0b74 100644 --- a/pybind/docstring/collision_detection/fcl/fcl_model.h +++ b/pybind/docstring/collision_detection/fcl/fcl_model.h @@ -51,25 +51,34 @@ Construct an FCL model from URDF and SRDF files. static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkCollisionWith = R"doc( -Check for collision in the current state with another ``FCLModel``. +Check for collision in the current state with another ``FCLModel``, ignoring the +distances between links that are allowed to always collide (as specified by +acm). :param other: another ``FCLModel`` to check collision with +:param acm: allowed collision matrix. :param request: collision request :return: List of ``WorldCollisionResult`` objects. If empty, no collision.)doc"; static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkCollisionWith_2 = R"doc( -Check for collision in the current state with an ``FCLObject``. +Check for collision in the current state with an ``FCLObject``, ignoring the +distances between objects that are allowed to always collide (as specified by +acm). :param object: an ``FCLObject`` to check collision with +:param acm: allowed collision matrix. :param request: collision request :return: List of ``WorldCollisionResult`` objects. If empty, no collision.)doc"; static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_checkSelfCollision = R"doc( -Check for self-collision in the current state and returns all found collisions. +Check for self-collision in the current state and returns all found collisions, +ignoring the distances between links that are allowed to always collide (as +specified by acm). :param request: collision request +:param acm: allowed collision matrix. :return: List of ``WorldCollisionResult`` objects. If empty, no self-collision.)doc"; static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_createFromURDFString = @@ -178,8 +187,10 @@ R"doc( static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_isStateColliding = R"doc( -Check if the current state is in self-collision +Check if the current state is in self-collision, ignoring the distances between +links that are allowed to always collide (as specified by acm). +:param acm: allowed collision matrix. :return: ``True`` if any collision pair collides and ``False`` otherwise.)doc"; static const char *__doc_mplib_collision_detection_fcl_FCLModelTpl_printCollisionPairs = diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index c7d0c318..3af34c38 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -237,72 +237,81 @@ void FCLModelTpl::updateCollisionObjects( template std::vector> FCLModelTpl::checkSelfCollision( - const CollisionRequest &request) const { + const CollisionRequest &request, const AllowedCollisionMatrixPtr &acm) const { std::vector ret; CollisionResult result; - for (const auto &[i, j] : collision_pairs_) { - result.clear(); - collision_detection::fcl::collide(collision_objects_[i], collision_objects_[j], - request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "self"; - tmp.object_name1 = name_; - tmp.object_name2 = name_; - tmp.link_name1 = collision_link_names_[i]; - tmp.link_name2 = collision_link_names_[j]; - ret.push_back(tmp); + for (const auto &[i, j] : collision_pairs_) + if (auto type = acm->getAllowedCollision(collision_link_names_[i], + collision_link_names_[j]); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::collide(collision_objects_[i], collision_objects_[j], + request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "self"; + tmp.object_name1 = name_; + tmp.object_name2 = name_; + tmp.link_name1 = collision_link_names_[i]; + tmp.link_name2 = collision_link_names_[j]; + ret.push_back(tmp); + } } - } return ret; } template std::vector> FCLModelTpl::checkCollisionWith( - const FCLModelTplPtr &other, const CollisionRequest &request) const { + const FCLModelTplPtr &other, const CollisionRequest &request, + const AllowedCollisionMatrixPtr &acm) const { std::vector ret; CollisionResult result; for (const auto &col_obj : collision_objects_) - for (const auto &col_obj2 : other->collision_objects_) { - result.clear(); - collision_detection::fcl::collide(col_obj, col_obj2, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "self_articulation"; - tmp.object_name1 = name_; - tmp.object_name2 = other->name_; - tmp.link_name1 = col_obj->name; - tmp.link_name2 = col_obj2->name; - ret.push_back(tmp); + for (const auto &col_obj2 : other->collision_objects_) + if (auto type = acm->getAllowedCollision(col_obj->name, col_obj2->name); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::collide(col_obj, col_obj2, request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "self_articulation"; + tmp.object_name1 = name_; + tmp.object_name2 = other->name_; + tmp.link_name1 = col_obj->name; + tmp.link_name2 = col_obj2->name; + ret.push_back(tmp); + } } - } return ret; } template std::vector> FCLModelTpl::checkCollisionWith( - const FCLObjectPtr &object, const CollisionRequest &request) const { + const FCLObjectPtr &object, const CollisionRequest &request, + const AllowedCollisionMatrixPtr &acm) const { std::vector ret; CollisionResult result; - for (const auto &col_obj : collision_objects_) { - result.clear(); - collision_detection::fcl::collide(col_obj, object, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "self_object"; - tmp.object_name1 = name_; - tmp.object_name2 = object->name; - tmp.link_name1 = col_obj->name; - tmp.link_name2 = object->name; - ret.push_back(tmp); + for (const auto &col_obj : collision_objects_) + if (auto type = acm->getAllowedCollision(col_obj->name, object->name); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::collide(col_obj, object, request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "self_object"; + tmp.object_name1 = name_; + tmp.object_name2 = object->name; + tmp.link_name1 = col_obj->name; + tmp.link_name2 = object->name; + ret.push_back(tmp); + } } - } return ret; } diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 7d916124..5fa8997d 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -269,18 +269,6 @@ void PlanningWorldTpl::setQposAll(const VectorX &state) const { ASSERT(i == static_cast(state.size()), "State dimension is not correct"); } -template -std::vector> PlanningWorldTpl::filterCollisions( - const std::vector> &collisions) const { - std::vector ret; - for (const auto &collision : collisions) - if (auto type = - acm_->getAllowedCollision(collision.link_name1, collision.link_name2); - !type || type == collision_detection::AllowedCollision::NEVER) - ret.push_back(collision); - return ret; -} - template std::vector> PlanningWorldTpl::checkSelfCollision( const CollisionRequest &request) const { @@ -293,13 +281,14 @@ std::vector> PlanningWorldTpl::checkSelfCollision( for (const auto &[art_name, art] : planned_articulation_map_) { auto fcl_model = art->getFCLModel(); // Articulation self-collision - const auto results = fcl_model->checkSelfCollision(request); + const auto results = fcl_model->checkSelfCollision(request, acm_); ret.insert(ret.end(), results.begin(), results.end()); // Collision among planned_articulation_map_ for (const auto &[art_name2, art2] : planned_articulation_map_) { if (art_name == art_name2) continue; - for (auto &result : fcl_model->checkCollisionWith(art2->getFCLModel(), request)) { + for (auto &result : + fcl_model->checkCollisionWith(art2->getFCLModel(), request, acm_)) { result.collision_type = "self_self"; ret.push_back(result); } @@ -308,7 +297,7 @@ std::vector> PlanningWorldTpl::checkSelfCollision( // Articulation collide with attached_body_map_ for (const auto &[attached_body_name, attached_body] : attached_body_map_) for (auto &result : - fcl_model->checkCollisionWith(attached_body->getObject(), request)) { + fcl_model->checkCollisionWith(attached_body->getObject(), request, acm_)) { result.collision_type = "self_attached"; ret.push_back(result); } @@ -317,22 +306,25 @@ std::vector> PlanningWorldTpl::checkSelfCollision( // Collision among attached_body_map_ for (auto it = attached_body_map_.begin(); it != attached_body_map_.end(); ++it) for (auto it2 = attached_body_map_.begin(); it2 != it; ++it2) { - result.clear(); - collision_detection::fcl::collide(it->second->getObject(), - it2->second->getObject(), request, result); - if (result.isCollision()) { - auto name1 = it->first, name2 = it2->first; - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "attached_attached"; - tmp.object_name1 = name1; - tmp.object_name2 = name2; - tmp.link_name1 = name1; - tmp.link_name2 = name2; - ret.push_back(tmp); + auto name1 = it->first, name2 = it2->first; + if (auto type = acm_->getAllowedCollision(name1, name2); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::collide(it->second->getObject(), + it2->second->getObject(), request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "attached_attached"; + tmp.object_name1 = name1; + tmp.object_name2 = name2; + tmp.link_name1 = name1; + tmp.link_name2 = name2; + ret.push_back(tmp); + } } } - return filterCollisions(ret); + return ret; } template @@ -358,13 +350,14 @@ std::vector> PlanningWorldTpl::checkRobotCollision auto fcl_model = art->getFCLModel(); // Collision with unplanned articulation for (const auto &art2 : unplanned_articulations) { - const auto results = fcl_model->checkCollisionWith(art2->getFCLModel(), request); + const auto results = + fcl_model->checkCollisionWith(art2->getFCLModel(), request, acm_); ret.insert(ret.end(), results.begin(), results.end()); } // Collision with scene objects for (const auto &scene_obj : scene_objects) - for (auto &result : fcl_model->checkCollisionWith(scene_obj, request)) { + for (auto &result : fcl_model->checkCollisionWith(scene_obj, request, acm_)) { result.collision_type = "self_sceneobject"; ret.push_back(result); } @@ -376,28 +369,30 @@ std::vector> PlanningWorldTpl::checkRobotCollision // Collision with unplanned articulation for (const auto &art2 : unplanned_articulations) for (auto &result : - art2->getFCLModel()->checkCollisionWith(attached_obj, request)) { + art2->getFCLModel()->checkCollisionWith(attached_obj, request, acm_)) { result.collision_type = "attached_articulation"; ret.push_back(result); } // Collision with scene objects - for (const auto &scene_obj : scene_objects) { - result.clear(); - collision_detection::fcl::collide(attached_obj, scene_obj, request, result); - if (result.isCollision()) { - WorldCollisionResult tmp; - tmp.res = result; - tmp.collision_type = "attached_sceneobject"; - tmp.object_name1 = attached_body_name; - tmp.object_name2 = scene_obj->name; - tmp.link_name1 = attached_body_name; - tmp.link_name2 = scene_obj->name; - ret.push_back(tmp); + for (const auto &scene_obj : scene_objects) + if (auto type = acm_->getAllowedCollision(attached_body_name, scene_obj->name); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::collide(attached_obj, scene_obj, request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "attached_sceneobject"; + tmp.object_name1 = attached_body_name; + tmp.object_name2 = scene_obj->name; + tmp.link_name1 = attached_body_name; + tmp.link_name2 = scene_obj->name; + ret.push_back(tmp); + } } - } } - return filterCollisions(ret); + return ret; } template From c73c37eed246583742e806fd620556dd8fa0c1b4 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 13:42:49 -0700 Subject: [PATCH 19/37] Add constructor to WorldCollisionResult and WorldDistanceResult --- .../collision_detection/collision_common.h | 33 +++++++++++++++++ mplib/pymp/collision_detection/__init__.pyi | 37 +++++++++++++++++++ .../pybind_collision_common.cpp | 19 ++++++++++ .../collision_detection/collision_common.h | 16 ++++++++ 4 files changed, 105 insertions(+) diff --git a/include/mplib/collision_detection/collision_common.h b/include/mplib/collision_detection/collision_common.h index b91db21a..4158d564 100644 --- a/include/mplib/collision_detection/collision_common.h +++ b/include/mplib/collision_detection/collision_common.h @@ -14,6 +14,22 @@ struct WorldCollisionResultTpl { // TODO: Update with // https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1CollisionResult.html + /// @brief Default constructor + WorldCollisionResultTpl() {}; + + /// Constructor with all members + WorldCollisionResultTpl(const ::fcl::CollisionResult &res, + const std::string &collision_type, + const std::string &object_name1, + const std::string &object_name2, + const std::string &link_name1, const std::string &link_name2) + : res(res), + collision_type(collision_type), + object_name1(object_name1), + object_name2(object_name2), + link_name1(link_name1), + link_name2(link_name2) {}; + ::fcl::CollisionResult res; ///< the fcl CollisionResult std::string collision_type, ///< type of the collision object_name1, ///< name of the first object @@ -28,6 +44,23 @@ struct WorldDistanceResultTpl { // TODO: Update with // https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1DistanceResult.html + /// @brief Default constructor + WorldDistanceResultTpl() {}; + + /// Constructor with all members + WorldDistanceResultTpl(const ::fcl::DistanceResult &res, S min_distance, + const std::string &distance_type, + const std::string &object_name1, + const std::string &object_name2, const std::string &link_name1, + const std::string &link_name2) + : res(res), + min_distance(min_distance), + distance_type(distance_type), + object_name1(object_name1), + object_name2(object_name2), + link_name1(link_name1), + link_name2(link_name2) {}; + ::fcl::DistanceResult res; ///< the fcl DistanceResult /// minimum distance between the two objects S min_distance {std::numeric_limits::max()}; diff --git a/mplib/pymp/collision_detection/__init__.pyi b/mplib/pymp/collision_detection/__init__.pyi index 02a7cc4a..050cf1e9 100644 --- a/mplib/pymp/collision_detection/__init__.pyi +++ b/mplib/pymp/collision_detection/__init__.pyi @@ -205,6 +205,24 @@ class WorldCollisionResult: """ Result of the collision checking. """ + @typing.overload + def __init__(self) -> None: + """ + Default constructor + """ + @typing.overload + def __init__( + self, + res: ..., + collision_type: str, + object_name1: str, + object_name2: str, + link_name1: str, + link_name2: str, + ) -> None: + """ + Constructor with all members + """ @property def collision_type(self) -> str: """ @@ -240,6 +258,25 @@ class WorldDistanceResult: """ Result of minimum distance-to-collision query. """ + @typing.overload + def __init__(self) -> None: + """ + Default constructor + """ + @typing.overload + def __init__( + self, + res: ..., + min_distance: float, + distance_type: str, + object_name1: str, + object_name2: str, + link_name1: str, + link_name2: str, + ) -> None: + """ + Constructor with all members + """ @property def distance_type(self) -> str: """ diff --git a/pybind/collision_detection/pybind_collision_common.cpp b/pybind/collision_detection/pybind_collision_common.cpp index 3e698321..74c35e48 100644 --- a/pybind/collision_detection/pybind_collision_common.cpp +++ b/pybind/collision_detection/pybind_collision_common.cpp @@ -10,6 +10,8 @@ namespace py = pybind11; namespace mplib::collision_detection { +using CollisionResult = ::fcl::CollisionResult; +using DistanceResult = ::fcl::DistanceResult; using WorldCollisionResult = WorldCollisionResultTpl; using WorldDistanceResult = WorldDistanceResultTpl; @@ -19,6 +21,14 @@ void build_pycollision_common(py::module &m) { m, "WorldCollisionResult", DOC(mplib, collision_detection, WorldCollisionResultTpl)); PyWorldCollisionResult + .def(py::init<>(), DOC(mplib, collision_detection, WorldCollisionResultTpl, + WorldCollisionResultTpl)) + .def(py::init(), + py::arg("res"), py::arg("collision_type"), py::arg("object_name1"), + py::arg("object_name2"), py::arg("link_name1"), py::arg("link_name2"), + DOC(mplib, collision_detection, WorldCollisionResultTpl, + WorldCollisionResultTpl, 2)) .def_readonly("res", &WorldCollisionResult::res, DOC(mplib, collision_detection, WorldCollisionResultTpl, res)) .def_readonly( @@ -42,6 +52,15 @@ void build_pycollision_common(py::module &m) { m, "WorldDistanceResult", DOC(mplib, collision_detection, WorldDistanceResultTpl)); PyWorldDistanceResult + .def(py::init<>(), DOC(mplib, collision_detection, WorldDistanceResultTpl, + WorldDistanceResultTpl)) + .def(py::init(), + py::arg("res"), py::arg("min_distance"), py::arg("distance_type"), + py::arg("object_name1"), py::arg("object_name2"), py::arg("link_name1"), + py::arg("link_name2"), + DOC(mplib, collision_detection, WorldDistanceResultTpl, + WorldDistanceResultTpl, 2)) .def_readonly("res", &WorldDistanceResult::res, DOC(mplib, collision_detection, WorldDistanceResultTpl, res)) .def_readonly( diff --git a/pybind/docstring/collision_detection/collision_common.h b/pybind/docstring/collision_detection/collision_common.h index c4bc278e..e2f3f4d9 100644 --- a/pybind/docstring/collision_detection/collision_common.h +++ b/pybind/docstring/collision_detection/collision_common.h @@ -26,6 +26,14 @@ static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl = R"doc(Result of the collision checking.)doc"; +static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_WorldCollisionResultTpl = +R"doc( +Default constructor)doc"; + +static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_WorldCollisionResultTpl_2 = +R"doc( +Constructor with all members)doc"; + static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_collision_type = R"doc(type of the collision)doc"; static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_link_name1 = R"doc(link name of the first object in collision)doc"; @@ -40,6 +48,14 @@ static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_res = static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl = R"doc(Result of minimum distance-to-collision query.)doc"; +static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_WorldDistanceResultTpl = +R"doc( +Default constructor)doc"; + +static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_WorldDistanceResultTpl_2 = +R"doc( +Constructor with all members)doc"; + static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_distance_type = R"doc(type of the distance result)doc"; static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_link_name1 = R"doc(link name of the first object)doc"; From b16bad1aa1f39a4135eb304128a3c55af9a5e598 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 14:13:36 -0700 Subject: [PATCH 20/37] Make acm kw_only in FCLModel collision/distance methods --- mplib/pymp/collision_detection/fcl.pyi | 12 +++++++++-- .../fcl/pybind_fcl_model.cpp | 20 +++++++++---------- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/mplib/pymp/collision_detection/fcl.pyi b/mplib/pymp/collision_detection/fcl.pyi index eec866b4..df8dd8ca 100644 --- a/mplib/pymp/collision_detection/fcl.pyi +++ b/mplib/pymp/collision_detection/fcl.pyi @@ -585,6 +585,7 @@ class FCLModel: self, other: FCLModel, request: CollisionRequest = ..., + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: """ @@ -602,6 +603,7 @@ class FCLModel: self, object: ..., request: CollisionRequest = ..., + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: """ @@ -617,6 +619,7 @@ class FCLModel: def check_self_collision( self, request: CollisionRequest = ..., + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> list[mplib.pymp.collision_detection.WorldCollisionResult]: """ @@ -631,6 +634,7 @@ class FCLModel: def distance_self( self, request: DistanceRequest = ..., + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> mplib.pymp.collision_detection.WorldDistanceResult: """ @@ -646,6 +650,7 @@ class FCLModel: def distance_to_collision_with( self, other: FCLModel, + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> float: """ @@ -661,6 +666,7 @@ class FCLModel: def distance_to_collision_with( self, object: ..., + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> float: """ @@ -673,7 +679,7 @@ class FCLModel: :return: minimum distance-to-collision with the ``FCLObject`` """ def distance_to_self_collision( - self, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... + self, *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... ) -> float: """ The minimum distance to self-collision given the robot in current state, @@ -688,6 +694,7 @@ class FCLModel: self, other: FCLModel, request: DistanceRequest = ..., + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> mplib.pymp.collision_detection.WorldDistanceResult: """ @@ -705,6 +712,7 @@ class FCLModel: self, object: ..., request: DistanceRequest = ..., + *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ..., ) -> mplib.pymp.collision_detection.WorldDistanceResult: """ @@ -739,7 +747,7 @@ class FCLModel: :return: name of the articulated model """ def is_state_colliding( - self, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... + self, *, acm: mplib.pymp.collision_detection.AllowedCollisionMatrix = ... ) -> bool: """ Check if the current state is in self-collision, ignoring the distances between diff --git a/pybind/collision_detection/fcl/pybind_fcl_model.cpp b/pybind/collision_detection/fcl/pybind_fcl_model.cpp index 38f145c7..5b26f615 100644 --- a/pybind/collision_detection/fcl/pybind_fcl_model.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl_model.cpp @@ -65,60 +65,60 @@ void build_pyfcl_model(py::module &m) { py::arg("link_poses"), DOC(mplib, collision_detection, fcl, FCLModelTpl, updateCollisionObjects)) - .def("is_state_colliding", &FCLModel::isStateColliding, + .def("is_state_colliding", &FCLModel::isStateColliding, py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, isStateColliding)) .def("check_self_collision", &FCLModel::checkSelfCollision, - py::arg("request") = CollisionRequest(), + py::arg("request") = CollisionRequest(), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, checkSelfCollision)) .def("check_collision_with", py::overload_cast( &FCLModel::checkCollisionWith, py::const_), - py::arg("other"), py::arg("request") = CollisionRequest(), + py::arg("other"), py::arg("request") = CollisionRequest(), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith)) .def("check_collision_with", py::overload_cast &, const CollisionRequest &, const AllowedCollisionMatrixPtr &>( &FCLModel::checkCollisionWith, py::const_), - py::arg("object"), py::arg("request") = CollisionRequest(), + py::arg("object"), py::arg("request") = CollisionRequest(), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, checkCollisionWith, 2)) .def("distance_to_self_collision", &FCLModel::distanceToSelfCollision, - py::arg("acm") = std::make_shared(), + py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceToSelfCollision)) .def("distance_self", &FCLModel::distanceSelf, - py::arg("request") = DistanceRequest(), + py::arg("request") = DistanceRequest(), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceSelf)) .def("distance_to_collision_with", py::overload_cast( &FCLModel::distanceToCollisionWith, py::const_), - py::arg("other"), + py::arg("other"), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceToCollisionWith)) .def("distance_with", py::overload_cast(&FCLModel::distanceWith, py::const_), - py::arg("other"), py::arg("request") = DistanceRequest(), + py::arg("other"), py::arg("request") = DistanceRequest(), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceWith)) .def( "distance_to_collision_with", py::overload_cast &, const AllowedCollisionMatrixPtr &>( &FCLModel::distanceToCollisionWith, py::const_), - py::arg("object"), + py::arg("object"), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceToCollisionWith, 2)) .def("distance_with", py::overload_cast &, const DistanceRequest &, const AllowedCollisionMatrixPtr &>(&FCLModel::distanceWith, py::const_), - py::arg("object"), py::arg("request") = DistanceRequest(), + py::arg("object"), py::arg("request") = DistanceRequest(), py::kw_only(), py::arg("acm") = std::make_shared(), DOC(mplib, collision_detection, fcl, FCLModelTpl, distanceWith, 2)); } From 03e11c496de86aa14ce028b00fe10bf9a451c6ef Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 14:14:10 -0700 Subject: [PATCH 21/37] Add check_collision_between/distance_between for SapienPlanningWorld Between two instances of PhysxArticulation or Entity --- mplib/sapien_utils/conversion.py | 127 ++++++++++++++++++++++++++++++- 1 file changed, 126 insertions(+), 1 deletion(-) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 89a34456..457bd587 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -20,6 +20,12 @@ from ..planner import Planner from ..pymp import ArticulatedModel, PlanningWorld, Pose +from ..pymp.collision_detection import ( + AllowedCollision, + AllowedCollisionMatrix, + WorldCollisionResult, + WorldDistanceResult, +) from ..pymp.collision_detection.fcl import ( Box, BVHModel, @@ -27,6 +33,7 @@ CollisionObject, Convex, Cylinder, + FCLModel, FCLObject, Halfspace, Sphere, @@ -38,7 +45,7 @@ from .urdf_exporter import export_kinematic_chain_urdf -# TODO(merge): link names? +# TODO: link names? def convert_object_name(obj: PhysxArticulation | Entity) -> str: """ Constructs a unique name for the corresponding mplib object. @@ -162,6 +169,124 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None "The scene might have changed since last update." ) + def check_collision_between( + self, + obj_A: PhysxArticulation | Entity, + obj_B: PhysxArticulation | Entity, + *, + acm: AllowedCollisionMatrix = AllowedCollisionMatrix(), # noqa: B008 + ) -> list[WorldCollisionResult]: + """ + Check collision between two objects, + which can either be a PhysxArticulation or an Entity. + + :param obj_A: object A to check for collision. + :param obj_B: object B to check for collision. + :param acm: allowed collision matrix. + :return: a list of WorldCollisionResult. Empty if there's no collision. + """ + col_obj_A = self._get_collision_obj(obj_A) + col_obj_B = self._get_collision_obj(obj_B) + + if isinstance(obj_A, PhysxArticulation): # A is articulation, B is anything + assert isinstance(col_obj_A, FCLModel), f"Wrong type: {type(col_obj_A)}" + return col_obj_A.check_collision_with(col_obj_B, acm=acm) + elif isinstance(obj_B, PhysxArticulation): # A is object, B is articulation + assert isinstance(col_obj_B, FCLModel), f"Wrong type: {type(col_obj_B)}" + return col_obj_B.check_collision_with(col_obj_A, acm=acm) + elif isinstance(obj_B, Entity): # A is object, B is object + assert isinstance(col_obj_A, FCLObject) and isinstance( + col_obj_B, FCLObject + ), f"Wrong type: col_obj_A={type(col_obj_A)}, col_obj_B={type(col_obj_B)}" + if ( + acm_type := acm.get_allowed_collision(col_obj_A.name, col_obj_B.name) + ) is None or acm_type == AllowedCollision.NEVER: + result = collide(col_obj_A, col_obj_B) + if result.is_collision(): + return [ + WorldCollisionResult( + result, + "object_object", + col_obj_A.name, + col_obj_B.name, + col_obj_A.name, + col_obj_B.name, + ) + ] + return [] + else: + raise NotImplementedError(f"obj_A={obj_A}, obj_B={obj_B}") + + def distance_between( + self, + obj_A: PhysxArticulation | Entity, + obj_B: PhysxArticulation | Entity, + *, + acm: AllowedCollisionMatrix = AllowedCollisionMatrix(), # noqa: B008 + return_distance_only: bool = True, + ) -> WorldDistanceResult | float: + """ + Check distance-to-collision between two objects, + which can either be a PhysxArticulation or an Entity. + + :param obj_A: object A to check for collision. + :param obj_B: object B to check for collision. + :param acm: allowed collision matrix. + :param return_distance_only: if True, return distance only. + :return: a WorldDistanceResult or a float if return_distance_only==True. + """ + col_obj_A = self._get_collision_obj(obj_A) + col_obj_B = self._get_collision_obj(obj_B) + ret = WorldDistanceResult() + + if isinstance(obj_A, PhysxArticulation): # A is articulation, B is anything + assert isinstance(col_obj_A, FCLModel), f"Wrong type: {type(col_obj_A)}" + ret = col_obj_A.distance_with(col_obj_B, acm=acm) + elif isinstance(obj_B, PhysxArticulation): # A is object, B is articulation + assert isinstance(col_obj_B, FCLModel), f"Wrong type: {type(col_obj_B)}" + ret = col_obj_B.distance_with(col_obj_A, acm=acm) + elif isinstance(obj_B, Entity): # A is object, B is object + assert isinstance(col_obj_A, FCLObject) and isinstance( + col_obj_B, FCLObject + ), f"Wrong type: col_obj_A={type(col_obj_A)}, col_obj_B={type(col_obj_B)}" + if ( + acm_type := acm.get_allowed_collision(col_obj_A.name, col_obj_B.name) + ) is None or acm_type == AllowedCollision.NEVER: + result = distance(col_obj_A, col_obj_B) + ret = WorldDistanceResult( + result, + result.min_distance, + "object_object", + col_obj_A.name, + col_obj_B.name, + col_obj_A.name, + col_obj_B.name, + ) + else: + raise NotImplementedError(f"obj_A={obj_A}, obj_B={obj_B}") + + return ret.min_distance if return_distance_only else ret + + def _get_collision_obj( + self, + obj: PhysxArticulation | Entity, + ) -> FCLModel | FCLObject | None: + """Helper function to get mplib collision object from sapien object""" + if isinstance(obj, PhysxArticulation) and ( + articulation := self.get_articulation(convert_object_name(obj)) + ): + return articulation.get_fcl_model() + elif isinstance(obj, Entity) and ( + fcl_obj := self.get_object(convert_object_name(obj)) + ): + return fcl_obj + else: + raise RuntimeError( + f"Unknown SAPIEN object type: {type(obj)} or " + f"Object {obj.name} not found in PlanningWorld " + "(The scene might have changed since last update)" + ) + @staticmethod def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: """ From 041a305e6d02fdffe23c5c25cd51967538238c82 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 14:24:35 -0700 Subject: [PATCH 22/37] Add convex argument to PlanningWorld::attachMesh --- include/mplib/planning_world.h | 3 ++- mplib/pymp/__init__.pyi | 9 ++++++++- pybind/docstring/planning_world.h | 3 ++- pybind/pybind_planning_world.cpp | 4 ++-- src/planning_world.cpp | 16 +++++++++++----- 5 files changed, 25 insertions(+), 10 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 8422ae13..7fdcaadd 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -342,9 +342,10 @@ class PlanningWorldTpl { * @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 convex: whether to load mesh as a convex mesh. Default: ``false``. */ void attachMesh(const std::string &mesh_path, const std::string &art_name, - int link_id, const Pose &pose); + int link_id, const Pose &pose, bool convex = false); /** * Detaches object with given name. diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi index 60b12e2a..4635178a 100644 --- a/mplib/pymp/__init__.pyi +++ b/mplib/pymp/__init__.pyi @@ -394,7 +394,13 @@ class PlanningWorld: :param pose: attached pose (relative pose from attached link to object) """ def attach_mesh( - self, mesh_path: str, art_name: str, link_id: int, pose: Pose + self, + mesh_path: str, + art_name: str, + link_id: int, + pose: Pose, + *, + convex: bool = False, ) -> None: """ Attaches given mesh to specified link of articulation (auto touch_links) @@ -403,6 +409,7 @@ class PlanningWorld: :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 convex: whether to load mesh as a convex mesh. Default: ``False``. """ @typing.overload def attach_object( diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index da6286b7..afb70104 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -88,7 +88,8 @@ Attaches given mesh to specified link of articulation (auto touch_links) :param mesh_path: path to a mesh file :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"; +:param pose: attached pose (relative pose from attached link to object) +:param convex: whether to load mesh as a convex mesh. Default: ``False``.)doc"; static const char *__doc_mplib_PlanningWorldTpl_attachObject = R"doc( diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 13b5029b..589c1b0e 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -119,8 +119,8 @@ void build_pyplanning_world(py::module &pymp) { py::arg("art_name"), py::arg("link_id"), py::arg("pose"), DOC(mplib, PlanningWorldTpl, attachBox)) .def("attach_mesh", &PlanningWorld::attachMesh, py::arg("mesh_path"), - py::arg("art_name"), py::arg("link_id"), py::arg("pose"), - DOC(mplib, PlanningWorldTpl, attachMesh)) + py::arg("art_name"), py::arg("link_id"), py::arg("pose"), py::kw_only(), + 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("print_attached_body_pose", &PlanningWorld::printAttachedBodyPose, diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 5fa8997d..b9f9fe1b 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -216,13 +216,19 @@ void PlanningWorldTpl::attachBox(const Vector3 &size, const std::string &a template void PlanningWorldTpl::attachMesh(const std::string &mesh_path, const std::string &art_name, int link_id, - const Pose &pose) { - // TODO(merge): Convex mesh? + const Pose &pose, bool convex) { // FIXME: Use link_name to avoid changes auto name = art_name + "_" + std::to_string(link_id) + "_mesh"; - attachObject( - name, collision_detection::fcl::loadMeshAsBVH(mesh_path, Vector3 {1, 1, 1}), - art_name, link_id, pose); + if (convex) + attachObject( + name, + collision_detection::fcl::loadMeshAsConvex(mesh_path, Vector3 {1, 1, 1}), + art_name, link_id, pose); + else + attachObject( + name, + collision_detection::fcl::loadMeshAsBVH(mesh_path, Vector3 {1, 1, 1}), + art_name, link_id, pose); } template From b0a320dc155bef584375b5fee46a2780817b984c Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 15:43:45 -0700 Subject: [PATCH 23/37] Move generate_srdf/replace_urdf_package_keyword functions to mplib.urdf_utils --- mplib/planner.py | 143 ++++----------------------------- mplib/urdf_utils.py | 188 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 204 insertions(+), 127 deletions(-) create mode 100644 mplib/urdf_utils.py diff --git a/mplib/planner.py b/mplib/planner.py index 9a8e452a..e760ea39 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -15,6 +15,7 @@ from mplib.pymp.collision_detection import AllowedCollisionMatrix, WorldCollisionResult from mplib.pymp.collision_detection.fcl import CollisionGeometry, FCLObject from mplib.pymp.planning import ompl +from mplib.urdf_utils import generate_srdf, replace_urdf_package_keyword class Planner: @@ -28,8 +29,8 @@ def __init__( urdf: str, move_group: str, *, - srdf: str = "", - package_keyword_replacement: str = "", + srdf: Optional[str] = None, + new_package_keyword: str = "", use_convex: bool = False, user_link_names: Sequence[str] = [], user_joint_names: Sequence[str] = [], @@ -48,7 +49,7 @@ def __init__( :param urdf: Unified Robot Description Format file. :param move_group: target link to move, usually the end-effector. :param srdf: Semantic Robot Description Format file. - :param package_keyword_replacement: replace ``package://`` keyword in URDF + :param new_package_keyword: the string to replace 'package://' keyword :param use_convex: if True, load collision mesh as convex mesh. If mesh is not convex, a ``RuntimeError`` will be raised. :param user_link_names: names of links, the order matters. @@ -62,23 +63,27 @@ def __init__( :param objects: list of FCLObject as non-articulated collision objects :param verbose: if True, print verbose logs for debugging """ - self.urdf = urdf - self.srdf = srdf - if self.srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")): - self.srdf = urdf.replace(".urdf", ".srdf") - print(f"No SRDF file provided but found {self.srdf}") + self.urdf, self.srdf = urdf, srdf + if self.srdf is None: + if os.path.exists(srdf := urdf.replace(".urdf", ".srdf")): + print(f"No SRDF file provided but found {self.srdf}") + self.srdf = srdf + else: + self.srdf = generate_srdf(self.urdf, new_package_keyword, verbose=True) # replace package:// keyword if exists - urdf = self.replace_package_keyword(package_keyword_replacement) + self.urdf = replace_urdf_package_keyword(self.urdf, new_package_keyword) self.robot = ArticulatedModel( - self.urdf, - self.srdf, + str(self.urdf), + str(self.srdf), link_names=user_link_names, # type: ignore joint_names=user_joint_names, # type: ignore convex=use_convex, verbose=verbose, ) + # Remove the temporary URDF file from replace_urdf_package_keyword() + self.urdf.unlink() self.pinocchio_model = self.robot.get_pinocchio_model() self.user_link_names = self.pinocchio_model.get_link_names() self.user_joint_names = self.pinocchio_model.get_joint_names() @@ -127,124 +132,8 @@ def __init__( self.planning_world = PlanningWorld([self.robot], objects) self.acm = self.planning_world.get_allowed_collision_matrix() - if self.srdf == "": - self.generate_collision_pair() - self.robot.update_SRDF(self.srdf) - self.planner = ompl.OMPLPlanner(world=self.planning_world) - def replace_package_keyword(self, package_keyword_replacement: str): - # TODO(merge): fix file writing - # TODO(merge): convert to staticmethod - """ - some ROS URDF files use package:// keyword to refer the package dir - replace it with the given string (default is empty) - - Args: - package_keyword_replacement: the string to replace 'package://' keyword - """ - rtn_urdf = self.urdf - with open(self.urdf) as in_f: - content = in_f.read() - if "package://" in content: - rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf") - content = content.replace("package://", package_keyword_replacement) - if not os.path.exists(rtn_urdf): - with open(rtn_urdf, "w") as out_f: - out_f.write(content) - return rtn_urdf - - def generate_collision_pair(self, num_samples=100000): - # TODO(merge): convert to staticmethod - """ - We read the srdf file to get the link pairs that should not collide. - If not provided, we need to randomly sample configurations - to find the link pairs that will always collide. - - :param num_samples: number of samples to find the link that will always collide - """ - print( - "Since no SRDF file is provided. We will first detect link pairs that will" - " always collide. This may take several minutes." - ) - - root = ET.Element("robot") - robot_name = self.urdf.split("/")[-1].split(".")[0] - root.set("name", robot_name) - self.srdf = self.urdf.replace(".urdf", ".srdf") - - acm = AllowedCollisionMatrix() - - # 1. disable adjacent link pairs - for link1, link2 in self.pinocchio_model.get_adjacent_links(): - print(f"Ignore collision pair: ({link1}, {link2}), reason: Adjacent") - acm.set_entry(link1, link2, True) - _ = ET.SubElement( - root, - "disable_collisions", - attrib={"link1": link1, "link2": link2, "reason": "Adjacent"}, - ) - - # 2. disable all-zeros qpos (default) collision - self.robot.set_qpos(np.zeros(len(self.user_joint_names)), True) - for collision in self.check_for_self_collision(): - link1, link2 = collision.link_name1, collision.link_name2 - if acm.get_entry(link1, link2) is not None: # already ignored - continue - print( - f"Ignore collision pair: ({link1}, {link2}), " - "reason: Default (collides at all-zeros qpos)" - ) - acm.set_entry(link1, link2, True) - _ = ET.SubElement( - root, - "disable_collisions", - attrib={"link1": link1, "link2": link2, "reason": "Default"}, - ) - - # 3. disable collision pairs that always collide and never collide via sampling - n_links = len(self.user_link_names) - collision_cnt = np.zeros((n_links, n_links), dtype=int) - for _ in range(num_samples): - self.robot.set_qpos(self.pinocchio_model.get_random_configuration(), True) - for collision in self.check_for_self_collision(): - u = self.link_name_2_idx[collision.link_name1] - v = self.link_name_2_idx[collision.link_name2] - collision_cnt[u][v] += 1 - - for i, link1 in enumerate(self.user_link_names): - for j in range(i + 1, n_links): - link2 = self.user_link_names[j] - if acm.get_entry(link1, link2) is not None: # already ignored - continue - if (cnt := (collision_cnt[i][j] + collision_cnt[j][i])) == num_samples: - print( - f"Ignore collision pair: ({link1}, {link2}), " - "reason: Always collide" - ) - _ = ET.SubElement( - root, - "disable_collisions", - attrib={"link1": link1, "link2": link2, "reason": "Always"}, - ) - elif cnt == 0: - print( - f"Ignore collision pair: ({link1}, {link2}), " - "reason: Never collide" - ) - _ = ET.SubElement( - root, - "disable_collisions", - attrib={"link1": link1, "link2": link2, "reason": "Never"}, - ) - - # Save SRDF - with open(self.srdf, "w") as srdf_file: - srdf_file.write( - minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") - ) - print(f"Saving the SRDF file to {self.srdf}") - def wrap_joint_limit(self, qpos: np.ndarray) -> bool: """ Checks if the joint configuration can be wrapped to be within the joint limits. diff --git a/mplib/urdf_utils.py b/mplib/urdf_utils.py new file mode 100644 index 00000000..3e331807 --- /dev/null +++ b/mplib/urdf_utils.py @@ -0,0 +1,188 @@ +import tempfile +import xml.etree.ElementTree as ET +from pathlib import Path +from typing import Optional +from xml.dom import minidom + +import numpy as np + +from mplib.pymp import ArticulatedModel +from mplib.pymp.collision_detection import AllowedCollisionMatrix + + +def compute_default_collisions( + robot: ArticulatedModel, *, num_samples=100000, verbose=False +) -> str: + """ + Compute default collision pairs for generating SRDF. + + This function mimics MoveIt2's + ``moveit_setup::srdf_setup::computeDefaultCollisions()`` + + Reference: + https://moveit.picknik.ai/main/api/html/namespacemoveit__setup_1_1srdf__setup.html#a2812f73b447d838cd7dba1b0ee1a0c95 + + :param robot: an ``ArticulatedModel`` + :param num_samples: number of samples to find the link that will always collide + :param verbose: print debug info + :return: SRDF content as an XML string + """ + if verbose: + print( + "Generating SRDF with default collision pairs. " + "This may take several minutes." + ) + + root = ET.Element("robot", {"name": robot.name}) + + pinocchio_model = robot.get_pinocchio_model() + user_link_names = pinocchio_model.get_link_names() + user_joint_names = pinocchio_model.get_joint_names() + link_name_2_idx = {link: i for i, link in enumerate(user_link_names)} + fcl_model = robot.get_fcl_model() + acm = AllowedCollisionMatrix() + + # 1. disable adjacent link pairs + for link1, link2 in pinocchio_model.get_adjacent_links(): + if verbose: + print(f"Ignore collision pair: ({link1}, {link2}), reason: Adjacent") + acm.set_entry(link1, link2, True) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Adjacent"}, + ) + + # 2. disable all-zeros qpos (default) collision + robot.set_qpos(np.zeros(len(user_joint_names)), True) + for collision in fcl_model.check_self_collision(): + link1, link2 = collision.link_name1, collision.link_name2 + if acm.get_entry(link1, link2) is not None: # already ignored + continue + if verbose: + print( + f"Ignore collision pair: ({link1}, {link2}), " + "reason: Default (collides at all-zeros qpos)" + ) + acm.set_entry(link1, link2, True) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Default"}, + ) + + # 3. disable collision pairs that always collide and never collide via sampling + n_links = len(user_link_names) + collision_cnt = np.zeros((n_links, n_links), dtype=int) + for _ in range(num_samples): + robot.set_qpos(pinocchio_model.get_random_configuration(), True) + for collision in fcl_model.check_self_collision(): + u = link_name_2_idx[collision.link_name1] + v = link_name_2_idx[collision.link_name2] + collision_cnt[u][v] += 1 + + for i, link1 in enumerate(user_link_names): + for j in range(i + 1, n_links): + link2 = user_link_names[j] + if acm.get_entry(link1, link2) is not None: # already ignored + continue + if (cnt := (collision_cnt[i][j] + collision_cnt[j][i])) == num_samples: + if verbose: + print( + f"Ignore collision pair: ({link1}, {link2}), " + "reason: Always collide" + ) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Always"}, + ) + elif cnt == 0: + if verbose: + print( + f"Ignore collision pair: ({link1}, {link2}), " + "reason: Never collide" + ) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Never"}, + ) + + return minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") + + +def replace_urdf_package_keyword( + urdf_path: str | Path, + new_package_keyword: str = "", + *, + save_path: Optional[str | Path] = None, +) -> Path: + """ + Some ROS URDF files use package:// keyword to refer the package dir. + Replace it with the given string (default is empty) + + :param urdf_path: Path to a Unified Robot Description Format file. + :param new_package_keyword: the string to replace 'package://' keyword + :param save_path: Path to save the modified URDF file. + If ``None``, create a temporary file. + :return: Path to the modified URDF file + """ + if save_path is None: + _, save_path = tempfile.mkstemp(suffix=".urdf") + save_path = Path(save_path) + if not (save_dir := save_path.parent).is_dir(): + save_dir.mkdir(parents=True) + + with Path(urdf_path).open("r") as in_f: + if "package://" in (content := in_f.read()): + with save_path.open("w") as out_f: + out_f.write(content.replace("package://", new_package_keyword)) + return save_path + + +def generate_srdf( + urdf_path: str | Path, + new_package_keyword: str = "", + *, + num_samples=100000, + save_path: Optional[str | Path] = None, + verbose=False, +) -> Path: + """ + Generate SRDF from URDF similar to MoveIt2's setup assistant. + + :param urdf_path: Path to a Unified Robot Description Format file. + :param new_package_keyword: the string to replace 'package://' keyword + :param num_samples: number of samples to find the link that will always collide + :param save_path: Path to save the generated SRDF file. + If ``None``, create a temporary file. + :param verbose: print debug info + :return: Path to the generated SRDF file + """ + assert Path(urdf_path).is_file(), f"URDF file {urdf_path} does not exist" + + # Replace 'package://' keyword + urdf_path = replace_urdf_package_keyword(urdf_path, new_package_keyword) + + robot = ArticulatedModel(str(urdf_path), "") + srdf_str = compute_default_collisions( + robot, num_samples=num_samples, verbose=verbose + ) + + if save_path is None: + _, save_path = tempfile.mkstemp(suffix=".srdf") + save_path = Path(save_path) + if not (save_dir := save_path.parent).is_dir(): + save_dir.mkdir(parents=True) + + # Save SRDF + with save_path.open("w") as f: + f.write(srdf_str) + + if verbose: + print(f"Saved the SRDF file to {save_path}") + + # Remove the temporary URDF file from replace_urdf_package_keyword() + urdf_path.unlink() + return save_path From d6049ed8a54840a27bb02c03663df8ce088339be Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 15:50:19 -0700 Subject: [PATCH 24/37] Remove duplicate IK call in Planner.plan_qpos_to_pose --- mplib/planner.py | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index e760ea39..e9f87ee4 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -666,19 +666,6 @@ def plan_qpos_to_pose( # we need to take only the move_group joints when planning # idx = self.move_group_joint_indices - # TODO(merge): verify this - ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) - if ik_status != "Success": - return {"status": ik_status} - - if verbose: - print("IK results:") - for i in range(len(goal_qpos)): # type: ignore - print(goal_qpos[i]) # type: ignore - - # goal_qpos_ = [goal_qpos[i][move_joint_idx] for i in range(len(goal_qpos))] - self.robot.set_qpos(current_qpos, True) - ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) if ik_status != "Success": return {"status": ik_status} From 9a69bb3dc88fcc4458f927d588f24b4243370da9 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 15:57:14 -0700 Subject: [PATCH 25/37] Rename Planner.plan_qpos_to_qpos/plan_qpos_to_pose methods * Planner.plan_qpos_to_qpos => Planner.plan_qpos * Planner.plan_qpos_to_pose => Planner.plan_pose --- mplib/planner.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index e9f87ee4..cc2ce7a2 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -520,7 +520,7 @@ def remove_object(self, name) -> bool: """returns true if the object was removed, false if it was not found""" return self.planning_world.remove_object(name) - def plan_qpos_to_qpos( + def plan_qpos( self, goal_qposes: list[np.ndarray], current_qpos: np.ndarray, @@ -617,7 +617,7 @@ def _transform_goal_to_wrt_base(self, goal_pose: Pose) -> Pose: """Converts goal pose from T_world_goal to T_base_goal""" return self.robot.get_base_pose().inv() * goal_pose - def plan_qpos_to_pose( + def plan_pose( self, goal_pose: Pose, current_qpos: np.ndarray, @@ -675,7 +675,7 @@ def plan_qpos_to_pose( for i in range(len(goal_qpos)): # type: ignore print(goal_qpos[i]) # type: ignore - return self.plan_qpos_to_qpos( + return self.plan_qpos( goal_qpos, # type: ignore current_qpos, time_step=time_step, @@ -708,7 +708,7 @@ def plan_screw( Args: goal_pose: pose of the goal current_qpos: current joint configuration (either full or move_group joints) - qpos_step: size of the random step for RRT + qpos_step: size of the random step time_step: time step for the discretization wrt_world: if True, interpret the target pose with respect to the world frame instead of the base frame From 3c36dae65406aadef9ef72008cbbd191f3dd477a Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 16:27:08 -0700 Subject: [PATCH 26/37] Remove unused imports in planner.py --- mplib/planner.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index cc2ce7a2..67f889fa 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -1,10 +1,8 @@ from __future__ import annotations import os -import xml.etree.ElementTree as ET from collections.abc import Callable from typing import Literal, Optional, Sequence -from xml.dom import minidom import numpy as np import toppra as ta @@ -12,7 +10,7 @@ import toppra.constraint as constraint from mplib.pymp import ArticulatedModel, PlanningWorld, Pose -from mplib.pymp.collision_detection import AllowedCollisionMatrix, WorldCollisionResult +from mplib.pymp.collision_detection import WorldCollisionResult from mplib.pymp.collision_detection.fcl import CollisionGeometry, FCLObject from mplib.pymp.planning import ompl from mplib.urdf_utils import generate_srdf, replace_urdf_package_keyword From 91a5daeb5bbd5d7c4da9d2b977fafb78750c0798 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 17:04:09 -0700 Subject: [PATCH 27/37] Add dummy submodules to get rid of importing from pymp --- mplib/__init__.py | 4 +--- mplib/collision_detection/__init__.py | 3 +++ mplib/collision_detection/fcl.py | 3 +++ mplib/kinematics/__init__.py | 3 +++ mplib/kinematics/kdl.py | 3 +++ mplib/kinematics/pinocchio.py | 3 +++ mplib/planning/__init__.py | 3 +++ mplib/planning/ompl.py | 3 +++ 8 files changed, 22 insertions(+), 3 deletions(-) create mode 100644 mplib/collision_detection/__init__.py create mode 100644 mplib/collision_detection/fcl.py create mode 100644 mplib/kinematics/__init__.py create mode 100644 mplib/kinematics/kdl.py create mode 100644 mplib/kinematics/pinocchio.py create mode 100644 mplib/planning/__init__.py create mode 100644 mplib/planning/ompl.py diff --git a/mplib/__init__.py b/mplib/__init__.py index c114b830..5dc0ddc9 100644 --- a/mplib/__init__.py +++ b/mplib/__init__.py @@ -3,11 +3,9 @@ from mplib.planner import Planner from mplib.pymp import ( ArticulatedModel, + AttachedBody, PlanningWorld, Pose, - collision_detection, - kinematics, - planning, set_global_seed, ) diff --git a/mplib/collision_detection/__init__.py b/mplib/collision_detection/__init__.py new file mode 100644 index 00000000..c7194833 --- /dev/null +++ b/mplib/collision_detection/__init__.py @@ -0,0 +1,3 @@ +from ..pymp.collision_detection import * + +__all__ = [v for v in dir() if not v.startswith("_")] # type: ignore diff --git a/mplib/collision_detection/fcl.py b/mplib/collision_detection/fcl.py new file mode 100644 index 00000000..63d05d34 --- /dev/null +++ b/mplib/collision_detection/fcl.py @@ -0,0 +1,3 @@ +from ..pymp.collision_detection.fcl import * + +__all__ = [v for v in dir() if not v.startswith("_")] # type: ignore diff --git a/mplib/kinematics/__init__.py b/mplib/kinematics/__init__.py new file mode 100644 index 00000000..5a13c291 --- /dev/null +++ b/mplib/kinematics/__init__.py @@ -0,0 +1,3 @@ +from ..pymp.kinematics import * + +__all__ = [v for v in dir() if not v.startswith("_")] # type: ignore diff --git a/mplib/kinematics/kdl.py b/mplib/kinematics/kdl.py new file mode 100644 index 00000000..d5e72cd4 --- /dev/null +++ b/mplib/kinematics/kdl.py @@ -0,0 +1,3 @@ +from ..pymp.kinematics.kdl import * + +__all__ = [v for v in dir() if not v.startswith("_")] # type: ignore diff --git a/mplib/kinematics/pinocchio.py b/mplib/kinematics/pinocchio.py new file mode 100644 index 00000000..149ad367 --- /dev/null +++ b/mplib/kinematics/pinocchio.py @@ -0,0 +1,3 @@ +from ..pymp.kinematics.pinocchio import * + +__all__ = [v for v in dir() if not v.startswith("_")] # type: ignore diff --git a/mplib/planning/__init__.py b/mplib/planning/__init__.py new file mode 100644 index 00000000..092825a7 --- /dev/null +++ b/mplib/planning/__init__.py @@ -0,0 +1,3 @@ +from ..pymp.planning import * + +__all__ = [v for v in dir() if not v.startswith("_")] # type: ignore diff --git a/mplib/planning/ompl.py b/mplib/planning/ompl.py new file mode 100644 index 00000000..f831d499 --- /dev/null +++ b/mplib/planning/ompl.py @@ -0,0 +1,3 @@ +from ..pymp.planning.ompl import * + +__all__ = [v for v in dir() if not v.startswith("_")] # type: ignore From d070b7a9ad3c80897ea109de8680e0f3ddd17fde Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 17:35:12 -0700 Subject: [PATCH 28/37] Remove pymp from planner/urdf_utils/sapien_utils and docs --- docs/source/reference/Planner.rst | 2 +- docs/source/reference/PlanningWorld.rst | 2 +- docs/source/reference/collision_detection/fcl.rst | 2 +- .../source/reference/collision_detection/index.rst | 2 +- docs/source/reference/core/ArticulatedModel.rst | 2 +- docs/source/reference/core/AttachedBody.rst | 2 +- docs/source/reference/index.rst | 1 + docs/source/reference/kinematics/kdl.rst | 2 +- docs/source/reference/kinematics/pinocchio.rst | 2 +- docs/source/reference/planning/ompl.rst | 2 +- docs/source/reference/utils/pose.rst | 2 +- docs/source/reference/utils/random.rst | 2 +- mplib/__init__.py | 4 ++-- mplib/planner.py | 14 +++++++------- mplib/sapien_utils/conversion.py | 14 +++++++------- mplib/urdf_utils.py | 4 ++-- 16 files changed, 30 insertions(+), 29 deletions(-) diff --git a/docs/source/reference/Planner.rst b/docs/source/reference/Planner.rst index 193619a3..29f3a000 100644 --- a/docs/source/reference/Planner.rst +++ b/docs/source/reference/Planner.rst @@ -1,7 +1,7 @@ ``Planner`` ------------- -.. automodule:: mplib.planner +.. autoclass:: mplib.Planner :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/PlanningWorld.rst b/docs/source/reference/PlanningWorld.rst index 28499bc3..d5775f01 100644 --- a/docs/source/reference/PlanningWorld.rst +++ b/docs/source/reference/PlanningWorld.rst @@ -1,7 +1,7 @@ ``PlanningWorld`` ------------------- -.. autoclass:: mplib.pymp.PlanningWorld +.. autoclass:: mplib.PlanningWorld :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/collision_detection/fcl.rst b/docs/source/reference/collision_detection/fcl.rst index 66d01fbe..c26a6759 100644 --- a/docs/source/reference/collision_detection/fcl.rst +++ b/docs/source/reference/collision_detection/fcl.rst @@ -1,7 +1,7 @@ fcl ---------- -.. automodule:: mplib.pymp.collision_detection.fcl +.. automodule:: mplib.collision_detection.fcl :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/collision_detection/index.rst b/docs/source/reference/collision_detection/index.rst index 344eabb2..7f8cdd33 100644 --- a/docs/source/reference/collision_detection/index.rst +++ b/docs/source/reference/collision_detection/index.rst @@ -1,7 +1,7 @@ collision_detection ------------------------------------------ -.. automodule:: mplib.pymp.collision_detection +.. automodule:: mplib.collision_detection :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/core/ArticulatedModel.rst b/docs/source/reference/core/ArticulatedModel.rst index fd677c79..c2cf77e2 100644 --- a/docs/source/reference/core/ArticulatedModel.rst +++ b/docs/source/reference/core/ArticulatedModel.rst @@ -1,7 +1,7 @@ ``ArticulatedModel`` ------------------------- -.. autoclass:: mplib.pymp.ArticulatedModel +.. autoclass:: mplib.ArticulatedModel :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/core/AttachedBody.rst b/docs/source/reference/core/AttachedBody.rst index 07d012fe..9ef472ee 100644 --- a/docs/source/reference/core/AttachedBody.rst +++ b/docs/source/reference/core/AttachedBody.rst @@ -1,7 +1,7 @@ ``AttachedBody`` ------------------------- -.. autoclass:: mplib.pymp.AttachedBody +.. autoclass:: mplib.AttachedBody :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/index.rst b/docs/source/reference/index.rst index 77e636a8..ae18bcd8 100644 --- a/docs/source/reference/index.rst +++ b/docs/source/reference/index.rst @@ -10,6 +10,7 @@ API Reference core/AttachedBody utils/pose utils/random + urdf_utils .. toctree:: :maxdepth: 3 diff --git a/docs/source/reference/kinematics/kdl.rst b/docs/source/reference/kinematics/kdl.rst index a1b0c8cd..9b15663c 100644 --- a/docs/source/reference/kinematics/kdl.rst +++ b/docs/source/reference/kinematics/kdl.rst @@ -1,7 +1,7 @@ kdl ------- -.. automodule:: mplib.pymp.kinematics.kdl +.. automodule:: mplib.kinematics.kdl :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/kinematics/pinocchio.rst b/docs/source/reference/kinematics/pinocchio.rst index 46c75311..7c01304a 100644 --- a/docs/source/reference/kinematics/pinocchio.rst +++ b/docs/source/reference/kinematics/pinocchio.rst @@ -1,7 +1,7 @@ pinocchio -------------- -.. automodule:: mplib.pymp.kinematics.pinocchio +.. automodule:: mplib.kinematics.pinocchio :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/planning/ompl.rst b/docs/source/reference/planning/ompl.rst index 00de58c6..b659b04c 100644 --- a/docs/source/reference/planning/ompl.rst +++ b/docs/source/reference/planning/ompl.rst @@ -1,7 +1,7 @@ ompl ------------ -.. automodule:: mplib.pymp.planning.ompl +.. automodule:: mplib.planning.ompl :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/utils/pose.rst b/docs/source/reference/utils/pose.rst index 81fda087..58a8383c 100644 --- a/docs/source/reference/utils/pose.rst +++ b/docs/source/reference/utils/pose.rst @@ -1,7 +1,7 @@ ``Pose`` ------------------------- -.. autoclass:: mplib.pymp.Pose +.. autoclass:: mplib.Pose :members: :undoc-members: :show-inheritance: diff --git a/docs/source/reference/utils/random.rst b/docs/source/reference/utils/random.rst index 0fb90924..fadcf738 100644 --- a/docs/source/reference/utils/random.rst +++ b/docs/source/reference/utils/random.rst @@ -1,4 +1,4 @@ ``set_global_seed`` ------------------------- -.. autofunction:: mplib.pymp.set_global_seed +.. autofunction:: mplib.set_global_seed diff --git a/mplib/__init__.py b/mplib/__init__.py index 5dc0ddc9..47a2bf93 100644 --- a/mplib/__init__.py +++ b/mplib/__init__.py @@ -1,7 +1,7 @@ from importlib.metadata import version -from mplib.planner import Planner -from mplib.pymp import ( +from .planner import Planner +from .pymp import ( ArticulatedModel, AttachedBody, PlanningWorld, diff --git a/mplib/planner.py b/mplib/planner.py index 67f889fa..8f7733da 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -9,11 +9,11 @@ import toppra.algorithm as algo import toppra.constraint as constraint -from mplib.pymp import ArticulatedModel, PlanningWorld, Pose -from mplib.pymp.collision_detection import WorldCollisionResult -from mplib.pymp.collision_detection.fcl import CollisionGeometry, FCLObject -from mplib.pymp.planning import ompl -from mplib.urdf_utils import generate_srdf, replace_urdf_package_keyword +from .collision_detection import WorldCollisionResult +from .collision_detection.fcl import CollisionGeometry, FCLObject +from .planning.ompl import FixedJoint, OMPLPlanner +from .pymp import ArticulatedModel, PlanningWorld, Pose +from .urdf_utils import generate_srdf, replace_urdf_package_keyword class Planner: @@ -130,7 +130,7 @@ def __init__( self.planning_world = PlanningWorld([self.robot], objects) self.acm = self.planning_world.get_allowed_collision_matrix() - self.planner = ompl.OMPLPlanner(world=self.planning_world) + self.planner = OMPLPlanner(world=self.planning_world) def wrap_joint_limit(self, qpos: np.ndarray) -> bool: """ @@ -578,7 +578,7 @@ def plan_qpos( fixed_joints = set() for joint_idx in fixed_joint_indices: - fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx])) + fixed_joints.add(FixedJoint(0, joint_idx, current_qpos[joint_idx])) assert len(current_qpos[move_joint_idx]) == len(goal_qpos_[0]) status, path = self.planner.plan( diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 457bd587..54ab8415 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -18,15 +18,14 @@ ) from transforms3d.euler import euler2quat -from ..planner import Planner -from ..pymp import ArticulatedModel, PlanningWorld, Pose -from ..pymp.collision_detection import ( +from .. import ArticulatedModel, PlanningWorld, Pose +from ..collision_detection import ( AllowedCollision, AllowedCollisionMatrix, WorldCollisionResult, WorldDistanceResult, ) -from ..pymp.collision_detection.fcl import ( +from ..collision_detection.fcl import ( Box, BVHModel, Capsule, @@ -40,7 +39,8 @@ collide, distance, ) -from ..pymp.planning import ompl +from ..planner import Planner +from ..planning.ompl import OMPLPlanner from .srdf_exporter import export_srdf from .urdf_exporter import export_kinematic_chain_urdf @@ -69,7 +69,7 @@ def __init__( planned_articulations: list[PhysxArticulation] = [], # noqa: B006 ): """ - Creates an mplib.pymp.planning_world.PlanningWorld from a sapien.Scene. + Creates an mplib.PlanningWorld from a sapien.Scene. :param planned_articulations: list of planned articulations. """ @@ -419,7 +419,7 @@ def __init__( t.startswith("JointModelR") for t in self.joint_types ] & (self.joint_limits[:, 1] - self.joint_limits[:, 0] > 2 * np.pi) - self.planner = ompl.OMPLPlanner(world=self.planning_world) + self.planner = OMPLPlanner(world=self.planning_world) def update_from_simulation(self, *, update_attached_object: bool = True) -> None: """ diff --git a/mplib/urdf_utils.py b/mplib/urdf_utils.py index 3e331807..90312ace 100644 --- a/mplib/urdf_utils.py +++ b/mplib/urdf_utils.py @@ -6,8 +6,8 @@ import numpy as np -from mplib.pymp import ArticulatedModel -from mplib.pymp.collision_detection import AllowedCollisionMatrix +from .collision_detection import AllowedCollisionMatrix +from .pymp import ArticulatedModel def compute_default_collisions( From 399e94b1b9d248e21acdc5af40d42771f9facc84 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 23:42:53 -0700 Subject: [PATCH 29/37] Fix docstring and docs --- docs/source/tutorials/collision_avoidance.rst | 10 +-------- docs/source/tutorials/plan_a_path.rst | 22 +++++++++---------- mplib/planner.py | 6 +++-- 3 files changed, 16 insertions(+), 22 deletions(-) diff --git a/docs/source/tutorials/collision_avoidance.rst b/docs/source/tutorials/collision_avoidance.rst index 7b41f0fd..6e1ec307 100644 --- a/docs/source/tutorials/collision_avoidance.rst +++ b/docs/source/tutorials/collision_avoidance.rst @@ -34,14 +34,6 @@ One way to model the environment and avoid collision is through point clouds. Th ``planner.update_point_cloud()`` takes two arguments. The first one is a NumPy array of shape :math:`(n \times 3)`, which describes the coordinates of the points. **The coordinates should be represented in the world frame**. The second (optional) argument is ``resolution``, which describes the resolution of each point. This can be used to create a buffer around the collision object. -After adding the point cloud, we can avoid collisions between the robot and the point cloud by setting ``use_point_cloud`` to be True. Both ``planner.plan_qpos_to_pose()`` and ``planner.plan_screw()`` support this flag: - -.. literalinclude:: ../../../mplib/examples/demo_setup.py - :dedent: 0 - :start-after: # plan_qpos_to_pose ankor - :end-before: # plan_qpos_to_pose ankor end - :emphasize-lines: 5 - You don't need to provide the point cloud for each ``planner.plan()`` or ``planner.plan_screw()`` call. You can use ``planner.update_point_cloud()`` to update the point cloud once it's changed. .. note:: @@ -62,7 +54,7 @@ As shown in the above figure (middle one), after adding the point cloud of the b - ``pose``: a list with seven elements indicates the relative pose from the box to the attached link. The first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part. - ``link_id = -1``: optional, an integer indicates the id of the link that the box is attached to. The link id is determined by the ``user_link_names`` (during Configuration), and starts from 0. The default value -1 indicates the ``move_group`` link. -After adding the attached box, we can avoid collisions between the attached box and the point cloud by setting both ``use_point_cloud`` and ``use_attach`` to be True. Both ``planner.plan_qpos_to_pose()`` and ``planner.plan_screw()`` support the flags. +After adding the attached box, we can avoid collisions between the attached box and the point cloud by setting both ``use_point_cloud`` and ``use_attach`` to be True. Both ``planner.plan_pose()`` and ``planner.plan_screw()`` support the flags. You can use ``planner.update_attached_box()`` again to update the box once it's changed. diff --git a/docs/source/tutorials/plan_a_path.rst b/docs/source/tutorials/plan_a_path.rst index 4568576d..c002566e 100644 --- a/docs/source/tutorials/plan_a_path.rst +++ b/docs/source/tutorials/plan_a_path.rst @@ -19,18 +19,18 @@ In this tutorial, we will talk about how to plan paths for the agent. As shown i Plan with sampling-based algorithms -------------------------------------- -``mplib`` supports state-of-the-art sampling-based motion planning algorithms by leveraging `OMPL `_. You can call ``planner.plan_qpos_to_pose()`` to plan a path for moving the ``move_group`` link to a target pose: +``mplib`` supports state-of-the-art sampling-based motion planning algorithms by leveraging `OMPL `_. You can call ``planner.plan_pose()`` to plan a path for moving the ``move_group`` link to a target pose: .. literalinclude:: ../../../mplib/examples/demo_setup.py :dedent: 0 - :start-after: # plan_qpos_to_pose ankor - :end-before: # plan_qpos_to_pose ankor end + :start-after: # plan_pose ankor + :end-before: # plan_pose ankor end -Specifically, ``planner.plan_qpos_to_pose()`` takes two required arguments as input. The first one is the target pose of the ``move_group`` link. It's a 7-dim list, where the first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part. **Note that the pose is relative to the world frame**. Normally, the base link of the robot is the world frame unless you have called ``set_base_pose(new_pose)`` in on the planner. You can also temporarily plan w.r.t. the robot base by passing in ``wrt_world=False``. +Specifically, ``planner.plan_pose()`` takes two required arguments as input. The first one is the target pose of the ``move_group`` link. It's a 7-dim list, where the first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part. **Note that the pose is relative to the world frame**. Normally, the base link of the robot is the world frame unless you have called ``set_base_pose(new_pose)`` in on the planner. You can also temporarily plan w.r.t. the robot base by passing in ``wrt_world=False``. -The second argument is the current joint positions of all the active joints (not just all the active joints in the movegroup). The ``planner.plan_qpos_to_pose()`` function first solves the inverse kinematics to get the joint positions for the target pose. It then calls the RRTConnect algorithm to find a path in the joint space. Finally, it simplifies the path and parameterizes the path to generate time, velocity, and acceleration information. +The second argument is the current joint positions of all the active joints (not just all the active joints in the movegroup). The ``planner.plan_pose()`` function first solves the inverse kinematics to get the joint positions for the target pose. It then calls the RRTConnect algorithm to find a path in the joint space. Finally, it simplifies the path and parameterizes the path to generate time, velocity, and acceleration information. -``planner.plan_qpos_to_pose()`` returns a dict which includes: +``planner.plan_pose()`` returns a dict which includes: - ``status``: a string indicates the status: @@ -44,14 +44,14 @@ The second argument is the current joint positions of all the active joints (not - ``acceleration``: a NumPy array of shape :math:`(n \times m)` describing the joint accelerations of the waypoints. -``planner.plan_qpos_to_pose()`` also takes other optional arguments with default values: +``planner.plan_pose()`` also takes other optional arguments with default values: -.. automethod:: mplib.planner.Planner.plan_qpos_to_pose +.. automethod:: mplib.Planner.plan_pose :no-index: Follow a path -------------------------------------- -``plan_qpos_to_pose()`` outputs a time-parameterized path, and we need to drive the robot to follow the path. In this demo, we use ``sapien`` to simulate and drive the robot. +``plan_pose()`` outputs a time-parameterized path, and we need to drive the robot to follow the path. In this demo, we use ``sapien`` to simulate and drive the robot. .. literalinclude:: ../../../mplib/examples/demo_setup.py :dedent: 0 @@ -72,14 +72,14 @@ Compared to the sampling-based algorithms, planning with screw motion has the fo - `straighter` path: there is no guarantee for sampling-based algorithms to generate `straight` paths even it's a simple lifting task since it connects states in the joint space. In contrast, the returned path by the exponential coordinates and the Jacobian matrix can sometimes be more reasonable. See the above figures for comparison. -You can call ``planner.plan_screw()`` to plan a path with screw motion. Similar to ``planner.plan_qpos_to_pose()``, it also takes two required arguments: target pose and current joint positions, and returns a dict containing the same set of elements. +You can call ``planner.plan_screw()`` to plan a path with screw motion. Similar to ``planner.plan_pose()``, it also takes two required arguments: target pose and current joint positions, and returns a dict containing the same set of elements. .. literalinclude:: ../../../mplib/planner.py :dedent: 0 :start-after: # plan_screw ankor :end-before: # plan_screw ankor end -However, planning with screw motion only succeeds when there is no collision during the planning since it can not detour or replan. We thus recommend use ``planner.plan_screw()`` for some simple tasks or combined with ``planner.plan_qpos_to_pose()``. As shown in the code, we first try ``planner.plan_screw()``, if it fails (e.g., collision during the planning), we then turn to the sampling-based algorithms. Other arguments are the same with ``planner.plan_qpos_to_pose()``. +However, planning with screw motion only succeeds when there is no collision during the planning since it can not detour or replan. We thus recommend use ``planner.plan_screw()`` for some simple tasks or combined with ``planner.plan_pose()``. As shown in the code, we first try ``planner.plan_screw()``, if it fails (e.g., collision during the planning), we then turn to the sampling-based algorithms. Other arguments are the same with ``planner.plan_pose()``. Move the boxes diff --git a/mplib/planner.py b/mplib/planner.py index 8f7733da..c1cb13fb 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -255,12 +255,14 @@ def IK( :param mask: qpos mask to disable IK sampling, (ndof,) bool np.ndarray. :param n_init_qpos: number of random initial configurations to sample. :param threshold: distance threshold for marking sampled IK as success. - distance is position error norm + quaternion error norm. + distance is position error norm + quaternion error norm. :param return_closest: whether to return the qpos that is closest to start_qpos, - considering equivalent joint values. + considering equivalent joint values. :param verbose: whether to print collision info if any collision exists. :return: (status, q_goals) + status: IK status, "Success" if succeeded. + q_goals: list of sampled IK qpos, (ndof,) np.floating np.ndarray. IK is successful if q_goals is not None. If return_closest, q_goals is np.ndarray if successful From d0ab37f3e3e8d81280055cc7445902c7cd1dc96d Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 23:54:06 -0700 Subject: [PATCH 30/37] Fix examples and tests --- mplib/examples/constrained_planning.py | 4 ++-- mplib/examples/demo_setup.py | 10 ++++------ mplib/examples/detect_collision.py | 2 +- mplib/examples/moving_robot.py | 2 +- mplib/examples/two_stage_motion.py | 6 +++--- tests/test_articulation.py | 13 +++++-------- tests/test_fcl_model.py | 4 ++-- tests/test_pinocchio.py | 2 +- tests/test_planner.py | 22 ++++++++-------------- 9 files changed, 27 insertions(+), 38 deletions(-) diff --git a/mplib/examples/constrained_planning.py b/mplib/examples/constrained_planning.py index 293c5fcb..2a03d0f4 100644 --- a/mplib/examples/constrained_planning.py +++ b/mplib/examples/constrained_planning.py @@ -108,7 +108,7 @@ def demo(self): "with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis" ) for pose in poses: - result = self.planner.plan_qpos_to_pose( + result = self.planner.plan_pose( pose, self.robot.get_qpos(), time_step=1 / 250, @@ -127,7 +127,7 @@ def demo(self): " almost upside down" ) for pose in poses: - result = self.planner.plan_qpos_to_pose( + result = self.planner.plan_pose( pose, self.robot.get_qpos(), time_step=1 / 250, diff --git a/mplib/examples/demo_setup.py b/mplib/examples/demo_setup.py index b52427b0..cd178ac0 100644 --- a/mplib/examples/demo_setup.py +++ b/mplib/examples/demo_setup.py @@ -170,12 +170,10 @@ def move_to_pose_with_RRTConnect(self, pose: Pose): """ # result is a dictionary with keys 'status', 'time', 'position', 'velocity', # 'acceleration', 'duration' - # plan_qpos_to_pose ankor - print("plan_qpos_to_pose") - result = self.planner.plan_qpos_to_pose( - pose, self.robot.get_qpos(), time_step=1 / 250 - ) - # plan_qpos_to_pose ankor end + # 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": print(result["status"]) return -1 diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py index 7c04b788..808938bb 100644 --- a/mplib/examples/detect_collision.py +++ b/mplib/examples/detect_collision.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 +from mplib.collision_detection import fcl from mplib.examples.demo_setup import DemoSetup -from mplib.pymp.collision_detection import fcl class DetectCollisionDemo(DemoSetup): diff --git a/mplib/examples/moving_robot.py b/mplib/examples/moving_robot.py index 7afcd27a..4d982b46 100644 --- a/mplib/examples/moving_robot.py +++ b/mplib/examples/moving_robot.py @@ -95,7 +95,7 @@ def demo(self): pose[1] -= 1 # plan a path to the first pose - result = self.planner.plan_qpos_to_pose( + result = self.planner.plan_pose( poses[0], self.planner.robot.get_qpos(), time_step=1 / 250, wrt_world=False ) if result["status"] != "Success": diff --git a/mplib/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py index 21f94f4a..badfdc55 100644 --- a/mplib/examples/two_stage_motion.py +++ b/mplib/examples/two_stage_motion.py @@ -3,8 +3,8 @@ import numpy as np import sapien.core as sapien +from mplib.collision_detection import fcl from mplib.examples.demo_setup import DemoSetup -from mplib.pymp.collision_detection import fcl class PlanningDemo(DemoSetup): @@ -114,7 +114,7 @@ def plan_without_base(self, pose, has_attach=False): print("IK failed") sys.exit(1) # now fix base and plan a path to the goal - result = self.planner.plan_qpos_to_qpos( + result = self.planner.plan_qpos( goal_qposes, self.robot.get_qpos(), time_step=1 / 250, @@ -134,7 +134,7 @@ def move_in_two_stage(self, pose, has_attach=False): print("IK failed") sys.exit(1) # now fix arm joints and plan a path to the goal - result = self.planner.plan_qpos_to_qpos( + result = self.planner.plan_qpos( goal_qposes, self.robot.get_qpos(), time_step=1 / 250, diff --git a/tests/test_articulation.py b/tests/test_articulation.py index ba4ea728..49430346 100644 --- a/tests/test_articulation.py +++ b/tests/test_articulation.py @@ -2,12 +2,11 @@ import unittest import numpy as np -import trimesh from transforms3d.quaternions import mat2quat, quat2mat -import mplib -from mplib.pymp import Pose -from mplib.pymp.collision_detection import fcl +from mplib import ArticulatedModel, Pose +from mplib.collision_detection import fcl +from mplib.kinematics.pinocchio import PinocchioModel FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) @@ -46,7 +45,7 @@ class TestArticulation(unittest.TestCase): def setUp(self): - self.robot = mplib.ArticulatedModel( + self.robot = ArticulatedModel( PANDA_SPEC["urdf"], PANDA_SPEC["srdf"], link_names=[], @@ -109,9 +108,7 @@ def test_get_move_group_qpos_dim(self): def test_get_pinocchio_model(self): pinocchio_model = self.robot.get_pinocchio_model() - self.assertIsInstance( - pinocchio_model, mplib.pymp.kinematics.pinocchio.PinocchioModel - ) + self.assertIsInstance(pinocchio_model, PinocchioModel) def test_get_qpos(self): qpos = self.robot.get_qpos() diff --git a/tests/test_fcl_model.py b/tests/test_fcl_model.py index 82746229..6404ff44 100644 --- a/tests/test_fcl_model.py +++ b/tests/test_fcl_model.py @@ -4,8 +4,8 @@ import numpy as np from transforms3d.quaternions import mat2quat -from mplib.pymp.collision_detection.fcl import FCLModel -from mplib.pymp.kinematics.pinocchio import PinocchioModel +from mplib.collision_detection.fcl import FCLModel +from mplib.kinematics.pinocchio import PinocchioModel FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) diff --git a/tests/test_pinocchio.py b/tests/test_pinocchio.py index 69a3331f..e42c4857 100644 --- a/tests/test_pinocchio.py +++ b/tests/test_pinocchio.py @@ -4,7 +4,7 @@ import numpy as np from transforms3d.quaternions import qinverse, qmult, quat2axangle -from mplib.pymp.kinematics.pinocchio import PinocchioModel +from mplib.kinematics.pinocchio import PinocchioModel FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) diff --git a/tests/test_planner.py b/tests/test_planner.py index abf07e62..3bfc3378 100644 --- a/tests/test_planner.py +++ b/tests/test_planner.py @@ -6,10 +6,8 @@ import trimesh from transforms3d.quaternions import mat2quat, quat2mat -import mplib -from mplib import Planner -from mplib.pymp import Pose -from mplib.pymp.collision_detection import fcl +from mplib import Planner, Pose +from mplib.collision_detection import fcl FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) @@ -56,9 +54,7 @@ def sample_pose(self): return pose def test_planning_to_pose(self): - result_sampling = self.planner.plan_qpos_to_pose( - self.target_pose, self.init_qpos - ) + result_sampling = self.planner.plan_pose(self.target_pose, self.init_qpos) self.assertEqual(result_sampling["status"], "Success") last_qpos = result_sampling["position"][-1] self.planner.robot.set_qpos(last_qpos) @@ -71,9 +67,7 @@ def test_planning_to_qpos(self): expected_least_num_success = 5 for _ in range(10): target_qpos = self.sample_qpos()[:7] - result_sampling = self.planner.plan_qpos_to_qpos( - [target_qpos], self.init_qpos - ) + result_sampling = self.planner.plan_qpos([target_qpos], self.init_qpos) # might not always be a valid pose, so check if the planner succeeded if result_sampling["status"] == "Success": num_success += 1 @@ -198,7 +192,7 @@ def test_set_base_pose(self): base_pose = self.sample_pose() self.planner.set_base_pose(base_pose) - result_sampling = self.planner.plan_qpos_to_pose( + result_sampling = self.planner.plan_pose( base_pose * self.target_pose, self.init_qpos ) self.assertEqual(result_sampling["status"], "Success") @@ -218,7 +212,7 @@ def test_set_base_pose(self): self.get_end_effector_pose().distance(self.target_pose), 0, places=1 ) - result_sampling = self.planner.plan_qpos_to_pose( + result_sampling = self.planner.plan_pose( self.target_pose, self.init_qpos, wrt_world=False ) self.assertEqual(result_sampling["status"], "Success") @@ -265,7 +259,7 @@ def test_fixed_joint(self): for _ in range(10): qpos = self.sample_qpos() fixed_joints = np.random.choice(range(7), 2, replace=False) - result = self.planner.plan_qpos_to_qpos( + result = self.planner.plan_qpos( [qpos], self.init_qpos, fixed_joint_indices=fixed_joints ) if result["status"] == "Success": @@ -324,7 +318,7 @@ def test_constrained_planning(self, success_percentage=0.3): total_count = 0 for _ in range(20): self.planner.robot.set_qpos(init_qpos, full=True) - result = self.planner.plan_qpos_to_pose( + result = self.planner.plan_pose( constrained_target_pose, init_qpos, constraint_function=self.make_f(), From 62095c897bc859553e52c80378f19a98f5ce1367 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Mon, 15 Apr 2024 23:55:32 -0700 Subject: [PATCH 31/37] Add urdf_utils/sapien_utils API reference --- docs/compose.yaml | 8 ++++++-- docs/source/reference/index.rst | 1 + docs/source/reference/sapien_utils.rst | 12 ++++++++++++ docs/source/reference/urdf_utils.rst | 7 +++++++ 4 files changed, 26 insertions(+), 2 deletions(-) create mode 100644 docs/source/reference/sapien_utils.rst create mode 100644 docs/source/reference/urdf_utils.rst diff --git a/docs/compose.yaml b/docs/compose.yaml index 24f86371..3d807e6f 100644 --- a/docs/compose.yaml +++ b/docs/compose.yaml @@ -12,8 +12,11 @@ services: command: - >- python3 -m pip install ./wheelhouse/mplib*.whl - && rm -rf ./docs/build - && sphinx-autobuild ./docs/source/ ./docs/build/html/ + && apt update && apt install -y libx11-6 + && python3 -m pip install sapien~=3.0.0.dev + && cd ./docs + && rm -rf ./build + && sphinx-autobuild ./source/ ./build/html/ image: kolinguo/sphinx build: network: host @@ -27,4 +30,5 @@ services: COPY ./requirements.txt /tmp RUN python3 -m pip install -r /tmp/requirements.txt \ && rm -r /tmp/* + RUN python3 -m pip install sphinx-autobuild watchfiles RUN git config --global --add safe.directory /MPlib diff --git a/docs/source/reference/index.rst b/docs/source/reference/index.rst index ae18bcd8..468659c6 100644 --- a/docs/source/reference/index.rst +++ b/docs/source/reference/index.rst @@ -11,6 +11,7 @@ API Reference utils/pose utils/random urdf_utils + sapien_utils .. toctree:: :maxdepth: 3 diff --git a/docs/source/reference/sapien_utils.rst b/docs/source/reference/sapien_utils.rst new file mode 100644 index 00000000..80d318c0 --- /dev/null +++ b/docs/source/reference/sapien_utils.rst @@ -0,0 +1,12 @@ +``sapien_utils`` +------------------------------------------ + +.. autoclass:: mplib.sapien_utils.SapienPlanningWorld + :members: + :undoc-members: + :show-inheritance: + +.. autoclass:: mplib.sapien_utils.SapienPlanner + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/source/reference/urdf_utils.rst b/docs/source/reference/urdf_utils.rst new file mode 100644 index 00000000..de7f7250 --- /dev/null +++ b/docs/source/reference/urdf_utils.rst @@ -0,0 +1,7 @@ +``urdf_utils`` +------------------- + +.. automodule:: mplib.urdf_utils + :members: + :undoc-members: + :show-inheritance: From 3927715a1aa9617ec364849223c76c6312f3c490 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Tue, 16 Apr 2024 11:57:11 -0700 Subject: [PATCH 32/37] Limit maximum method/function signature length --- docs/source/conf.py | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/source/conf.py b/docs/source/conf.py index eb5abeaf..d068bd2f 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -35,6 +35,7 @@ templates_path = ["_templates"] exclude_patterns = [] +maximum_signature_line_length = 88 # limit maximum method/function signature length # -- Options for HTML output ------------------------------------------------- From c585a9c29a1039e1c7aa24373ac54ec5e28e7059 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Tue, 16 Apr 2024 12:10:21 -0700 Subject: [PATCH 33/37] Add autodoc_default_options to conf.py --- docs/source/conf.py | 9 +++++++++ docs/source/reference/Planner.rst | 3 --- docs/source/reference/PlanningWorld.rst | 3 --- docs/source/reference/collision_detection/fcl.rst | 3 --- docs/source/reference/collision_detection/index.rst | 3 --- docs/source/reference/core/ArticulatedModel.rst | 3 --- docs/source/reference/core/AttachedBody.rst | 3 --- docs/source/reference/kinematics/kdl.rst | 3 --- docs/source/reference/kinematics/pinocchio.rst | 3 --- docs/source/reference/planning/ompl.rst | 3 --- docs/source/reference/sapien_utils.rst | 6 ------ docs/source/reference/urdf_utils.rst | 3 --- docs/source/reference/utils/pose.rst | 3 --- 13 files changed, 9 insertions(+), 39 deletions(-) diff --git a/docs/source/conf.py b/docs/source/conf.py index d068bd2f..499f2c57 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -35,7 +35,16 @@ templates_path = ["_templates"] exclude_patterns = [] + maximum_signature_line_length = 88 # limit maximum method/function signature length +autodoc_default_options = { + "members": True, + "member-order": "bysource", + "inherited-members": True, + "show-inheritance": True, + "undoc-members": True, + "special-members": "__init__", +} # -- Options for HTML output ------------------------------------------------- diff --git a/docs/source/reference/Planner.rst b/docs/source/reference/Planner.rst index 29f3a000..ed1f8acf 100644 --- a/docs/source/reference/Planner.rst +++ b/docs/source/reference/Planner.rst @@ -2,6 +2,3 @@ ------------- .. autoclass:: mplib.Planner - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/PlanningWorld.rst b/docs/source/reference/PlanningWorld.rst index d5775f01..5f7b389c 100644 --- a/docs/source/reference/PlanningWorld.rst +++ b/docs/source/reference/PlanningWorld.rst @@ -2,6 +2,3 @@ ------------------- .. autoclass:: mplib.PlanningWorld - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/collision_detection/fcl.rst b/docs/source/reference/collision_detection/fcl.rst index c26a6759..ed683b49 100644 --- a/docs/source/reference/collision_detection/fcl.rst +++ b/docs/source/reference/collision_detection/fcl.rst @@ -2,6 +2,3 @@ fcl ---------- .. automodule:: mplib.collision_detection.fcl - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/collision_detection/index.rst b/docs/source/reference/collision_detection/index.rst index 7f8cdd33..5f7dfa0e 100644 --- a/docs/source/reference/collision_detection/index.rst +++ b/docs/source/reference/collision_detection/index.rst @@ -2,9 +2,6 @@ collision_detection ------------------------------------------ .. automodule:: mplib.collision_detection - :members: - :undoc-members: - :show-inheritance: .. toctree:: :maxdepth: 2 diff --git a/docs/source/reference/core/ArticulatedModel.rst b/docs/source/reference/core/ArticulatedModel.rst index c2cf77e2..f1e1708c 100644 --- a/docs/source/reference/core/ArticulatedModel.rst +++ b/docs/source/reference/core/ArticulatedModel.rst @@ -2,6 +2,3 @@ ------------------------- .. autoclass:: mplib.ArticulatedModel - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/core/AttachedBody.rst b/docs/source/reference/core/AttachedBody.rst index 9ef472ee..1f01ac9c 100644 --- a/docs/source/reference/core/AttachedBody.rst +++ b/docs/source/reference/core/AttachedBody.rst @@ -2,6 +2,3 @@ ------------------------- .. autoclass:: mplib.AttachedBody - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/kinematics/kdl.rst b/docs/source/reference/kinematics/kdl.rst index 9b15663c..94814726 100644 --- a/docs/source/reference/kinematics/kdl.rst +++ b/docs/source/reference/kinematics/kdl.rst @@ -2,6 +2,3 @@ kdl ------- .. automodule:: mplib.kinematics.kdl - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/kinematics/pinocchio.rst b/docs/source/reference/kinematics/pinocchio.rst index 7c01304a..c1f7844a 100644 --- a/docs/source/reference/kinematics/pinocchio.rst +++ b/docs/source/reference/kinematics/pinocchio.rst @@ -2,6 +2,3 @@ pinocchio -------------- .. automodule:: mplib.kinematics.pinocchio - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/planning/ompl.rst b/docs/source/reference/planning/ompl.rst index b659b04c..2a959c0a 100644 --- a/docs/source/reference/planning/ompl.rst +++ b/docs/source/reference/planning/ompl.rst @@ -2,6 +2,3 @@ ompl ------------ .. automodule:: mplib.planning.ompl - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/sapien_utils.rst b/docs/source/reference/sapien_utils.rst index 80d318c0..6bfcb341 100644 --- a/docs/source/reference/sapien_utils.rst +++ b/docs/source/reference/sapien_utils.rst @@ -2,11 +2,5 @@ ------------------------------------------ .. autoclass:: mplib.sapien_utils.SapienPlanningWorld - :members: - :undoc-members: - :show-inheritance: .. autoclass:: mplib.sapien_utils.SapienPlanner - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/urdf_utils.rst b/docs/source/reference/urdf_utils.rst index de7f7250..e9911e92 100644 --- a/docs/source/reference/urdf_utils.rst +++ b/docs/source/reference/urdf_utils.rst @@ -2,6 +2,3 @@ ------------------- .. automodule:: mplib.urdf_utils - :members: - :undoc-members: - :show-inheritance: diff --git a/docs/source/reference/utils/pose.rst b/docs/source/reference/utils/pose.rst index 58a8383c..3aec6631 100644 --- a/docs/source/reference/utils/pose.rst +++ b/docs/source/reference/utils/pose.rst @@ -2,6 +2,3 @@ ------------------------- .. autoclass:: mplib.Pose - :members: - :undoc-members: - :show-inheritance: From 6621039346210d8d0373c9e47563821a197e32a6 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Tue, 16 Apr 2024 13:12:02 -0700 Subject: [PATCH 34/37] Keep generated URDF/SRDF as *_mplib.urdf/srdf --- mplib/planner.py | 20 ++++++++++---------- mplib/urdf_utils.py | 39 ++++++++++----------------------------- 2 files changed, 20 insertions(+), 39 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index c1cb13fb..6969cf13 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -1,7 +1,7 @@ from __future__ import annotations -import os from collections.abc import Callable +from pathlib import Path from typing import Literal, Optional, Sequence import numpy as np @@ -24,10 +24,10 @@ class Planner: # constructor ankor def __init__( self, - urdf: str, + urdf: str | Path, move_group: str, *, - srdf: Optional[str] = None, + srdf: Optional[str | Path] = None, new_package_keyword: str = "", use_convex: bool = False, user_link_names: Sequence[str] = [], @@ -47,7 +47,7 @@ def __init__( :param urdf: Unified Robot Description Format file. :param move_group: target link to move, usually the end-effector. :param srdf: Semantic Robot Description Format file. - :param new_package_keyword: the string to replace 'package://' keyword + :param new_package_keyword: the string to replace ``package://`` keyword :param use_convex: if True, load collision mesh as convex mesh. If mesh is not convex, a ``RuntimeError`` will be raised. :param user_link_names: names of links, the order matters. @@ -61,10 +61,12 @@ def __init__( :param objects: list of FCLObject as non-articulated collision objects :param verbose: if True, print verbose logs for debugging """ - self.urdf, self.srdf = urdf, srdf - if self.srdf is None: - if os.path.exists(srdf := urdf.replace(".urdf", ".srdf")): - print(f"No SRDF file provided but found {self.srdf}") + self.urdf = Path(urdf) + if srdf is None: + if (srdf := self.urdf.with_suffix(".srdf")).is_file() or ( + srdf := self.urdf.with_name(self.urdf.stem + "_mplib.srdf") + ).is_file(): + print(f"No SRDF file provided but found {srdf}") self.srdf = srdf else: self.srdf = generate_srdf(self.urdf, new_package_keyword, verbose=True) @@ -80,8 +82,6 @@ def __init__( convex=use_convex, verbose=verbose, ) - # Remove the temporary URDF file from replace_urdf_package_keyword() - self.urdf.unlink() self.pinocchio_model = self.robot.get_pinocchio_model() self.user_link_names = self.pinocchio_model.get_link_names() self.user_joint_names = self.pinocchio_model.get_joint_names() diff --git a/mplib/urdf_utils.py b/mplib/urdf_utils.py index 90312ace..14d967b4 100644 --- a/mplib/urdf_utils.py +++ b/mplib/urdf_utils.py @@ -1,7 +1,5 @@ -import tempfile import xml.etree.ElementTree as ET from pathlib import Path -from typing import Optional from xml.dom import minidom import numpy as np @@ -115,30 +113,23 @@ def compute_default_collisions( def replace_urdf_package_keyword( urdf_path: str | Path, new_package_keyword: str = "", - *, - save_path: Optional[str | Path] = None, ) -> Path: """ Some ROS URDF files use package:// keyword to refer the package dir. Replace it with the given string (default is empty) :param urdf_path: Path to a Unified Robot Description Format file. - :param new_package_keyword: the string to replace 'package://' keyword - :param save_path: Path to save the modified URDF file. - If ``None``, create a temporary file. + :param new_package_keyword: the string to replace ``package://`` keyword :return: Path to the modified URDF file """ - if save_path is None: - _, save_path = tempfile.mkstemp(suffix=".urdf") - save_path = Path(save_path) - if not (save_dir := save_path.parent).is_dir(): - save_dir.mkdir(parents=True) - - with Path(urdf_path).open("r") as in_f: + urdf_path = Path(urdf_path) + with urdf_path.open("r") as in_f: if "package://" in (content := in_f.read()): - with save_path.open("w") as out_f: + # Create a new URDF file + urdf_path = urdf_path.with_name(urdf_path.stem + "_mplib.urdf") + with urdf_path.open("w") as out_f: out_f.write(content.replace("package://", new_package_keyword)) - return save_path + return urdf_path def generate_srdf( @@ -146,17 +137,14 @@ def generate_srdf( new_package_keyword: str = "", *, num_samples=100000, - save_path: Optional[str | Path] = None, verbose=False, ) -> Path: """ Generate SRDF from URDF similar to MoveIt2's setup assistant. :param urdf_path: Path to a Unified Robot Description Format file. - :param new_package_keyword: the string to replace 'package://' keyword + :param new_package_keyword: the string to replace ``package://`` keyword :param num_samples: number of samples to find the link that will always collide - :param save_path: Path to save the generated SRDF file. - If ``None``, create a temporary file. :param verbose: print debug info :return: Path to the generated SRDF file """ @@ -170,19 +158,12 @@ def generate_srdf( robot, num_samples=num_samples, verbose=verbose ) - if save_path is None: - _, save_path = tempfile.mkstemp(suffix=".srdf") - save_path = Path(save_path) - if not (save_dir := save_path.parent).is_dir(): - save_dir.mkdir(parents=True) - - # Save SRDF + # Create a new SRDF file and save + save_path = urdf_path.with_name(urdf_path.stem + "_mplib.srdf") with save_path.open("w") as f: f.write(srdf_str) if verbose: print(f"Saved the SRDF file to {save_path}") - # Remove the temporary URDF file from replace_urdf_package_keyword() - urdf_path.unlink() return save_path From 2e8abf6b7815f84d09ec610107d04b829f2179f9 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Tue, 16 Apr 2024 13:38:41 -0700 Subject: [PATCH 35/37] Do not evaluate default argument values --- docs/source/conf.py | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/source/conf.py b/docs/source/conf.py index 499f2c57..c06239d3 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -37,6 +37,7 @@ exclude_patterns = [] maximum_signature_line_length = 88 # limit maximum method/function signature length +autodoc_preserve_defaults = True autodoc_default_options = { "members": True, "member-order": "bysource", From 7b7af5d4653d527edd47d7e62fff04d8cbb7cea3 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Tue, 16 Apr 2024 13:27:04 -0700 Subject: [PATCH 36/37] Fix examples/tests and Planner --- .../tutorials/planning_with_fixed_joints.rst | 6 +++--- mplib/examples/detect_collision.py | 9 +++------ mplib/examples/two_stage_motion.py | 11 +++++------ mplib/planner.py | 4 ++-- tests/test_fcl_model.py | 10 +++------- tests/test_planner.py | 18 +++++------------- 6 files changed, 21 insertions(+), 37 deletions(-) diff --git a/docs/source/tutorials/planning_with_fixed_joints.rst b/docs/source/tutorials/planning_with_fixed_joints.rst index 989691fe..183a5aed 100644 --- a/docs/source/tutorials/planning_with_fixed_joints.rst +++ b/docs/source/tutorials/planning_with_fixed_joints.rst @@ -19,7 +19,7 @@ The optional `link_names` and `joint_names` parameters used to order the joints :language: python :start-after: # pickup ankor :end-before: # pickup ankor end - :emphasize-lines: 14 + :emphasize-lines: 12 Notice we have abstracted away how to decouple this motion into two stages. Here is the function definition: @@ -27,6 +27,6 @@ Notice we have abstracted away how to decouple this motion into two stages. Here :language: python :start-after: # move_in_two_stage ankor :end-before: # move_in_two_stage ankor end - :emphasize-lines: 18 + :emphasize-lines: 16 -The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9. \ No newline at end of file +The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9. diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py index 808938bb..330c1655 100644 --- a/mplib/examples/detect_collision.py +++ b/mplib/examples/detect_collision.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 +from mplib import Pose from mplib.collision_detection import fcl from mplib.examples.demo_setup import DemoSetup @@ -45,13 +46,9 @@ def demo(self): # floor ankor floor = fcl.Box([2, 2, 0.1]) # create a 2 x 2 x 0.1m box # create a collision object for the floor, with a 10cm offset in the z direction - floor_fcl_collision_object = fcl.CollisionObject( - floor, [0, 0, -0.1], [1, 0, 0, 0] - ) + floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1])) # update the planning world with the floor collision object - self.planner.planning_world.add_normal_object( - "floor", floor_fcl_collision_object - ) + self.planner.planning_world.add_object("floor", floor_fcl_collision_object) # floor ankor end print("\n----- self-collision-free qpos -----") # if the joint qpos does not include the gripper joints, diff --git a/mplib/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py index badfdc55..e2f9dcb6 100644 --- a/mplib/examples/two_stage_motion.py +++ b/mplib/examples/two_stage_motion.py @@ -3,6 +3,7 @@ import numpy as np import sapien.core as sapien +from mplib import Pose from mplib.collision_detection import fcl from mplib.examples.demo_setup import DemoSetup @@ -159,10 +160,8 @@ def demo(self): self.add_point_cloud() # also add the target as a collision object so we don't hit it fcl_red_cube = fcl.Box([0.04, 0.04, 0.14]) - collision_object = fcl.CollisionObject( - fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0] - ) - self.planner.planning_world.add_normal_object("target", collision_object) + collision_object = fcl.CollisionObject(fcl_red_cube, Pose(p=[0.7, 0, 0.07])) + self.planner.planning_world.add_object("target", collision_object) # go above the target pickup_pose[2] += 0.2 @@ -170,7 +169,7 @@ def demo(self): # pickup ankor end self.open_gripper() # move down to pick - self.planner.planning_world.remove_normal_object( + self.planner.planning_world.remove_object( "target" ) # remove the object so we don't report collision pickup_pose[2] -= 0.12 @@ -182,7 +181,7 @@ def demo(self): self.planner.robot.set_qpos(self.robot.get_qpos(), True) # add the attached box to the planning world - self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) + self.planner.update_attached_box([0.04, 0.04, 0.12], Pose(p=[0, 0, 0.14])) pickup_pose[2] += 0.12 result = self.plan_without_base(pickup_pose, has_attach=True) diff --git a/mplib/planner.py b/mplib/planner.py index 6969cf13..ae210537 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -67,9 +67,9 @@ def __init__( srdf := self.urdf.with_name(self.urdf.stem + "_mplib.srdf") ).is_file(): print(f"No SRDF file provided but found {srdf}") - self.srdf = srdf else: - self.srdf = generate_srdf(self.urdf, new_package_keyword, verbose=True) + srdf = generate_srdf(urdf, new_package_keyword, verbose=True) + self.srdf = srdf # replace package:// keyword if exists self.urdf = replace_urdf_package_keyword(self.urdf, new_package_keyword) diff --git a/tests/test_fcl_model.py b/tests/test_fcl_model.py index 6404ff44..245bf21b 100644 --- a/tests/test_fcl_model.py +++ b/tests/test_fcl_model.py @@ -60,15 +60,11 @@ def test_remove_collision_pairs_from_srdf(self): self.assertIn(pair, old_collision_pairs) def test_collision(self): - collisions = self.model.collide_full() + collisions = self.model.check_self_collision() self.assertGreater(len(collisions), 0) - # some of them are collisions - is_collision = [collision.is_collision() for collision in collisions] - self.assertTrue(any(is_collision)) self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"]) - collisions = self.model.collide_full() - is_collision = [collision.is_collision() for collision in collisions] - self.assertFalse(any(is_collision)) + collisions = self.model.check_self_collision() + self.assertEqual(len(collisions), 0) if __name__ == "__main__": diff --git a/tests/test_planner.py b/tests/test_planner.py index 3bfc3378..a6dcb319 100644 --- a/tests/test_planner.py +++ b/tests/test_planner.py @@ -131,13 +131,9 @@ def test_self_collision(self): def test_env_collision(self): floor = fcl.Box([2, 2, 0.1]) # create a 2 x 2 x 0.1m box # create a collision object for the floor, with a 10cm offset in the z direction - floor_fcl_collision_object = fcl.CollisionObject( - floor, Pose([0, 0, -0.1], [1, 0, 0, 0]) - ) + floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1])) # update the planning world with the floor collision object - self.planner.planning_world.add_normal_object( - "floor", floor_fcl_collision_object - ) + self.planner.planning_world.add_object("floor", floor_fcl_collision_object) env_collision_qpos = [ 0, @@ -151,7 +147,7 @@ def test_env_collision(self): self.assertTrue(self.planner.check_for_env_collision(env_collision_qpos)) # remove the floor and check for env-collision returns no collision - self.planner.remove_normal_object("floor") + self.planner.remove_object("floor") self.assertFalse(self.planner.check_for_env_collision(env_collision_qpos)) def test_IK(self): @@ -173,17 +169,13 @@ def test_IK(self): # now put down a floor and check that the robot can't reach the pose floor = fcl.Box([2, 2, 0.1]) # create a 2 x 2 x 0.1m box # create a collision object for the floor, with a 10cm offset in the z direction - floor_fcl_collision_object = fcl.CollisionObject( - floor, Pose([0, 0, -0.1], [1, 0, 0, 0]) - ) + floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1])) under_floor_target_pose = deepcopy(self.target_pose) under_floor_target_pose.set_p([0.4, 0.3, -0.1]) status, _ = self.planner.IK(under_floor_target_pose, self.init_qpos) self.assertEqual(status, "Success") - self.planner.planning_world.add_normal_object( - "floor", floor_fcl_collision_object - ) + self.planner.planning_world.add_object("floor", floor_fcl_collision_object) status, _ = self.planner.IK(under_floor_target_pose, self.init_qpos) self.assertNotEqual(status, "Success") From f247d2a417120eb1195e929f0e9f26c6a9f52d0e Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Tue, 16 Apr 2024 14:12:17 -0700 Subject: [PATCH 37/37] Update readthedocs dependencies --- .readthedocs.yaml | 2 ++ docs/requirements.txt | 1 + 2 files changed, 3 insertions(+) diff --git a/.readthedocs.yaml b/.readthedocs.yaml index d023ff0e..93336e8e 100644 --- a/.readthedocs.yaml +++ b/.readthedocs.yaml @@ -4,6 +4,8 @@ build: os: "ubuntu-22.04" tools: python: "3.10" + apt_packages: + - libx11-6 jobs: post_install: - python docs/get_wheel_artifact.py haosulab/MPlib --py cp310 --wait-sec 600 diff --git a/docs/requirements.txt b/docs/requirements.txt index 343e9a85..2e2cb99d 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -2,3 +2,4 @@ myst-parser furo sphinx-copybutton sphinxext-opengraph +sapien~=3.0.0.dev