Device stores the joint list of each objects

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);
 
         /// \}
 

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;
         };
 

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
     {

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 ()));