add halfspace collision

Author: alaurenzi

CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.5)
 
-project(xbot2_interface LANGUAGES CXX VERSION 1.0.0)
+project(xbot2_interface LANGUAGES CXX VERSION 1.1.0)
 
 option(XBOT2_IFC_BUILD_PINOCCHIO "Build Pinocchio implementation" ON)
 option(XBOT2_IFC_BUILD_RBDL "Build RBDL implementation" OFF)

include/xbot2_interface/collision.h

@@ -28,6 +28,12 @@ struct XBOT2IFC_API Shape
         double length;
     };
 
+    struct Halfspace 
+    {
+        Eigen::Vector3d normal;
+        double d;
+    };
+
     struct Box
     {
         Eigen::Vector3d size;
@@ -63,6 +69,7 @@ struct XBOT2IFC_API Shape
         Capsule,
         Box,
         Cylinder,
+        Halfspace,
         Mesh,
         Octree,
         HeightMap

src/collision/collision.cpp

@@ -384,10 +384,16 @@ void CollisionModel::Impl::updateCollisionPairData()
         auto c = _link_collision_map.at(l);
         auto env = _env_collision;
 
+        
+
         for(int i = 0; i < c->coll_obj.size(); i++)
         {
             for(int j = 0; j < env->coll_obj.size(); j++)
             {
+                if(env->disabled_collisions[j].contains(l) || c->disabled_collisions[i].contains("world"))
+                {
+                    continue;
+                }
 
                 CollisionPairData cpd(c->coll_obj[i], env->coll_obj[j]);
 
@@ -544,6 +550,17 @@ bool CollisionModel::Impl::addCollisionShape(string_const_ref name,
 
             return true;
         },
+        [&](const Shape::Halfspace& hs)
+        {
+            fcl_geom = std::make_shared<hpp::fcl::Halfspace>(
+                hs.normal,
+                hs.d
+                );
+
+            std::cout << "halfspace";
+
+            return true;
+        },
         [&](const Shape::HeightMap& caps)
         {
             throw std::runtime_error("heightmap not implemented");
@@ -651,6 +668,17 @@ bool CollisionModel::Impl::addCollisionShape(string_const_ref name,
     auto fcl_obj = std::make_shared<fcl::CollisionObject>(fcl_geom);
     link_collision->addCollisionObject(fcl_obj, link_T_shape);
 
+    // check disabled collisions are valid link names
+    for(auto& dc : disabled_collisions)
+    {
+        if(!_link_collision_map.contains(dc))
+        {
+            throw std::invalid_argument(
+                fmt::format("disabled collision '{}' is not a valid collision link name",
+                            dc));
+        }
+    }
+
     // add disabled collisions
     link_collision->disabled_collisions.back().insert(disabled_collisions.begin(),
                                                       disabled_collisions.end());