[C++][unittest] Added unittest geom.cpp . It tests that we obtain the same...

[C++][unittest] Added unittest geom.cpp . It tests that we obtain the same results between Pinocchio and hpp-model-urdf for joints and geometry objects positions ; for collisions and distances between two geometry objects

src/multibody/parser/sample-models.cpp

@@ -18,6 +18,10 @@
 
 #include "pinocchio/multibody/parser/sample-models.hpp"
 
+#ifdef WITH_HPP_FCL
+#include <hpp/fcl/shape/geometric_shapes.h>
+#endif
+
 namespace se3
 {
   namespace buildModels
@@ -193,5 +197,24 @@ namespace se3
                     "larm6_joint", "larm6_body");
     }
 
+    #ifdef WITH_HPP_FCL
+    void collisionModel( Model& model, GeometryModel& model_geom)
+    {
+      model.addBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+                    "planar1_joint", "planar1_body");
+      model.addBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+                    "planar2_joint", "planar2_body");
+
+      boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
+      fcl::CollisionObject box1(Sample, fcl::Transform3f());
+      model_geom.addGeomObject(model.getJointId("planar1_joint"),box1, SE3::Identity(),  "ff1_collision_object"); 
+
+      boost::shared_ptr<fcl::Box> Sample2(new fcl::Box(1));
+      fcl::CollisionObject box2(Sample, fcl::Transform3f());
+      model_geom.addGeomObject(model.getJointId("planar2_joint"),box2, SE3::Identity(),  "ff2_collision_object");
+
+    }
+    #endif
+
   } // namespace buildModels
 } // namespace se3

src/multibody/parser/sample-models.hpp

@@ -21,6 +21,10 @@
 
 #include "pinocchio/multibody/model.hpp"
 
+#ifdef WITH_HPP_FCL
+#include "pinocchio/multibody/geometry.hpp"
+#endif
+
 namespace se3
 {
   namespace buildModels
@@ -30,6 +34,10 @@ namespace se3
 
     void humanoidSimple(Model& model, bool usingFF = true);
 
+    #ifdef WITH_HPP_FCL
+    void collisionModel( Model& model, GeometryModel& geom);
+    #endif
+
   } // namespace buildModels
 } // namespace se3
 

src/multibody/parser/urdf-with-geometry.hpp

@@ -45,39 +45,6 @@ namespace se3
 {
   namespace urdf
   {
-    Inertia convertFromUrdf( const ::urdf::Inertial& Y )
-    {
-      const ::urdf::Vector3 & p = Y.origin.position;
-      const ::urdf::Rotation & q = Y.origin.rotation;
-
-      const Eigen::Vector3d com(p.x,p.y,p.z);
-      const Eigen::Matrix3d & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();
-
-      Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
-			   ,  Y.ixy,Y.iyy,Y.iyz
-			   ,  Y.ixz,Y.iyz,Y.izz;
-      return Inertia(Y.mass,com,R*I*R.transpose());
-    }
-
-    SE3 convertFromUrdf( const ::urdf::Pose & M )
-    {
-      const ::urdf::Vector3 & p = M.position;
-      const ::urdf::Rotation & q = M.rotation;
-      return SE3( Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix(), Eigen::Vector3d(p.x,p.y,p.z));
-    }
-
-    enum AxisCartesian { AXIS_X, AXIS_Y, AXIS_Z, AXIS_UNALIGNED };
-    AxisCartesian extractCartesianAxis( const ::urdf::Vector3 & axis )
-    {
-      if( (axis.x==1.0)&&(axis.y==0.0)&&(axis.z==0.0) )
-	return AXIS_X;
-      else if( (axis.x==0.0)&&(axis.y==1.0)&&(axis.z==0.0) )
-	return AXIS_Y;
-      else if( (axis.x==0.0)&&(axis.y==0.0)&&(axis.z==1.0) )
-	return AXIS_Z;
-      else
-	return AXIS_UNALIGNED;
-    }
 
 
 fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, std::string meshRootDir)
@@ -127,29 +94,25 @@ fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & lin
   //   std::cout << "#" << link->parent_joint->name << std::endl;
   // else std::cout << "###ROOT" << std::endl;
 
+
+  //assert(link->inertial && "The parser cannot accept trivial mass");
+  const Inertia & Y = (link->inertial) ?  convertFromUrdf(*link->inertial) :
+                                          Inertia::Identity();
+
   // std::cout << "placementOffset: " << placementOffset << std::endl;
 
+  bool visual = (link->visual) ? true : false;
+  // bool collision = (link->collision) ? true  : false;
+
   if(joint!=NULL)
   {
     assert(link->getParent()!=NULL);
-    
-    if (!link->inertial && joint->type != ::urdf::Joint::FIXED)
-    {
-      const std::string exception_message (link->name + " - spatial inertia information missing.");
-      throw std::invalid_argument(exception_message);
-    }
 
     Model::Index parent = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
                                                                     model.getJointId( link->getParent()->parent_joint->name );
     //std::cout << joint->name << " === " << parent << std::endl;
 
     const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
-    
-    const Inertia & Y = (link->inertial) ?  convertFromUrdf(*link->inertial) :
-    Inertia::Identity();
-    
-    bool visual = (link->visual) ? true : false;
-
 
     //std::cout << "Parent = " << parent << std::endl;
     //std::cout << "Placement = " << (Matrix4)jointPlacement << std::endl;
@@ -250,18 +213,13 @@ fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & lin
       }
       case ::urdf::Joint::FIXED:
       {
-        // In case of fixed joint, if link has inertial tag:
+        // In case of fixed join:
         //    -add the inertia of the link to his parent in the model
-        // Otherwise do nothing.
-        // In all cases:
-        //    -let all the children become children of parent
+        //    -let all the children become children of parent 
         //    -inform the parser of the offset to apply
         //    -add fixed body in model to display it in gepetto-viewer
-        if (link->inertial)
-        {
-          model.mergeFixedBody(parent, jointPlacement, Y); //Modify the parent inertia in the model
-        }
 
+        model.mergeFixedBody(parent, jointPlacement, Y); //Modify the parent inertia in the model
         SE3 ptjot_se3 = convertFromUrdf(link->parent_joint->parent_to_joint_origin_transform);
 
         //transformation of the current placement offset
@@ -270,7 +228,7 @@ fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & lin
         //add the fixed Body in the model for the viewer
         model.addFixedBody(parent,nextPlacementOffset,link->name,visual);
 
-        BOOST_FOREACH(::urdf::LinkPtr child_link,link->child_links)
+        BOOST_FOREACH(::urdf::LinkPtr child_link,link->child_links) 
         {
           child_link->setParent(link->getParent() );  //skip the fixed generation
         }
@@ -291,16 +249,11 @@ fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & lin
       model_geom.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
     }      
   }
-          else if (link->getParent() != NULL)
-          {
-            const std::string exception_message (link->name + " - joint information missing.");
-            throw std::invalid_argument(exception_message);
-          }
 
 
   BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
   {
-    parseTree(child, model, nextPlacementOffset);
+    parseTreeWithGeom(child, model, model_geom, meshRootDir, nextPlacementOffset);
   }
 }
 

unittest/CMakeLists.txt

@@ -61,6 +61,15 @@ IF(URDFDOM_FOUND)
 
   ADD_UNIT_TEST(value "eigen3;urdfdom")
   ADD_TEST_CFLAGS(value '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+  IF(HPP_FCL_FOUND)
+    ADD_UNIT_TEST(geom "eigen3;urdfdom;assimp;hpp-fcl")
+    ADD_TEST_CFLAGS(geom '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+    ADD_TEST_CFLAGS(geom "-DWITH_HPP_FCL")
+    IF(HPP_MODEL_URDF_FOUND)
+      PKG_CONFIG_USE_DEPENDENCY(geom hpp-model-urdf)
+      ADD_TEST_CFLAGS(geom "-DWITH_HPP_MODEL_URDF")
+    ENDIF(HPP_MODEL_URDF_FOUND)
+  ENDIF(HPP_FCL_FOUND)
 ENDIF(URDFDOM_FOUND)
 
 IF(LUA5_1_FOUND)