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 77f59d32..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
@@ -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 && \
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<S> 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<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),
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<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_);
 };
 
 /**
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<S> 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 4a71f500..491adbc1 100644
--- a/include/mplib/core/attached_body.h
+++ b/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
diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h
index ddf91496..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<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_;
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<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(
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 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>>(
diff --git a/pybind/docstring/collision_detection/collision_common.h b/pybind/docstring/collision_detection/collision_common.h
index e2f3f4d9..320128f8 100644
--- a/pybind/docstring/collision_detection/collision_common.h
+++ b/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";
diff --git a/pybind/docstring/collision_detection/fcl/collision_common.h b/pybind/docstring/collision_detection/fcl/collision_common.h
index 2e67fdb6..25f2eb57 100644
--- a/pybind/docstring/collision_detection/fcl/collision_common.h
+++ b/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";
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 10238055..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,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
diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp
index a0684b30..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<S>(),
            DOC(mplib, PlanningWorldTpl, addPointCloud))
       .def("remove_object", &PlanningWorld::removeObject, py::arg("name"),
            DOC(mplib, PlanningWorldTpl, removeObject))
@@ -152,6 +153,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 +179,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..d828ea18 100644
--- a/src/collision_detection/fcl/collision_common.cpp
+++ b/src/collision_detection/fcl/collision_common.cpp
@@ -31,6 +31,14 @@ FCLObject<S>::FCLObject(const std::string &name_, const Pose<S> &pose_,
   }
 }
 
+template <typename S>
+void FCLObject<S>::setPose(const Pose<S> &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]);
+}
+
 template <typename S>
 size_t collide(const FCLObjectPtr<S> &obj1, const FCLObjectPtr<S> &obj2,
                const fcl::CollisionRequest<S> &request,
diff --git a/src/planning_world.cpp b/src/planning_world.cpp
index 3b7283d9..9a29d50e 100644
--- a/src/planning_world.cpp
+++ b/src/planning_world.cpp
@@ -8,11 +8,17 @@
 
 namespace mplib {
 
-// Explicit Template Instantiation Definition ==========================================
-#define DEFINE_TEMPLATE_PLANNING_WORLD(S) template class PlanningWorldTpl<S>
+template <typename S>
+S getFCLContactMaxPenetration(const fcl::CollisionResult<S> &result) {
+  S max_penetration = std::numeric_limits<S>::min();
+  if (result.numContacts() == 0) return 1;
 
-DEFINE_TEMPLATE_PLANNING_WORLD(float);
-DEFINE_TEMPLATE_PLANNING_WORLD(double);
+  for (size_t 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 <typename S>
 PlanningWorldTpl<S>::PlanningWorldTpl(
@@ -88,12 +94,16 @@ void PlanningWorldTpl<S>::addObject(const std::string &name,
 template <typename S>
 void PlanningWorldTpl<S>::addPointCloud(const std::string &name,
                                         const MatrixX3<S> &vertices,
-                                        double resolution) {
+                                        double resolution,
+                                        const Pose<S> &pose) {
   auto tree = std::make_shared<octomap::OcTree>(resolution);
   for (const auto &row : vertices.rowwise())
     tree->updateNode(octomap::point3d(row(0), row(1), row(2)), true);
-  addObject(name,
-            std::make_shared<CollisionObject>(std::make_shared<fcl::OcTree<S>>(tree)));
+  auto cobject = std::make_shared<CollisionObject>(std::make_shared<fcl::OcTree<S>>(tree));
+  addObject(
+    std::make_shared<FCLObject>(
+      name, pose, std::vector<CollisionObjectPtr> {cobject}, std::vector<Pose<S>> {Pose<S>()}
+  ));
 }
 
 template <typename S>
@@ -113,7 +123,7 @@ void PlanningWorldTpl<S>::attachObject(const std::string &name,
                                        const std::vector<std::string> &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);
+      planned_articulation_map_.at(art_name)->getLinkGlobalPose(link_id);
   attachObject(name, art_name, link_id, Pose<S>(T_world_link.inverse() * T_world_obj),
                touch_links);
 }
@@ -123,7 +133,7 @@ void PlanningWorldTpl<S>::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);
+      planned_articulation_map_.at(art_name)->getLinkGlobalPose(link_id);
   attachObject(name, art_name, link_id, Pose<S>(T_world_link.inverse() * T_world_obj));
 }
 
@@ -230,6 +240,26 @@ void PlanningWorldTpl<S>::attachMesh(const std::string &mesh_path,
         art_name, link_id, pose);
 }
 
+/*
+template <typename S>
+void PlanningWorldTpl<S>::attachPointCloud(
+  const std::string &name,
+  const MatrixX3<S> &vertices,
+  double resolution,
+  const std::string &art_name,
+  int link_id,
+  const Pose<S> &pose) {
+  auto tree = std::make_shared<octomap::OcTree>(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<CollisionObject>(std::make_shared<fcl::OcTree<S>>(tree));
+  addObject(name, obj);
+  attachObject(name, art_name, link_id, pose);
+}
+*/
+
 template <typename S>
 bool PlanningWorldTpl<S>::detachObject(const std::string &name, bool also_remove) {
   if (also_remove) {
@@ -334,6 +364,7 @@ std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::checkSelfCollision(
           tmp.object_name2 = name2;
           tmp.link_name1 = name1;
           tmp.link_name2 = name2;
+          tmp.max_penetration = getFCLContactMaxPenetration<S>(result);
           ret.push_back(tmp);
         }
       }
@@ -341,6 +372,92 @@ std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::checkSelfCollision(
   return ret;
 }
 
+template <typename S>
+std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::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 <typename S>
+std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::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 <typename S>
+std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::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<WorldCollisionResultTpl<S>> 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<S>(result);
+      results.push_back(tmp);
+    }
+  }
+  return results;
+}
+
+template <typename S>
+std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::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<WorldCollisionResult> ret;
+  return ret;
+}
+
+template <typename S>
+std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::checkGeneralObjectCollision(
+  const std::string &name, const CollisionRequest &request) const {
+
+  std::vector<WorldCollisionResult> 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 <typename S>
 std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::checkRobotCollision(
     const CollisionRequest &request) const {
@@ -402,6 +519,7 @@ std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::checkRobotCollision
           tmp.object_name2 = scene_obj->name;
           tmp.link_name1 = attached_body_name;
           tmp.link_name2 = scene_obj->name;
+          tmp.max_penetration = getFCLContactMaxPenetration<S>(result);
           ret.push_back(tmp);
         }
       }
@@ -418,6 +536,40 @@ std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::checkCollision(
   return ret1;
 }
 
+template <typename S>
+std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::checkSceneCollision(
+  const std::vector<std::string> &scene_object_names,
+  const CollisionRequest &request) const {
+
+  std::vector<WorldCollisionResult> ret;
+  CollisionResult result;
+
+  std::vector<FCLObjectPtr> 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 (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();
+        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<S>(result);
+          ret.push_back(tmp);
+        }
+      }
+    }
+  }
+  return ret;
+}
+
 template <typename S>
 WorldDistanceResultTpl<S> PlanningWorldTpl<S>::distanceSelf(
     const DistanceRequest &request) const {
@@ -553,4 +705,42 @@ WorldDistanceResultTpl<S> PlanningWorldTpl<S>::distance(
   return ret1.min_distance < ret2.min_distance ? ret1 : ret2;
 }
 
+template <typename S>
+std::vector<WorldDistanceResultTpl<S>> PlanningWorldTpl<S>::distanceScene(
+    const std::vector<std::string> &scene_object_names,
+    const DistanceRequest &request) const {
+
+    std::vector<WorldDistanceResult> ret_list;
+    WorldDistanceResult ret;
+    DistanceResult result;
+
+    std::vector<FCLObjectPtr> 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 (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();
+                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<S>
+
+DEFINE_TEMPLATE_PLANNING_WORLD(float);
+DEFINE_TEMPLATE_PLANNING_WORLD(double);
+
 }  // namespace mplib