From be560d7200a0e3763a193a2b8b3261e95079f909 Mon Sep 17 00:00:00 2001 From: Yuyao Liu Date: Tue, 14 Jan 2025 23:18:33 +0800 Subject: [PATCH 1/5] bugfix for attach object when articulation base pose is non-zero; update stale link in dockerfile --- docker/Dockerfile | 2 +- include/mplib/core/attached_body.h | 5 ++++- pybind/docstring/planning_world.h | 16 ++++++++-------- src/planning_world.cpp | 12 ++++++++++-- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 77f59d32..2c3721a1 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -23,7 +23,7 @@ RUN git clone --single-branch -b v2.1 --depth 1 https://github.com/danfis/libccd # Reference: https://www.boost.org/doc/libs/1_76_0/more/getting_started/unix-variants.html#easy-build-and-install # NOTE(jigu): there are compilation errors when boost.python is also built. # To build boost.python, maybe we need to refer to https://www.boost.org/doc/libs/1_35_0/libs/python/doc/building.html#examples -RUN wget https://boostorg.jfrog.io/artifactory/main/release/1.84.0/source/boost_1_84_0.tar.gz && \ +RUN wget https://archives.boost.io/release/1.84.0/source/boost_1_84_0.tar.gz && \ tar -xf boost_1_84_0.tar.gz && \ rm boost_1_84_0.tar.gz && \ cd boost_1_84_0 && ./bootstrap.sh --without-libraries=python && ./b2 install && \ diff --git a/include/mplib/core/attached_body.h b/include/mplib/core/attached_body.h index 4a71f500..ce85e2e6 100644 --- a/include/mplib/core/attached_body.h +++ b/include/mplib/core/attached_body.h @@ -65,7 +65,10 @@ class AttachedBodyTpl { /// @brief Gets the global pose of the articulation link that this body is attached to Isometry3 getAttachedLinkGlobalPose() const { - return pinocchio_model_->getLinkPose(attached_link_id_); + const auto pose_wrt_base = pinocchio_model_->getLinkPose(attached_link_id_); + const auto base_pose = attached_articulation_->getBasePose().toIsometry(); + return base_pose * pose_wrt_base; + // return pinocchio_model_->getLinkPose(attached_link_id_); } /// @brief Gets the global pose of the attached object diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 10238055..0bfba89f 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -287,14 +287,6 @@ static const char *__doc_mplib_PlanningWorldTpl_getAllowedCollisionMatrix = R"doc( Get the current allowed collision matrix)doc"; -static const char *__doc_mplib_PlanningWorldTpl_setAllowedCollision = -R"doc( -Set the allowed collision. For more comprehensive API, please get the -``AllowedCollisionMatrix`` object and use its methods. - -:param name1: name of the first object -:param name2: name of the second object)doc"; - static const char *__doc_mplib_PlanningWorldTpl_getArticulation = R"doc( Gets the articulation (ArticulatedModelPtr) with given name @@ -384,6 +376,14 @@ Removes (and detaches) the collision object with given name if exists. Updates :return: ``True`` if success, ``False`` if the non-articulated object with given name does not exist)doc"; +static const char *__doc_mplib_PlanningWorldTpl_setAllowedCollision = +R"doc( +Set the allowed collision. For more comprehensive API, please get the +``AllowedCollisionMatrix`` object and use its methods. + +:param name1: name of the first object +:param name2: name of the second object)doc"; + static const char *__doc_mplib_PlanningWorldTpl_setArticulationPlanned = R"doc( Sets articulation with given name as being planned diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 3b7283d9..738a40a7 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -112,8 +112,12 @@ 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 = object_map_.at(name)->pose; - const auto T_world_link = + // const auto T_world_link = + // planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); + const auto T_link_wrt_base = planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); + const auto T_world_link = planned_articulation_map_.at(art_name)->getBasePose().toIsometry() * + T_link_wrt_base; attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj), touch_links); } @@ -122,8 +126,12 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id) { const auto T_world_obj = object_map_.at(name)->pose; - const auto T_world_link = + // const auto T_world_link = + // planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); + const auto T_link_wrt_base = planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); + const auto T_world_link = planned_articulation_map_.at(art_name)->getBasePose().toIsometry() * + T_link_wrt_base; attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj)); } From be6a63e12458a75e4dd43419ddea8c8ffe5d6bab Mon Sep 17 00:00:00 2001 From: Yuyao Liu Date: Wed, 15 Jan 2025 15:32:14 +0800 Subject: [PATCH 2/5] before merge --- .gitignore | 1 + commands.md | 39 +++++++++++++++++++++++++++++++++++++++ docker/Dockerfile | 8 ++++++++ 3 files changed, 48 insertions(+) create mode 100644 commands.md diff --git a/.gitignore b/.gitignore index 5919087b..b86be8cf 100644 --- a/.gitignore +++ b/.gitignore @@ -5,6 +5,7 @@ _ext *.so build/ dist/ +repaired_wheels/ eggs/ .eggs/ *.egg-info/ diff --git a/commands.md b/commands.md new file mode 100644 index 00000000..46577bd3 --- /dev/null +++ b/commands.md @@ -0,0 +1,39 @@ +## build docker (only for the first time) +```bash +sudo docker build -f docker/Dockerfile -t mplib-manylinux2014 . + +# with proxy (for china mainland) +sudo docker build -f docker/Dockerfile --network host --build-arg HTTP_PROXY=http://127.0.0.1:7890 --build-arg HTTPS_PROXY=http://127.0.0.1:7890 --build-arg NO_PROXY=localhost,127.0.0.1 -t mplib-manylinux2014 . +``` + +When running with proxy, prepend the proxy settings to the dockerfile as well. +```dockerfile +ARG HTTP_PROXY +ARG HTTPS_PROXY +ARG NO_PROXY + +ENV http_proxy=$HTTP_PROXY +ENV https_proxy=$HTTPS_PROXY +ENV no_proxy=$NO_PROXY +``` + +## launch docker +```bash +sudo docker run --rm -it --network host \ + -v "$(pwd)":/workspace \ + -w /workspace \ + mplib-manylinux2014 \ + /bin/bash +``` + +## build wheel +Before buiding wheel, we should have the python version we want to build wheel for installed in the docker container (conda is recommended). For distribution to other people or other machines, use `auditwheel` to wrap all the dependencies into the wheel file. +```bash +pip wheel . -w dist -v +# the wheel file will be in the ./dist folder + +# replace cp3xx with the python version we have +auditwheel repair dist/mplib-0.2.0-cp3xx-cp3xx-linux_x86_64.whl -w repaired_wheels +# the repaired wheel file will be in the ./repaired_wheels folder +``` +Now we can just `pip install` the repaired wheel file on any (linux) machine with the same python version. diff --git a/docker/Dockerfile b/docker/Dockerfile index 2c3721a1..cfeec473 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,5 +1,13 @@ FROM quay.io/pypa/manylinux2014_x86_64 +# ARG HTTP_PROXY +# ARG HTTPS_PROXY +# ARG NO_PROXY + +# ENV http_proxy=$HTTP_PROXY +# ENV https_proxy=$HTTPS_PROXY +# ENV no_proxy=$NO_PROXY + # basic development tools RUN yum update -y && yum install -y wget git && yum upgrade -y && yum clean all WORKDIR /workspace From 75e6a2a9f150c144c2ffd9ceb93090bf803795a6 Mon Sep 17 00:00:00 2001 From: Yuyao Liu Date: Wed, 15 Jan 2025 16:10:14 +0800 Subject: [PATCH 3/5] add set_pose to FCLObject, add check_general_object_collision and check_general_object_pair_collision --- .../collision_detection/collision_common.h | 11 +- .../fcl/collision_common.h | 2 + include/mplib/planning_world.h | 50 +++++ mplib/__init__.py | 1 + .../fcl/pybind_collision_common.cpp | 4 +- .../pybind_collision_common.cpp | 9 +- .../collision_detection/collision_common.h | 2 + .../fcl/collision_common.h | 2 + pybind/docstring/planning_world.h | 33 +++ pybind/pybind_planning_world.cpp | 17 +- .../fcl/collision_common.cpp | 7 + src/planning_world.cpp | 194 +++++++++++++++++- 12 files changed, 319 insertions(+), 13 deletions(-) diff --git a/include/mplib/collision_detection/collision_common.h b/include/mplib/collision_detection/collision_common.h index 4158d564..5fa0389c 100644 --- a/include/mplib/collision_detection/collision_common.h +++ b/include/mplib/collision_detection/collision_common.h @@ -22,13 +22,16 @@ struct WorldCollisionResultTpl { 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) + const std::string &link_name1, + const std::string &link_name2, + S max_penetration) : res(res), collision_type(collision_type), object_name1(object_name1), object_name2(object_name2), link_name1(link_name1), - link_name2(link_name2) {}; + link_name2(link_name2), + max_penetration(max_penetration) {}; ::fcl::CollisionResult res; ///< the fcl CollisionResult std::string collision_type, ///< type of the collision @@ -36,6 +39,7 @@ struct WorldCollisionResultTpl { object_name2, ///< name of the second object link_name1, ///< link name of the first object in collision link_name2; ///< link name of the second object in collision + S max_penetration {-1}; ///< max penentration distance between the two objects }; /// Result of minimum distance-to-collision query. @@ -51,7 +55,8 @@ struct WorldDistanceResultTpl { 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 &object_name2, + const std::string &link_name1, const std::string &link_name2) : res(res), min_distance(min_distance), diff --git a/include/mplib/collision_detection/fcl/collision_common.h b/include/mplib/collision_detection/fcl/collision_common.h index 37b635cd..4b9f0aec 100644 --- a/include/mplib/collision_detection/fcl/collision_common.h +++ b/include/mplib/collision_detection/fcl/collision_common.h @@ -53,6 +53,8 @@ struct FCLObject { std::vector> shapes; /// Relative poses from this FCLObject to each collision shape std::vector> shape_poses; + + void setPose(const Pose &pose_); }; /** diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index ddf91496..3982c33b 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -435,6 +435,51 @@ class PlanningWorldTpl { std::vector checkCollision( const CollisionRequest &request = CollisionRequest()) const; + /** + * Check full collisions between all pairs of scene objects + * + * @param scene_object_names: list of scene object names + * @param request: collision request params. + * @return: List of ``WorldCollisionResult`` objects + */ + std::vector checkSceneCollision( + const std::vector &scene_object_names, + const CollisionRequest &request = CollisionRequest()) const; + + std::vector checkArticulationArticulationCollision( + const ArticulatedModelPtr &art1, const ArticulatedModelPtr &art2, + const CollisionRequest &request) const; + + std::vector checkArticulationObjectCollision( + const ArticulatedModelPtr &art, const FCLObjectPtr &obj, + const CollisionRequest &request) const; + + std::vector checkObjectObjectCollision( + const std::string &name1, const std::string &name2, + const CollisionRequest &request) const; + + /* + * Check collision between an object and the world. + * + * @param name: name of the object + * @param request: collision request params. + * @return: List of ``WorldCollisionResult`` objects + */ + std::vector checkGeneralObjectCollision( + const std::string &name, const CollisionRequest &request = CollisionRequest()) const; + + /** + * Check collision between two specified objects. + * + * @param name1: name of the first object + * @param name2: name of the second object + * @param request: collision request params. + * @return: List of ``WorldCollisionResult`` objects + */ + std::vector checkGeneralObjectPairCollision( + const std::string &name1, const std::string &name2, + const CollisionRequest &request = CollisionRequest()) const; + /** * The minimum distance to self-collision given the robot in current state. * Calls ``distanceSelf()``. @@ -490,6 +535,11 @@ class PlanningWorldTpl { WorldDistanceResult distance( const DistanceRequest &request = DistanceRequest()) const; + std::vector distanceScene( + const std::vector &scene_object_names, + const DistanceRequest &request = DistanceRequest()) const; + + private: std::unordered_map articulation_map_; std::unordered_map object_map_; diff --git a/mplib/__init__.py b/mplib/__init__.py index 47a2bf93..ee877ea2 100644 --- a/mplib/__init__.py +++ b/mplib/__init__.py @@ -8,5 +8,6 @@ Pose, set_global_seed, ) +from .collision_detection.fcl import FCLObject __version__ = version("mplib") diff --git a/pybind/collision_detection/fcl/pybind_collision_common.cpp b/pybind/collision_detection/fcl/pybind_collision_common.cpp index 3abb498a..20e35a92 100644 --- a/pybind/collision_detection/fcl/pybind_collision_common.cpp +++ b/pybind/collision_detection/fcl/pybind_collision_common.cpp @@ -47,7 +47,9 @@ void build_pyfcl_collision_common(py::module &m) { for (const auto &pose : fcl_obj.shape_poses) ret.push_back(Pose(pose)); return ret; }, - DOC(mplib, collision_detection, fcl, FCLObject, shape_poses)); + DOC(mplib, collision_detection, fcl, FCLObject, shape_poses)) + .def("set_pose", &FCLObject::setPose, py::arg("pose"), + DOC(mplib, collision_detection, fcl, FCLObject, setPose)); // collide / distance functions m.def( diff --git a/pybind/collision_detection/pybind_collision_common.cpp b/pybind/collision_detection/pybind_collision_common.cpp index 74c35e48..608b60e1 100644 --- a/pybind/collision_detection/pybind_collision_common.cpp +++ b/pybind/collision_detection/pybind_collision_common.cpp @@ -24,9 +24,9 @@ void build_pycollision_common(py::module &m) { .def(py::init<>(), DOC(mplib, collision_detection, WorldCollisionResultTpl, WorldCollisionResultTpl)) .def(py::init(), + const std::string &, const std::string &, const std::string &, S>(), py::arg("res"), py::arg("collision_type"), py::arg("object_name1"), - py::arg("object_name2"), py::arg("link_name1"), py::arg("link_name2"), + py::arg("object_name2"), py::arg("link_name1"), py::arg("link_name2"), py::arg("max_penetration"), DOC(mplib, collision_detection, WorldCollisionResultTpl, WorldCollisionResultTpl, 2)) .def_readonly("res", &WorldCollisionResult::res, @@ -45,7 +45,10 @@ void build_pycollision_common(py::module &m) { DOC(mplib, collision_detection, WorldCollisionResultTpl, link_name1)) .def_readonly( "link_name2", &WorldCollisionResult::link_name2, - DOC(mplib, collision_detection, WorldCollisionResultTpl, link_name2)); + DOC(mplib, collision_detection, WorldCollisionResultTpl, link_name2)) + .def_readonly( + "max_penetration", &WorldCollisionResult::max_penetration, + DOC(mplib, collision_detection, WorldCollisionResultTpl, max_penetration)); auto PyWorldDistanceResult = py::class_>( diff --git a/pybind/docstring/collision_detection/collision_common.h b/pybind/docstring/collision_detection/collision_common.h index e2f3f4d9..d699c965 100644 --- a/pybind/docstring/collision_detection/collision_common.h +++ b/pybind/docstring/collision_detection/collision_common.h @@ -46,6 +46,8 @@ static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_objec static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_res = R"doc(the fcl CollisionResult)doc"; +static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_max_penetration = R"doc(max penetration depth between the two objects)doc"; + 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 = diff --git a/pybind/docstring/collision_detection/fcl/collision_common.h b/pybind/docstring/collision_detection/fcl/collision_common.h index 2e67fdb6..904091c2 100644 --- a/pybind/docstring/collision_detection/fcl/collision_common.h +++ b/pybind/docstring/collision_detection/fcl/collision_common.h @@ -57,6 +57,8 @@ static const char *__doc_mplib_collision_detection_fcl_FCLObject_shape_poses = R static const char *__doc_mplib_collision_detection_fcl_FCLObject_shapes = R"doc(All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLObject_setPose = R"doc(Update the pose of this FCLObject)doc"; + static const char *__doc_mplib_collision_detection_fcl_collide = R"doc( Collision function between two ``FCLObject`` diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 0bfba89f..56743f38 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -219,6 +219,31 @@ articulation-attach collision, attach-attach collision) :param request: collision request params. :return: List of ``WorldCollisionResult`` objects)doc"; +static const char *__doc_mplib_PlanningWorldTpl_checkSceneCollision = +R"doc( +Check collision with scene objects (non-articulated objects) + +:param scene_object_names: names of scene objects to check collision against. +:param request: collision request params. +:return: List of ``WorldCollisionResult`` objects)doc"; + +static const char *__doc_mplib_PlanningWorldTpl_checkGeneralObjectCollision = + R"doc( +Check collision between an object and the world. + +:param name: name of the object +:param request: collision request params. +:return: List of ``WorldCollisionResult`` objects)doc"; + +static const char *__doc_mplib_PlanningWorldTpl_checkGeneralObjectPairCollision = + R"doc( +Check collision between two specified objects. + +:param name1: name of the first object +:param name2: name of the second object +:param request: collision request params. +:return: List of ``WorldCollisionResult`` objects)doc"; + static const char *__doc_mplib_PlanningWorldTpl_detachAllObjects = R"doc( Detaches all attached objects. Updates ``acm_`` to disallow collision between @@ -259,6 +284,14 @@ Get the minimum distance to self-collision given the robot in current state :param request: distance request params. :return: a ``WorldDistanceResult`` object)doc"; +static const char *__doc_mplib_PlanningWorldTpl_distanceScene = +R"doc( +Compute the minimum distance-to-all-collision between a lsit of objects + +:param scene_object_names: list of scene object names +:param request: distance request params. +:return: a ``WorldDistanceResult`` object)doc"; + static const char *__doc_mplib_PlanningWorldTpl_distanceToCollision = R"doc( Compute the minimum distance-to-all-collision. Calls ``distance()``. diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index a0684b30..7ed1329f 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -152,6 +152,18 @@ void build_pyplanning_world(py::module &pymp) { .def("check_collision", &PlanningWorld::checkCollision, py::arg("request") = CollisionRequest(), DOC(mplib, PlanningWorldTpl, checkCollision)) + .def("check_scene_collision", &PlanningWorld::checkSceneCollision, + py::arg("scene_object_names"), + py::arg("request") = CollisionRequest(), + DOC(mplib, PlanningWorldTpl, checkSceneCollision)) + .def("check_general_object_collision", &PlanningWorld::checkGeneralObjectCollision, + py::arg("name1"), + py::arg("request") = CollisionRequest(), + DOC(mplib, PlanningWorldTpl, checkGeneralObjectCollision)) + .def("check_general_object_pair_collision", + &PlanningWorld::checkGeneralObjectPairCollision, py::arg("name1"), + py::arg("name2"), py::arg("request") = CollisionRequest(), + DOC(mplib, PlanningWorldTpl, checkGeneralObjectPairCollision)) .def("distance_to_self_collision", &PlanningWorld::distanceToSelfCollision, DOC(mplib, PlanningWorldTpl, distanceToSelfCollision)) @@ -166,7 +178,10 @@ void build_pyplanning_world(py::module &pymp) { .def("distance_to_collision", &PlanningWorld::distanceToCollision, DOC(mplib, PlanningWorldTpl, distanceToCollision)) .def("distance", &PlanningWorld::distance, py::arg("request") = DistanceRequest(), - DOC(mplib, PlanningWorldTpl, distance)); + DOC(mplib, PlanningWorldTpl, distance)) + .def("distance_scene", &PlanningWorld::distanceScene, + py::arg("scene_object_names"), py::arg("request") = DistanceRequest(), + DOC(mplib, PlanningWorldTpl, distanceScene)); } } // namespace mplib diff --git a/src/collision_detection/fcl/collision_common.cpp b/src/collision_detection/fcl/collision_common.cpp index e7c048ef..a6b3ea51 100644 --- a/src/collision_detection/fcl/collision_common.cpp +++ b/src/collision_detection/fcl/collision_common.cpp @@ -31,6 +31,13 @@ FCLObject::FCLObject(const std::string &name_, const Pose &pose_, } } +template +void FCLObject::setPose(const Pose &pose_) { + pose = pose_.toIsometry(); + for (size_t i = 0; i < shapes.size(); i++) + shapes[i]->setTransform(pose * shape_poses[i]); +} + template size_t collide(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, const fcl::CollisionRequest &request, diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 738a40a7..f19850c0 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -8,11 +8,15 @@ namespace mplib { -// Explicit Template Instantiation Definition ========================================== -#define DEFINE_TEMPLATE_PLANNING_WORLD(S) template class PlanningWorldTpl - -DEFINE_TEMPLATE_PLANNING_WORLD(float); -DEFINE_TEMPLATE_PLANNING_WORLD(double); +template +S getFCLContactMaxPenetration(const fcl::CollisionResult &result) { + S max_penetration = -1; + for (int i = 0; i < result.numContacts(); ++i) { + const auto &contact = result.getContact(i); + max_penetration = std::max(max_penetration, contact.penetration_depth); + } + return max_penetration; +} template PlanningWorldTpl::PlanningWorldTpl( @@ -238,6 +242,26 @@ void PlanningWorldTpl::attachMesh(const std::string &mesh_path, art_name, link_id, pose); } +/* +template +void PlanningWorldTpl::attachPointCloud( + const std::string &name, + const MatrixX3 &vertices, + double resolution, + const std::string &art_name, + int link_id, + const Pose &pose) { + auto tree = std::make_shared(resolution); + for (const auto &row : vertices.rowwise()) + tree->updateNode(octomap::point3d(row(0), row(1), row(2)), true); + + removeObject(name); + auto obj = std::make_shared(std::make_shared>(tree)); + addObject(name, obj); + attachObject(name, art_name, link_id, pose); +} +*/ + template bool PlanningWorldTpl::detachObject(const std::string &name, bool also_remove) { if (also_remove) { @@ -342,6 +366,7 @@ std::vector> PlanningWorldTpl::checkSelfCollision( tmp.object_name2 = name2; tmp.link_name1 = name1; tmp.link_name2 = name2; + tmp.max_penetration = getFCLContactMaxPenetration(result); ret.push_back(tmp); } } @@ -349,6 +374,92 @@ std::vector> PlanningWorldTpl::checkSelfCollision( return ret; } +template +std::vector> PlanningWorldTpl::checkArticulationArticulationCollision( + const ArticulatedModelPtr &art1, const ArticulatedModelPtr &art2, + const CollisionRequest &request) const { + const auto &fcl_model1 = art1->getFCLModel(); + const auto &fcl_model2 = art2->getFCLModel(); + auto results = fcl_model1->checkCollisionWith(fcl_model2, request, acm_); + return results; +} + +template +std::vector> PlanningWorldTpl::checkArticulationObjectCollision( + const ArticulatedModelPtr &art, const FCLObjectPtr &obj, + const CollisionRequest &request) const { + const auto &fcl_model1 = art->getFCLModel(); + auto results = fcl_model1->checkCollisionWith(obj, request, acm_); + return results; +} + +template +std::vector> PlanningWorldTpl::checkObjectObjectCollision( + const std::string &name1, const std::string &name2, + const CollisionRequest &request) const { + const auto &obj1 = object_map_.at(name1); + const auto &obj2 = object_map_.at(name2); + std::vector> results; + CollisionResult result; + if (auto type = acm_->getAllowedCollision(name1, name2); + !type || type == collision_detection::AllowedCollision::NEVER) { + collision_detection::fcl::collide(obj1, obj2, request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "object_object"; + tmp.object_name1 = name1; + tmp.object_name2 = name2; + tmp.max_penetration = getFCLContactMaxPenetration(result); + results.push_back(tmp); + } + } + return results; +} + +template +std::vector> PlanningWorldTpl::checkGeneralObjectPairCollision( + const std::string &name1, const std::string &name2, const CollisionRequest &request) const { + + if (hasArticulation(name1) && hasArticulation(name2)) { + const auto &art1 = getArticulation(name1); + const auto &art2 = getArticulation(name2); + return checkArticulationArticulationCollision(art1, art2, request); + } else if (hasArticulation(name1) && hasObject(name2)) { + const auto &art = getArticulation(name1); + const auto &obj = getObject(name2); + return checkArticulationObjectCollision(art, obj, request); + } else if (hasObject(name1) && hasArticulation(name2)) { + const auto &art = getArticulation(name2); + const auto &obj = getObject(name1); + return checkArticulationObjectCollision(art, obj, request); + } else if (hasObject(name1) && hasObject(name2)) { + return checkObjectObjectCollision(name1, name2, request); + } + + std::vector ret; + return ret; +} + +template +std::vector> PlanningWorldTpl::checkGeneralObjectCollision( + const std::string &name, const CollisionRequest &request) const { + + std::vector ret; + + for (const auto &[art_name, art] : articulation_map_) { + if (name == art_name) continue; + auto results = checkGeneralObjectPairCollision(name, art_name, request); + ret.insert(ret.end(), results.begin(), results.end()); + } + for (const auto &[obj_name, obj] : object_map_) { + if (name == obj_name) continue; + auto results = checkGeneralObjectPairCollision(name, obj_name, request); + ret.insert(ret.end(), results.begin(), results.end()); + } + return ret; +} + template std::vector> PlanningWorldTpl::checkRobotCollision( const CollisionRequest &request) const { @@ -410,6 +521,7 @@ std::vector> PlanningWorldTpl::checkRobotCollision tmp.object_name2 = scene_obj->name; tmp.link_name1 = attached_body_name; tmp.link_name2 = scene_obj->name; + tmp.max_penetration = getFCLContactMaxPenetration(result); ret.push_back(tmp); } } @@ -426,6 +538,40 @@ std::vector> PlanningWorldTpl::checkCollision( return ret1; } +template +std::vector> PlanningWorldTpl::checkSceneCollision( + const std::vector &scene_object_names, + const CollisionRequest &request) const { + + std::vector ret; + CollisionResult result; + + std::vector scene_objects; + for (const auto &name : scene_object_names) { + if (auto it = object_map_.find(name); it != object_map_.end()) + scene_objects.push_back(it->second); + } + for (int i = 0; i < scene_object_names.size(); i++) { + for (int j = i + 1; j < scene_object_names.size(); j++) { + if (auto type = acm_->getAllowedCollision(scene_object_names[i], scene_object_names[j]); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::collide(scene_objects[i], scene_objects[j], request, result); + if (result.isCollision()) { + WorldCollisionResult tmp; + tmp.res = result; + tmp.collision_type = "sceneobject_sceneobject"; + tmp.object_name1 = scene_object_names[i]; + tmp.object_name2 = scene_object_names[j]; + tmp.max_penetration = getFCLContactMaxPenetration(result); + ret.push_back(tmp); + } + } + } + } + return ret; +} + template WorldDistanceResultTpl PlanningWorldTpl::distanceSelf( const DistanceRequest &request) const { @@ -561,4 +707,42 @@ WorldDistanceResultTpl PlanningWorldTpl::distance( return ret1.min_distance < ret2.min_distance ? ret1 : ret2; } +template +std::vector> PlanningWorldTpl::distanceScene( + const std::vector &scene_object_names, + const DistanceRequest &request) const { + + std::vector ret_list; + WorldDistanceResult ret; + DistanceResult result; + + std::vector scene_objects; + for (const auto &name : scene_object_names) { + if (auto it = object_map_.find(name); it != object_map_.end()) + scene_objects.push_back(it->second); + } + for (int i = 0; i < scene_object_names.size(); i++) { + for (int j = i + 1; j < scene_object_names.size(); j++) { + if (auto type = acm_->getAllowedCollision(scene_object_names[i], scene_object_names[j]); + !type || type == collision_detection::AllowedCollision::NEVER) { + result.clear(); + collision_detection::fcl::distance(scene_objects[i], scene_objects[j], request, result); + ret.res = result; + ret.min_distance = result.min_distance; + ret.distance_type = "sceneobject_sceneobject"; + ret.object_name1 = scene_object_names[i]; + ret.object_name2 = scene_object_names[j]; + ret_list.push_back(ret); + } + } + } + return ret_list; +} + +// Explicit Template Instantiation Definition ========================================== +#define DEFINE_TEMPLATE_PLANNING_WORLD(S) template class PlanningWorldTpl + +DEFINE_TEMPLATE_PLANNING_WORLD(float); +DEFINE_TEMPLATE_PLANNING_WORLD(double); + } // namespace mplib From b403630f2b1f02be70e8972fcae4918f6a247194 Mon Sep 17 00:00:00 2001 From: Yuyao Liu Date: Wed, 15 Jan 2025 16:12:19 +0800 Subject: [PATCH 4/5] fix attached object pose to consider base_pose --- include/mplib/core/articulated_model.h | 4 +++ include/mplib/core/attached_body.h | 5 +--- include/mplib/planning_world.h | 2 +- pybind/pybind_planning_world.cpp | 1 + .../fcl/collision_common.cpp | 1 + src/planning_world.cpp | 30 +++++++++---------- 6 files changed, 22 insertions(+), 21 deletions(-) diff --git a/include/mplib/core/articulated_model.h b/include/mplib/core/articulated_model.h index f816461b..601fb9db 100644 --- a/include/mplib/core/articulated_model.h +++ b/include/mplib/core/articulated_model.h @@ -198,6 +198,10 @@ class ArticulatedModelTpl { setQpos(current_qpos_, true); // update all collision links in the fcl_model_ } + Isometry3 getLinkGlobalPose(size_t link_index) const { + return base_pose_ * pinocchio_model_->getLinkPose(link_index); + } + /** * Update the SRDF file to disable self-collisions. * diff --git a/include/mplib/core/attached_body.h b/include/mplib/core/attached_body.h index ce85e2e6..491adbc1 100644 --- a/include/mplib/core/attached_body.h +++ b/include/mplib/core/attached_body.h @@ -65,10 +65,7 @@ class AttachedBodyTpl { /// @brief Gets the global pose of the articulation link that this body is attached to Isometry3 getAttachedLinkGlobalPose() const { - const auto pose_wrt_base = pinocchio_model_->getLinkPose(attached_link_id_); - const auto base_pose = attached_articulation_->getBasePose().toIsometry(); - return base_pose * pose_wrt_base; - // return pinocchio_model_->getLinkPose(attached_link_id_); + return attached_articulation_->getLinkGlobalPose(attached_link_id_); } /// @brief Gets the global pose of the attached object diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 3982c33b..fff49b79 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -173,7 +173,7 @@ class PlanningWorldTpl { * @param resolution: resolution of the point in ``octomap::OcTree`` */ void addPointCloud(const std::string &name, const MatrixX3 &vertices, - double resolution = 0.01); + double resolution = 0.01, const Pose &pose = Pose()); /** * Removes (and detaches) the collision object with given name if exists. diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 7ed1329f..0610eec2 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -68,6 +68,7 @@ void build_pyplanning_world(py::module &pymp) { DOC(mplib, PlanningWorldTpl, addObject, 2)) .def("add_point_cloud", &PlanningWorld::addPointCloud, py::arg("name"), py::arg("vertices"), py::arg("resolution") = 0.01, + py::arg("pose") = Pose(), DOC(mplib, PlanningWorldTpl, addPointCloud)) .def("remove_object", &PlanningWorld::removeObject, py::arg("name"), DOC(mplib, PlanningWorldTpl, removeObject)) diff --git a/src/collision_detection/fcl/collision_common.cpp b/src/collision_detection/fcl/collision_common.cpp index a6b3ea51..d828ea18 100644 --- a/src/collision_detection/fcl/collision_common.cpp +++ b/src/collision_detection/fcl/collision_common.cpp @@ -34,6 +34,7 @@ FCLObject::FCLObject(const std::string &name_, const Pose &pose_, template void FCLObject::setPose(const Pose &pose_) { pose = pose_.toIsometry(); + // std::cerr << "FCLObject::setPose isometry: " << pose << std::endl; for (size_t i = 0; i < shapes.size(); i++) shapes[i]->setTransform(pose * shape_poses[i]); } diff --git a/src/planning_world.cpp b/src/planning_world.cpp index f19850c0..356dd6d3 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -10,7 +10,9 @@ namespace mplib { template S getFCLContactMaxPenetration(const fcl::CollisionResult &result) { - S max_penetration = -1; + S max_penetration = std::numeric_limits::min(); + if (result.numContacts() == 0) return 1; + for (int i = 0; i < result.numContacts(); ++i) { const auto &contact = result.getContact(i); max_penetration = std::max(max_penetration, contact.penetration_depth); @@ -92,12 +94,16 @@ void PlanningWorldTpl::addObject(const std::string &name, template void PlanningWorldTpl::addPointCloud(const std::string &name, const MatrixX3 &vertices, - double resolution) { + double resolution, + const Pose &pose) { auto tree = std::make_shared(resolution); for (const auto &row : vertices.rowwise()) tree->updateNode(octomap::point3d(row(0), row(1), row(2)), true); - addObject(name, - std::make_shared(std::make_shared>(tree))); + auto cobject = std::make_shared(std::make_shared>(tree)); + addObject( + std::make_shared( + name, pose, std::vector {cobject}, std::vector> {Pose()} + )); } template @@ -116,12 +122,8 @@ 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 = object_map_.at(name)->pose; - // const auto T_world_link = - // planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); - const auto T_link_wrt_base = - planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); - const auto T_world_link = planned_articulation_map_.at(art_name)->getBasePose().toIsometry() * - T_link_wrt_base; + const auto T_world_link = + planned_articulation_map_.at(art_name)->getLinkGlobalPose(link_id); attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj), touch_links); } @@ -130,12 +132,8 @@ template void PlanningWorldTpl::attachObject(const std::string &name, const std::string &art_name, int link_id) { 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); - const auto T_link_wrt_base = - planned_articulation_map_.at(art_name)->getPinocchioModel()->getLinkPose(link_id); - const auto T_world_link = planned_articulation_map_.at(art_name)->getBasePose().toIsometry() * - T_link_wrt_base; + const auto T_world_link = + planned_articulation_map_.at(art_name)->getLinkGlobalPose(link_id); attachObject(name, art_name, link_id, Pose(T_world_link.inverse() * T_world_obj)); } From ced3d30c82cedcb45a0aba59c1db1e1de9a96281 Mon Sep 17 00:00:00 2001 From: Yuyao Liu Date: Wed, 15 Jan 2025 16:37:10 +0800 Subject: [PATCH 5/5] minor bug fix --- .../collision_detection/collision_common.h | 4 +- .../fcl/collision_common.h | 6 +- pybind/docstring/core/articulated_model.h | 4 ++ pybind/docstring/planning_world.h | 62 ++++++++++--------- src/planning_world.cpp | 10 +-- 5 files changed, 48 insertions(+), 38 deletions(-) diff --git a/pybind/docstring/collision_detection/collision_common.h b/pybind/docstring/collision_detection/collision_common.h index d699c965..320128f8 100644 --- a/pybind/docstring/collision_detection/collision_common.h +++ b/pybind/docstring/collision_detection/collision_common.h @@ -40,14 +40,14 @@ static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_link_ static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_link_name2 = R"doc(link name of the second object in collision)doc"; +static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_max_penetration = R"doc(max penentration distance between the two objects)doc"; + static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_object_name1 = R"doc(name of the first object)doc"; static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_object_name2 = R"doc(name of the second object)doc"; static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_res = R"doc(the fcl CollisionResult)doc"; -static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_max_penetration = R"doc(max penetration depth between the two objects)doc"; - 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 = diff --git a/pybind/docstring/collision_detection/fcl/collision_common.h b/pybind/docstring/collision_detection/fcl/collision_common.h index 904091c2..25f2eb57 100644 --- a/pybind/docstring/collision_detection/fcl/collision_common.h +++ b/pybind/docstring/collision_detection/fcl/collision_common.h @@ -53,12 +53,14 @@ static const char *__doc_mplib_collision_detection_fcl_FCLObject_name = R"doc(Na static const char *__doc_mplib_collision_detection_fcl_FCLObject_pose = R"doc(Pose of this FCLObject. All shapes are relative to this pose)doc"; +static const char *__doc_mplib_collision_detection_fcl_FCLObject_setPose = +R"doc( +)doc"; + static const char *__doc_mplib_collision_detection_fcl_FCLObject_shape_poses = R"doc(Relative poses from this FCLObject to each collision shape)doc"; static const char *__doc_mplib_collision_detection_fcl_FCLObject_shapes = R"doc(All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject)doc"; -static const char *__doc_mplib_collision_detection_fcl_FCLObject_setPose = R"doc(Update the pose of this FCLObject)doc"; - static const char *__doc_mplib_collision_detection_fcl_collide = R"doc( Collision function between two ``FCLObject`` diff --git a/pybind/docstring/core/articulated_model.h b/pybind/docstring/core/articulated_model.h index 6077e97c..5bad49a8 100644 --- a/pybind/docstring/core/articulated_model.h +++ b/pybind/docstring/core/articulated_model.h @@ -76,6 +76,10 @@ Get the underlying FCL model. :return: FCL model used for collision checking)doc"; +static const char *__doc_mplib_ArticulatedModelTpl_getLinkGlobalPose = +R"doc( +)doc"; + static const char *__doc_mplib_ArticulatedModelTpl_getMoveGroupEndEffectors = R"doc( Get the end effectors of the move group. diff --git a/pybind/docstring/planning_world.h b/pybind/docstring/planning_world.h index 56743f38..4950c8bd 100644 --- a/pybind/docstring/planning_world.h +++ b/pybind/docstring/planning_world.h @@ -195,6 +195,14 @@ 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_checkArticulationArticulationCollision = +R"doc( +)doc"; + +static const char *__doc_mplib_PlanningWorldTpl_checkArticulationObjectCollision = +R"doc( +)doc"; + static const char *__doc_mplib_PlanningWorldTpl_checkCollision = R"doc( Check full collision (calls ``checkSelfCollision()`` and @@ -203,44 +211,44 @@ Check full collision (calls ``checkSelfCollision()`` and :param request: collision request params. :return: List of ``WorldCollisionResult`` objects)doc"; -static const char *__doc_mplib_PlanningWorldTpl_checkRobotCollision = +static const char *__doc_mplib_PlanningWorldTpl_checkGeneralObjectCollision = R"doc( -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"; +)doc"; -static const char *__doc_mplib_PlanningWorldTpl_checkSelfCollision = +static const char *__doc_mplib_PlanningWorldTpl_checkGeneralObjectPairCollision = R"doc( -Check for self collision (including planned articulation self-collision, planned -articulation-attach collision, attach-attach collision) +Check collision between two specified objects. +:param name1: name of the first object +:param name2: name of the second object :param request: collision request params. :return: List of ``WorldCollisionResult`` objects)doc"; -static const char *__doc_mplib_PlanningWorldTpl_checkSceneCollision = +static const char *__doc_mplib_PlanningWorldTpl_checkObjectObjectCollision = R"doc( -Check collision with scene objects (non-articulated objects) +)doc"; + +static const char *__doc_mplib_PlanningWorldTpl_checkRobotCollision = +R"doc( +Check collision with other scene bodies in the world (planned articulations with +attached objects collide against unplanned articulations and scene objects) -:param scene_object_names: names of scene objects to check collision against. :param request: collision request params. :return: List of ``WorldCollisionResult`` objects)doc"; -static const char *__doc_mplib_PlanningWorldTpl_checkGeneralObjectCollision = - R"doc( -Check collision between an object and the world. +static const char *__doc_mplib_PlanningWorldTpl_checkSceneCollision = +R"doc( +Check full collisions between all pairs of scene objects -:param name: name of the object +:param scene_object_names: list of scene object names :param request: collision request params. :return: List of ``WorldCollisionResult`` objects)doc"; -static const char *__doc_mplib_PlanningWorldTpl_checkGeneralObjectPairCollision = - R"doc( -Check collision between two specified objects. +static const char *__doc_mplib_PlanningWorldTpl_checkSelfCollision = +R"doc( +Check for self collision (including planned articulation self-collision, planned +articulation-attach collision, attach-attach collision) -:param name1: name of the first object -:param name2: name of the second object :param request: collision request params. :return: List of ``WorldCollisionResult`` objects)doc"; @@ -277,18 +285,14 @@ Compute the minimum distance-to-collision between a robot and the world :param request: distance request params. :return: a ``WorldDistanceResult`` object)doc"; -static const char *__doc_mplib_PlanningWorldTpl_distanceSelf = +static const char *__doc_mplib_PlanningWorldTpl_distanceScene = R"doc( -Get the minimum distance to self-collision given the robot in current state +)doc"; -:param request: distance request params. -:return: a ``WorldDistanceResult`` object)doc"; - -static const char *__doc_mplib_PlanningWorldTpl_distanceScene = +static const char *__doc_mplib_PlanningWorldTpl_distanceSelf = R"doc( -Compute the minimum distance-to-all-collision between a lsit of objects +Get the minimum distance to self-collision given the robot in current state -:param scene_object_names: list of scene object names :param request: distance request params. :return: a ``WorldDistanceResult`` object)doc"; diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 356dd6d3..9a29d50e 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -13,7 +13,7 @@ S getFCLContactMaxPenetration(const fcl::CollisionResult &result) { S max_penetration = std::numeric_limits::min(); if (result.numContacts() == 0) return 1; - for (int i = 0; i < result.numContacts(); ++i) { + for (size_t i = 0; i < result.numContacts(); ++i) { const auto &contact = result.getContact(i); max_penetration = std::max(max_penetration, contact.penetration_depth); } @@ -549,8 +549,8 @@ std::vector> PlanningWorldTpl::checkSceneCollision if (auto it = object_map_.find(name); it != object_map_.end()) scene_objects.push_back(it->second); } - for (int i = 0; i < scene_object_names.size(); i++) { - for (int j = i + 1; j < scene_object_names.size(); j++) { + for (size_t i = 0; i < scene_object_names.size(); i++) { + for (size_t j = i + 1; j < scene_object_names.size(); j++) { if (auto type = acm_->getAllowedCollision(scene_object_names[i], scene_object_names[j]); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear(); @@ -719,8 +719,8 @@ std::vector> PlanningWorldTpl::distanceScene( if (auto it = object_map_.find(name); it != object_map_.end()) scene_objects.push_back(it->second); } - for (int i = 0; i < scene_object_names.size(); i++) { - for (int j = i + 1; j < scene_object_names.size(); j++) { + for (size_t i = 0; i < scene_object_names.size(); i++) { + for (size_t j = i + 1; j < scene_object_names.size(); j++) { if (auto type = acm_->getAllowedCollision(scene_object_names[i], scene_object_names[j]); !type || type == collision_detection::AllowedCollision::NEVER) { result.clear();