Support multiple attachment frames

CMakeLists.txt

@@ -7,10 +7,11 @@ option(VAMP_FORCE_CLANG "Force the use of Clang." OFF)
 option(VAMP_BUILD_PYTHON_BINDINGS "Build VAMP Python bindings" ON)
 option(VAMP_INSTALL_CPP_LIBRARY "Install VAMP C++ library (disable for Python wheel builds)" ON)
 
-option(VAMP_BUILD_CPP_DEMO "Build VAMP C++ Demo Scripts" OFF)
+option(VAMP_BUILD_CPP_DEMO "Build VAMP C++ Demo Scripts" ON)
 option(VAMP_BUILD_OMPL_DEMO "Build VAMP C++ OMPL Integration Demo Scripts" OFF)
 option(VAMP_OMPL_PATH "Search Path for OMPL Installation - Only Needed for Demo Script" "")
 
+
 if(VAMP_FORCE_CLANG)
   find_program(CLANG "clang")
   find_program(CLANGPP "clang++")

src/impl/vamp/bindings/environment.cc

@@ -161,8 +161,8 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule)
             })
         .def(
             "attach",
-            [](vc::Environment<float> &e, const vc::Attachment<float> &a) { e.attachments.emplace(a); })
-        .def("detach", [](vc::Environment<float> &e) { e.attachments.reset(); });
+            [](vc::Environment<float> &e, const vc::Attachment<float> &a, const size_t eef_id = 0) { e.attachments.emplace(eef_id, a); })
+        .def("detach", [](vc::Environment<float> &e, const size_t eef_id = 0) { e.attachments.erase(eef_id); });
 
     pymodule.def(
         "filter_pointcloud",

src/impl/vamp/bindings/robot_helper.hh

@@ -276,9 +276,12 @@ namespace vamp::binding
                 path, EnvironmentVector(environment), settings, rng);
         }
 
-        inline static auto eefk(const Type &start) -> Eigen::Matrix4f
+        inline static auto eefk(const Type &start) -> std::vector<Eigen::Matrix4f>
         {
-            return Robot::eefk(Input::array(start)).matrix();
+            std::vector<Eigen::Matrix4f> eefk_m;
+            for(const auto &fk : Robot::eefk(Input::array(start)))
+                eefk_m.push_back(fk.matrix());
+            return eefk_m;
         }
 
         inline static auto filter_self_from_pointcloud(
@@ -355,7 +358,7 @@ namespace vamp::binding
             "Minimum and maximum radii sizes of robot spheres.");
         submodule.def(
             "joint_names", []() { return Robot::joint_names; }, "Joint names for the robot in order of DoF");
-        submodule.def("end_effector", []() { return Robot::end_effector; }, "End-effector frame name.");
+        submodule.def("end_effectors", []() { return Robot::end_effectors; }, "End-effector frame names.");
 
         using RNG = vamp::rng::RNG<Robot>;
         nb::class_<typename RNG::Ptr>(submodule, "RNG", "RNG for robot configurations.")

src/impl/vamp/collision/environment.hh

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <map>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
 
@@ -22,7 +23,7 @@ namespace vamp::collision
         std::vector<Cuboid<DataT>> z_aligned_cuboids;
         std::vector<HeightField<DataT>> heightfields;
         std::vector<CAPT> pointclouds;
-        std::optional<Attachment<DataT>> attachments;
+        std::map<size_t, Attachment<DataT>> attachments; // eef_id to attachment
 
         Environment() = default;
 
@@ -36,7 +37,7 @@ namespace vamp::collision
           , z_aligned_cuboids(other.z_aligned_cuboids.begin(), other.z_aligned_cuboids.end())
           , heightfields(other.heightfields.begin(), other.heightfields.end())
           , pointclouds(other.pointclouds.begin(), other.pointclouds.end())
-          , attachments(other.template clone_attachments<DataT>())
+          , attachments(other.attachments.begin(), other.attachments.end())
         {
         }
 
@@ -71,25 +72,18 @@ namespace vamp::collision
     private:
         template <typename OtherDataT>
         friend struct Environment;
-
-        template <typename OtherDataT>
-        inline auto clone_attachments() const noexcept -> std::optional<Attachment<OtherDataT>>
-        {
-            if (attachments)
-            {
-                return Attachment<OtherDataT>(*attachments);
-            }
-
-            return std::nullopt;
-        }
     };
 
-    template <typename DataT>
+    template <size_t N, typename DataT>
     inline auto set_attachment_pose(
         const Environment<DataT> &e,
-        const Eigen::Transform<DataT, 3, Eigen::Isometry> &p_tf) noexcept
+        const std::array<Eigen::Transform<DataT, 3, Eigen::Isometry>, N> &p_tfs
+    ) noexcept
     {
-        e.attachments->pose(p_tf);
+        for(auto i=0U; i < N; i++){
+            if(e.attachments.find(i) != e.attachments.end())
+                e.attachments.at(i).pose(p_tfs[i]);
+        }
     }
 
 }  // namespace vamp::collision

src/impl/vamp/collision/validity.hh

@@ -252,18 +252,39 @@ namespace vamp
     inline constexpr auto attachment_environment_collision(const collision::Environment<DataT> &e) noexcept
         -> bool
     {
-        for (const auto &s : e.attachments->posed_spheres)
-        {
-            // HACK: The radius needs to be a float, and unfortunately the spheres assume homogeneous
-            // DataT for storage
-            // TODO: Fix the sphere representation to allow to store float radii even with vector
-            // centers
-            if (sphere_environment_in_collision(e, s.x, s.y, s.z, s.r[{0, 0}]))
+        for (const auto &attachment: e.attachments){
+            for (const auto &s : attachment.second.posed_spheres)
             {
-                return true;
+                // HACK: The radius needs to be a float, and unfortunately the spheres assume homogeneous
+                // DataT for storage
+                // TODO: Fix the sphere representation to allow to store float radii even with vector
+                // centers
+                if (sphere_environment_in_collision(e, s.x, s.y, s.z, s.r[{0, 0}]))
+                {
+                    return true;
+                }
             }
+
+        // now treat the other attachments also as potential collision objects
+        // This is probably expensive, find a more efficient way. 
+            for (const auto &other_attachment: e.attachments){
+                if(attachment.first != other_attachment.first) { // different obj
+                    for (const auto &s : attachment.second.posed_spheres) {
+                        for(const auto &s2 : other_attachment.second.posed_spheres) {
+                            if (not collision::sphere_sphere_sql2(s, s2).test_zero())
+                            {
+                                return true;
+                            }
+                        }
+                    }
+                }
+            }
+
         }
 
+
+
+
         return false;
     }
 
@@ -280,15 +301,17 @@ namespace vamp
         auto sy = static_cast<DataT>(sy_);
         auto sz = static_cast<DataT>(sz_);
         auto sr = static_cast<DataT>(sr_);
-        for (const auto &att_s : e.attachments->posed_spheres)
+        for (const auto &attachment: e.attachments)
         {
-            if (not collision::sphere_sphere_sql2(sx, sy, sz, sr, att_s.x, att_s.y, att_s.z, att_s.r)
-                        .test_zero())
+            for (const auto &att_s : attachment.second.posed_spheres)
             {
-                return true;
+                if (not collision::sphere_sphere_sql2(sx, sy, sz, sr, att_s.x, att_s.y, att_s.z, att_s.r)
+                            .test_zero())
+                {
+                    return true;
+                }
             }
         }
-
         return false;
     }
 }  // namespace vamp

src/impl/vamp/planning/validate.hh

@@ -40,7 +40,7 @@ namespace vamp::planning
 
         const std::size_t n = std::max(std::ceil(distance / static_cast<float>(rake) * resolution), 1.F);
 
-        bool valid = (environment.attachments) ? Robot::template fkcc_attach<rake>(environment, block) :
+        bool valid = (environment.attachments.size()) ? Robot::template fkcc_attach<rake>(environment, block) :
                                                  Robot::template fkcc<rake>(environment, block);
         if (not valid or n == 1)
         {
@@ -55,7 +55,7 @@ namespace vamp::planning
                 block[j] = block[j] - backstep.broadcast(j);
             }
 
-            bool valid = (environment.attachments) ? Robot::template fkcc_attach<rake>(environment, block) :
+            bool valid = (environment.attachments.size()) ? Robot::template fkcc_attach<rake>(environment, block) :
                                                      Robot::template fkcc<rake>(environment, block);
             if (not valid)
             {