Update Device [WIP]

include/hpp/manipulation/device.hh

@@ -20,36 +20,37 @@
 #ifndef HPP_MANIPULATION_DEVICE_HH
 # define HPP_MANIPULATION_DEVICE_HH
 
-# include <hpp/model/humanoid-robot.hh>
+# include <hpp/pinocchio/humanoid-robot.hh>
+
 # include <hpp/core/container.hh>
 
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/config.hh"
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/config.hh>
 
 namespace hpp {
   namespace manipulation {
     /// Device with handles.
     ///
-    /// As a deriving class of hpp::model::HumanoidRobot,
-    /// it is compatible with hpp::model::urdf::loadHumanoidRobot
+    /// As a deriving class of hpp::pinocchio::HumanoidRobot,
+    /// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot
     ///
-    /// This class also contains model::Gripper, Handle and \ref JointAndTriangles_t
+    /// This class also contains pinocchio::Gripper, Handle and \ref JointAndShapes_t
     class HPP_MANIPULATION_DLLAPI Device :
-      public model::HumanoidRobot,
+      public pinocchio::HumanoidRobot,
       public core::Containers<
         boost::mpl::vector < HandlePtr_t,
-                             model::GripperPtr_t,
+                             GripperPtr_t,
                              JointAndShapes_t,
-                             model::JointVector_t> >
+                             JointIndexes_t> >
     {
       public:
-        typedef model::HumanoidRobot Parent_t;
+        typedef pinocchio::HumanoidRobot Parent_t;
 
         typedef core::Containers<
           boost::mpl::vector < HandlePtr_t,
-          model::GripperPtr_t,
+          pinocchio::GripperPtr_t,
           JointAndShapes_t,
-          model::JointVector_t> > Containers_t;
+          JointIndexes_t> > Containers_t;
 
         /// Constructor
         /// \param name of the new instance,
@@ -67,13 +68,6 @@ namespace hpp {
         /// \name Collisions
         /// \{
 
-        /// Cache joint vector
-        void prepareInsertRobot ()
-        {
-          didPrepare_ = true;
-          jointCache_ = getJointVector ();
-        }
-
         /// Add collisions
         /// between the joint vector cache initialized by prepareInsertRobot()
         /// add the current Robot.
@@ -89,7 +83,7 @@ namespace hpp {
         /// \param robot Robots that manipulate objects,
         /// \param objects Set of objects manipulated by the robot.
         Device (const std::string& name) :
-          Parent_t (name), jointCache_ (), didPrepare_ (false)
+          Parent_t (name), jointCacheSize_ (0)
         {}
 
         void init (const DeviceWkPtr_t& self)
@@ -101,8 +95,7 @@ namespace hpp {
       private:
         DeviceWkPtr_t self_;
 
-        model::JointVector_t jointCache_;
-        bool didPrepare_;
+        std::size_t jointCacheSize_;
     }; // class Device
   } // namespace manipulation
 } // namespace hpp

src/device.cc

@@ -17,42 +17,27 @@
 // hpp-manipulation. If not, see
 // <http://www.gnu.org/licenses/>.
 
-#include <hpp/model/gripper.hh>
 #include <hpp/manipulation/device.hh>
-#include <hpp/manipulation/handle.hh>
 
-#include <hpp/model/joint.hh>
+#include <hpp/pinocchio/joint.hh>
+#include <hpp/pinocchio/gripper.hh>
+
+#include <hpp/manipulation/handle.hh>
 
 namespace hpp {
   namespace manipulation {
-        void Device::didInsertRobot (const std::string& name)
-        {
-          if (!didPrepare_) {
-            hppDout (error, "You must call prepareInsertRobot before.");
-          }
-          didPrepare_ = false;
-          /// 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);
-            if (retFind == jointCache_.end ())
-              newj.push_back (*it1);
-          }
-          /// Add collision between old joints and new ones.
-          for (it1 = newj.begin (); it1 != newj.end (); ++it1) {
-            if (!(*it1)->linkedBody ()) continue;
-            for (it2 = jointCache_.begin (); it2 != jointCache_.end (); ++it2) {
-              if (!(*it2)->linkedBody ()) continue;
-              addCollisionPairs (*it1, *it2, model::COLLISION);
-              addCollisionPairs (*it1, *it2, model::DISTANCE);
-            }
-          }
-          jointCache_.clear ();
-          add (name, newj);
-        }
+    void Device::didInsertRobot (const std::string& name)
+    {
+      /// Build list of new joints
+      std::size_t jvSize = model().joints.size();
+      assert (jvSize >= jointCacheSize_);
+      JointIndexes_t newJ (jvSize - jointCacheSize_);
+      for (std::size_t i = jointCacheSize_; i < jvSize; ++i) newJ[i] = i;
+
+      jointCacheSize_ = model().joints.size();
+      add (name, newJ);
+    }
+
     std::ostream& Device::print (std::ostream& os) const
     {
       Parent_t::print (os);
@@ -61,7 +46,7 @@ namespace hpp {
       Containers_t::print <HandlePtr_t> (os);
       // print grippers
       os << "Grippers:" << std::endl;
-      Containers_t::print <model::GripperPtr_t> (os);
+      Containers_t::print <GripperPtr_t> (os);
       return os;
     }