diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index fec0b6ac73605a3e5d41e70e72d72ac40cb10e1c..6dc1156eb3b59c863a0a9f2ddde2a36acba2e052 100644
--- a/include/hpp/manipulation/device.hh
+++ b/include/hpp/manipulation/device.hh
@@ -37,7 +37,8 @@ namespace hpp {
     class HPP_MANIPULATION_DLLAPI Device : public model::HumanoidRobot,
       public core::Container <HandlePtr_t>,
       public core::Container <model::GripperPtr_t>,
-      public core::Container <JointAndShapes_t>
+      public core::Container <JointAndShapes_t>,
+      public core::Container <model::JointVector_t>
     {
       public:
         typedef model::HumanoidRobot Parent_t;
@@ -112,7 +113,7 @@ namespace hpp {
         /// add the current Robot.
         /// When creating a robot from several URDF files, this enables
         /// collisions between joints from different files.
-        void didInsertRobot ();
+        void didInsertRobot (const std::string& name);
 
         /// \}
 
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index eadb13177404ef13530aec1c9ebfc374a5795615..267dc6bcd363dab1a80cebebd8bc88acfff8de8e 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -171,7 +171,6 @@ namespace hpp {
 
         struct ObjectDef_t {
           std::string name;
-          JointPtr_t lastJoint;
           StringList_t handles, shapes;
         };
 
diff --git a/src/device.cc b/src/device.cc
index 392d89f3d194cde8fc185205498edd17c208d1e9..c8f320fe6248c5c7f367f5a1be46cb013a6b79d8 100644
--- a/src/device.cc
+++ b/src/device.cc
@@ -25,7 +25,7 @@
 
 namespace hpp {
   namespace manipulation {
-        void Device::didInsertRobot ()
+        void Device::didInsertRobot (const std::string& name)
         {
           if (!didPrepare_) {
             hppDout (error, "You must call prepareInsertRobot before.");
@@ -34,6 +34,7 @@ namespace hpp {
           /// Build list of new joints
           const model::JointVector_t jv = getJointVector ();
           model::JointVector_t newj;
+          newj.reserve (jv.size () - jointCache_.size ());
           model::JointVector_t::const_iterator retFind, it1, it2;
           for (it1 = jv.begin (); it1 != jv.end (); ++it1) {
             retFind = find (jointCache_.begin (), jointCache_.end (), *it1);
@@ -50,6 +51,7 @@ namespace hpp {
             }
           }
           jointCache_.clear ();
+          add (name, newj);
         }
     std::ostream& Device::print (std::ostream& os) const
     {
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index c25f8a94f2d1b423e161cc8f9a204345089d9704..10eb08b607b8a0be244fddcd711860e9739a739a 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -871,8 +871,9 @@ namespace hpp {
               }
             }
             // Create object lock
-            for (JointPtr_t oj = od.lastJoint;
-                oj != NULL; oj = oj->parentJoint ()) {
+            using model::JointVector_t;
+            assert (robot.has <JointVector_t> (od.name));
+            BOOST_FOREACH (const JointPtr_t& oj, robot.get<JointVector_t> (od.name)) {
               LockedJointPtr_t lj = core::LockedJoint::create (oj,
                   robot.currentConfiguration()
                   .segment (oj->rankInConfiguration (), oj->configSize ()));