multibody/geom: restore behavior with model input in addGeometryObject

src/multibody/geometry.hpp

@@ -70,6 +70,7 @@ namespace se3
 
     /**
      * @brief      Add a geometry object to a GeometryModel
+     * @deprecated This method has been set to deprecated. Please use other signature of addGeometryObject.
      *
      * @param[in]  object     Object 
      * @param[in]  model      Corresponding model, used to assert the attributes of object.
@@ -82,16 +83,34 @@ namespace se3
     PINOCCHIO_DEPRECATED
     GeomIndex addGeometryObject(GeometryObject object,
                                 const ModelTpl<S2,O2,_JointCollectionTpl> & model,
-                                const bool autofillJointParent = false);
+                                const bool autofillJointParent)
+    {
+      if(autofillJointParent)
+        addGeometryObject(object,model);
+      else
+        addGeometryObject(object);
+    }
     
     /**
-     * @brief      Add a geometry object to a GeometryModel.
+     * @brief      Add a geometry object to a GeometryModel and set its parent joint.
      *
      * @param[in]  object     Object
+     * @param[in]  model      Corresponding model, used to assert the attributes of object.
      *
      * @return     The index of the new added GeometryObject in geometryObjects
      * @note object is a nonconst copy to ease the insertion code.
      */
+    template<typename S2, int O2, template<typename,int> class _JointCollectionTpl>
+    GeomIndex addGeometryObject(const GeometryObject & object,
+                                const ModelTpl<S2,O2,_JointCollectionTpl> & model);
+    
+    /**
+     * @brief      Add a geometry object to a GeometryModel.
+     *
+     * @param[in]  object     Object
+     *
+     * @return     The index of the new added GeometryObject in geometryObjects.
+     */
     GeomIndex addGeometryObject(const GeometryObject & object);
 
     /**

src/multibody/geometry.hxx

@@ -50,19 +50,11 @@ namespace se3
 #else
   {}
 #endif // WITH_HPP_FCL   
-
+  
   template<typename S2, int O2, template<typename,int> class JointCollectionTpl>
-  PINOCCHIO_DEPRECATED
-  GeomIndex GeometryModel::addGeometryObject(GeometryObject object,
-                                             const ModelTpl<S2,O2,JointCollectionTpl> & model,
-                                             const bool autofillJointParent)
+  GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object,
+                                             const ModelTpl<S2,O2,JointCollectionTpl> & model)
   {
-    // TODO reenable when relevant: assert( (object.parentFrame != -1) || (object.parentJoint != -1) );
-    
-    if( autofillJointParent )
-      // TODO: this might be automatically done for some default value of parentJoint (eg ==-1)
-      object.parentJoint = model.frames[object.parentFrame].parent;
-    
     assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
            (model.frames[object.parentFrame].type == se3::BODY)  );
     assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
@@ -70,11 +62,12 @@ namespace se3
     
     GeomIndex idx = (GeomIndex) (ngeoms ++);
     geometryObjects.push_back(object);
+    geometryObjects.back().parentJoint = model.frames[object.parentFrame].parent;
     return idx;
   }
   
   inline GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object)
-  {    
+  {
     GeomIndex idx = (GeomIndex) (ngeoms ++);
     geometryObjects.push_back(object);
     return idx;

unittest/geom.cpp

@@ -64,18 +64,21 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
   boost::shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1));
   Model::FrameIndex body_id_1 = model.getBodyId("planar1_body");
   Model::JointIndex joint_parent_1 = model.frames[body_id_1].parent;
-  geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
-                                             model.getBodyId("planar1_body"),joint_parent_1,
-                                             sample,SE3::Identity(), "", Eigen::Vector3d::Ones())
-                              );
+  Model::JointIndex idx_geom1 = geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
+                                                                           model.getBodyId("planar1_body"),joint_parent_1,
+                                                                           sample,SE3::Identity(), "", Eigen::Vector3d::Ones())
+                                                            );
+  geomModel.geometryObjects[idx_geom1].parentJoint = model.frames[body_id_1].parent;
+  
   
   boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1));
   Model::FrameIndex body_id_2 = model.getBodyId("planar2_body");
   Model::JointIndex joint_parent_2 = model.frames[body_id_2].parent;
-  geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
-                                             model.getBodyId("planar2_body"),joint_parent_2,
-                                             sample2,SE3::Identity(), "", Eigen::Vector3d::Ones())
-                              );
+  Model::JointIndex idx_geom2 = geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
+                                                                           model.getBodyId("planar2_body"),joint_parent_2,
+                                                                           sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()),
+                                                            model);
+  BOOST_CHECK(geomModel.geometryObjects[idx_geom2].parentJoint == model.frames[body_id_2].parent);
 
   geomModel.addAllCollisionPairs();
   se3::Data data(model);