diff --git a/src/algorithm/geometry.hpp b/src/algorithm/geometry.hpp
index b99352e61b9855c389214859a24652986b70ee43..914f5db854a1f165ca5f58fb0987943e84ce5377 100644
--- a/src/algorithm/geometry.hpp
+++ b/src/algorithm/geometry.hpp
@@ -93,6 +93,14 @@ namespace se3
                                 const GeometryModel & geomModel,
                                 GeometryData &        geomData);
 #endif // WITH_HPP_FCL
+
+  /// Append geomModel2 to geomModel1
+  ///
+  /// \warning Radius should be recomputed.
+  /// \todo The geometry objects of geomModel2 should be added as outerObjects
+  ///       of the joints originating from model1 but I do not know how to do it.
+  inline void appendGeometryModel(GeometryModel & geomModel1,
+                                  const GeometryModel & geomModel2);
 } // namespace se3 
 
 /* --- Details -------------------------------------------------------------------- */
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 7ea87eea7acc580603e955de5aa35546298e302e..1ca8ca5d9707cb5ec71549b1bb11607377ad3e82 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -181,6 +181,59 @@ namespace se3
 #undef SE3_GEOM_AABB
 #endif // WITH_HPP_FCL
 
+  inline void appendGeometryModel(GeometryModel & geomModel1,
+                                  const GeometryModel & geomModel2)
+  {
+    assert (geomModel1.ngeoms == geomModel1.geometryObjects.size());
+    Index nGeom1 = geomModel1.geometryObjects.size();
+    Index nColPairs1 = geomModel1.collisionPairs.size();
+    assert (geomModel2.ngeoms == geomModel2.geometryObjects.size());
+    Index nGeom2 = geomModel2.geometryObjects.size();
+    Index nColPairs2 = geomModel2.collisionPairs.size();
+
+    /// Append the geometry objects and geometry positions
+    geomModel1.geometryObjects.insert(geomModel1.geometryObjects.end(),
+        geomModel2.geometryObjects.begin(), geomModel2.geometryObjects.end());
+
+    /// 1. copy the collision pairs and update geomData1 accordingly.
+    geomModel1.collisionPairs.reserve(nColPairs1 + nColPairs2 + nGeom1 * nGeom2);
+    for (Index i = 0; i < nColPairs2; ++i)
+    {
+      const CollisionPair& cp = geomModel2.collisionPairs[i];
+      geomModel1.collisionPairs.push_back(
+          CollisionPair (cp.first + nGeom1, cp.second + nGeom1)
+          );
+    }
+
+    /// 2. Update the inner/outer objects
+    typedef GeometryModel::GeomIndexList GeomIndexList;
+    typedef std::map < JointIndex, GeomIndexList > Map_t;
+    BOOST_FOREACH(const Map_t::value_type& innerObject, geomModel2.innerObjects)
+    {
+      GeomIndexList& innerGeoms = geomModel1.innerObjects[innerObject.first];
+      innerGeoms.reserve(innerGeoms.size() + innerObject.second.size());
+      BOOST_FOREACH(const GeomIndex& gid, innerObject.second)
+      {
+        innerGeoms.push_back(nGeom1 + gid);
+      }
+    }
+    BOOST_FOREACH(const Map_t::value_type& outerObject, geomModel2.outerObjects)
+    {
+      GeomIndexList& outerGeoms = geomModel1.outerObjects[outerObject.first];
+      outerGeoms.reserve(outerGeoms.size() + outerObject.second.size());
+      BOOST_FOREACH(const GeomIndex& gid, outerObject.second)
+      {
+        outerGeoms.push_back(nGeom1 + gid);
+      }
+    }
+
+    /// 3. add the collision pairs between geomModel1 and geomModel2.
+    for (Index i = 0; i < nGeom1; ++i) {
+      for (Index j = 0; j < nGeom2; ++j) {
+        geomModel1.collisionPairs.push_back(CollisionPair(i, nGeom1 + j));
+      }
+    }
+  }
 } // namespace se3
 
 #endif // ifnded __se3_algo_geometry_hxx__