bugfix for attach object when articulation base pose is non-zero; update stale link in dockerfile

.gitignore

@@ -5,6 +5,7 @@ _ext
 *.so
 build/
 dist/
+repaired_wheels/
 eggs/
 .eggs/
 *.egg-info/

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.

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
@@ -23,7 +31,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 && \

include/mplib/collision_detection/collision_common.h

@@ -22,20 +22,24 @@ 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<S> res;  ///< the fcl CollisionResult
   std::string collision_type,     ///< type of the collision
       object_name1,               ///< name of the first object
       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<S> &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),

include/mplib/collision_detection/fcl/collision_common.h

@@ -53,6 +53,8 @@ struct FCLObject {
   std::vector<fcl::CollisionObjectPtr<S>> shapes;
   /// Relative poses from this FCLObject to each collision shape
   std::vector<Isometry3<S>> shape_poses;
+
+  void setPose(const Pose<S> &pose_);
 };
 
 /**

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<S> getLinkGlobalPose(size_t link_index) const {
+    return base_pose_ * pinocchio_model_->getLinkPose(link_index);
+  }
+
   /**
    * Update the SRDF file to disable self-collisions.
    *

include/mplib/core/attached_body.h

@@ -65,7 +65,7 @@ class AttachedBodyTpl {
 
   /// @brief Gets the global pose of the articulation link that this body is attached to
   Isometry3<S> getAttachedLinkGlobalPose() const {
-    return pinocchio_model_->getLinkPose(attached_link_id_);
+    return attached_articulation_->getLinkGlobalPose(attached_link_id_);
   }
 
   /// @brief Gets the global pose of the attached object

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<S> &vertices,
-                     double resolution = 0.01);
+                     double resolution = 0.01, const Pose<S> &pose = Pose<S>());
 
   /**
    * Removes (and detaches) the collision object with given name if exists.
@@ -435,6 +435,51 @@ class PlanningWorldTpl {
   std::vector<WorldCollisionResult> 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<WorldCollisionResult> checkSceneCollision(
+      const std::vector<std::string> &scene_object_names,
+      const CollisionRequest &request = CollisionRequest()) const;
+
+  std::vector<WorldCollisionResult> checkArticulationArticulationCollision(
+    const ArticulatedModelPtr &art1, const ArticulatedModelPtr &art2,
+    const CollisionRequest &request) const;
+
+  std::vector<WorldCollisionResult> checkArticulationObjectCollision(
+    const ArticulatedModelPtr &art, const FCLObjectPtr &obj,
+    const CollisionRequest &request) const;
+
+  std::vector<WorldCollisionResult> 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<WorldCollisionResult> 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<WorldCollisionResult> 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<WorldDistanceResult> distanceScene(
+    const std::vector<std::string> &scene_object_names,
+    const DistanceRequest &request = DistanceRequest()) const;
+
+
  private:
   std::unordered_map<std::string, ArticulatedModelPtr> articulation_map_;
   std::unordered_map<std::string, FCLObjectPtr> object_map_;

mplib/__init__.py

@@ -8,5 +8,6 @@
     Pose,
     set_global_seed,
 )
+from .collision_detection.fcl import FCLObject
 
 __version__ = version("mplib")

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<S>(pose));
             return ret;
           },
-          DOC(mplib, collision_detection, fcl, FCLObject, shape_poses));
+          DOC(mplib, collision_detection, fcl, FCLObject, shape_poses))
+      .def("set_pose", &FCLObject<S>::setPose, py::arg("pose"),
+          DOC(mplib, collision_detection, fcl, FCLObject, setPose));
 
   // collide / distance functions
   m.def(

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 CollisionResult &, const std::string &, const std::string &,
-                    const std::string &, const std::string &, const std::string &>(),
+                    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_<WorldDistanceResult, std::shared_ptr<WorldDistanceResult>>(

pybind/docstring/collision_detection/collision_common.h

@@ -40,6 +40,8 @@ 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";

pybind/docstring/collision_detection/fcl/collision_common.h

@@ -53,6 +53,10 @@ 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";

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.

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,6 +211,23 @@ Check full collision (calls ``checkSelfCollision()`` and
 :param request: collision request params.
 :return: List of ``WorldCollisionResult`` objects)doc";
 
+static const char *__doc_mplib_PlanningWorldTpl_checkGeneralObjectCollision =
+R"doc(
+)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_checkObjectObjectCollision =
+R"doc(
+)doc";
+
 static const char *__doc_mplib_PlanningWorldTpl_checkRobotCollision =
 R"doc(
 Check collision with other scene bodies in the world (planned articulations with
@@ -211,6 +236,14 @@ attached objects collide against unplanned articulations and scene objects)
 :param request: collision request params.
 :return: List of ``WorldCollisionResult`` objects)doc";
 
+static const char *__doc_mplib_PlanningWorldTpl_checkSceneCollision =
+R"doc(
+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)doc";
+
 static const char *__doc_mplib_PlanningWorldTpl_checkSelfCollision =
 R"doc(
 Check for self collision (including planned articulation self-collision, planned
@@ -252,6 +285,10 @@ 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_distanceScene =
+R"doc(
+)doc";
+
 static const char *__doc_mplib_PlanningWorldTpl_distanceSelf =
 R"doc(
 Get the minimum distance to self-collision given the robot in current state
@@ -287,14 +324,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 +413,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