bugfix for attach object when articulation base pose is non-zero; upd…

…ate stale link in dockerfile

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 && \

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<S> 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

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

src/planning_world.cpp

@@ -112,8 +112,12 @@ void PlanningWorldTpl<S>::attachObject(const std::string &name,
                                        const std::string &art_name, int link_id,
                                        const std::vector<std::string> &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<S>(T_world_link.inverse() * T_world_obj),
                touch_links);
 }
@@ -122,8 +126,12 @@ template <typename S>
 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 =
+  // 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<S>(T_world_link.inverse() * T_world_obj));
 }