Update to modifications in hpp-constraints -> LiegroupElement.

include/hpp/manipulation/device.hh

@@ -41,7 +41,7 @@ namespace hpp {
         boost::mpl::vector < HandlePtr_t,
                              GripperPtr_t,
                              JointAndShapes_t,
-                             FrameIndexes_t> >
+                             FrameIndices_t> >
     {
       public:
         typedef pinocchio::HumanoidRobot Parent_t;
@@ -50,7 +50,7 @@ namespace hpp {
           boost::mpl::vector < HandlePtr_t,
           pinocchio::GripperPtr_t,
           JointAndShapes_t,
-          FrameIndexes_t> > Containers_t;
+          FrameIndices_t> > Containers_t;
 
         /// Constructor
         /// \param name of the new instance,

include/hpp/manipulation/fwd.hh

@@ -31,14 +31,17 @@ namespace hpp {
     typedef pinocchio::Joint Joint;
     typedef pinocchio::JointPtr_t JointPtr_t;
     typedef pinocchio::JointIndex JointIndex;
-    typedef std::vector<JointIndex> JointIndexes_t;
+    typedef std::vector<JointIndex> JointIndices_t;
     typedef se3::FrameIndex FrameIndex;
-    typedef std::vector<se3::FrameIndex> FrameIndexes_t;
+    typedef std::vector<se3::FrameIndex> FrameIndices_t;
     typedef pinocchio::Configuration_t Configuration_t;
     typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
     typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
     typedef core::ConfigurationPtr_t ConfigurationPtr_t;
     typedef pinocchio::GripperPtr_t GripperPtr_t;
+    typedef pinocchio::LiegroupElement LiegroupElement;
+    typedef pinocchio::LiegroupSpace LiegroupSpace;
+    typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
     HPP_PREDEF_CLASS (AxialHandle);
     typedef boost::shared_ptr <AxialHandle> AxialHandlePtr_t;
     HPP_PREDEF_CLASS (Handle);

src/device.cc

@@ -43,7 +43,7 @@ namespace hpp {
     void Device::setRobotRootPosition (const std::string& rn,
         const Transform3f& t)
     {
-      FrameIndexes_t idxs = get<JointIndexes_t> (rn);
+      FrameIndices_t idxs = get<JointIndices_t> (rn);
       pinocchio::Model& m = model();
       pinocchio::GeomModel& gm = geomModel();
       // The root frame should be the first frame.
@@ -83,7 +83,7 @@ namespace hpp {
       /// Build list of new joints
       std::size_t fvSize = model().frames.size();
       assert (fvSize >= frameCacheSize_);
-      FrameIndexes_t newF (fvSize - frameCacheSize_);
+      FrameIndices_t newF (fvSize - frameCacheSize_);
       for (std::size_t i = frameCacheSize_; i < fvSize; ++i) {
         assert (
             (model().frames[i].name.compare(0, name.size(), name) == 0)
@@ -93,8 +93,8 @@ namespace hpp {
       }
 
       frameCacheSize_ = model().frames.size();
-      if (has<FrameIndexes_t>(name)) {
-        const FrameIndexes_t& old = get<FrameIndexes_t>(name);
+      if (has<FrameIndices_t>(name)) {
+        const FrameIndices_t& old = get<FrameIndices_t>(name);
         newF.insert(newF.begin(), old.begin(), old.end());
       }
       add (name, newF);

src/graph-steering-method.cc

@@ -19,7 +19,7 @@
 #include <hpp/util/pointer.hh>
 
 #include <hpp/core/straight-path.hh>
-#include <hpp/core/steering-method-straight.hh>
+#include <hpp/core/steering-method/straight.hh>
 
 #include <hpp/manipulation/problem.hh>
 #include <hpp/manipulation/graph/graph.hh>

src/graph/helper.cc

@@ -1072,14 +1072,16 @@ namespace hpp {
             // Create object lock
 	    // Loop over all frames of object, detect joint and create locked
 	    // joint.
-            assert (robot.has <FrameIndexes_t> (od.name));
-            BOOST_FOREACH (const se3::FrameIndex& f, robot.get<FrameIndexes_t> (od.name)) {
+            assert (robot.has <FrameIndices_t> (od.name));
+            BOOST_FOREACH (const se3::FrameIndex& f, robot.get<FrameIndices_t> (od.name)) {
               if (model.frames[f].type != se3::JOINT) continue;
               const JointIndex j = model.frames[f].parent;
               JointPtr_t oj (new Joint (ps->robot(), j));
-              LockedJointPtr_t lj = core::LockedJoint::create (oj,
-                  robot.currentConfiguration()
-                  .segment (oj->rankInConfiguration (), oj->configSize ()));
+              LiegroupSpacePtr_t space (oj->configurationSpace ());
+              LiegroupElement lge (robot.currentConfiguration()
+                                   .segment (oj->rankInConfiguration (),
+                                             oj->configSize ()), space);
+              LockedJointPtr_t lj = core::LockedJoint::create (oj, lge);
               ps->add <LockedJointPtr_t> ("lock_" + oj->name (), lj);
               objects[i].get<0> ().get<2> ().push_back (lj);
             }

src/handle.cc

@@ -43,12 +43,12 @@ namespace hpp {
       struct ZeroDiffFunc : public constraints::DifferentiableFunction {
         ZeroDiffFunc (size_type sIn, size_type sInD,
             std::string name=std::string("Empty function"))
-          : DifferentiableFunction (sIn, sInD, 0, 0, name)
+          : DifferentiableFunction (sIn, sInD, LiegroupSpace::empty (), name)
         {
           context ("Grasp complement");
         }
 
-        inline void impl_compute (vectorOut_t, vectorIn_t) const {}
+        inline void impl_compute (LiegroupElement&, vectorIn_t) const {}
         inline void impl_jacobian (matrixOut_t, vectorIn_t) const {}
       };
     }

src/path-optimization/spline-gradient-based.cc

@@ -180,13 +180,13 @@ namespace hpp {
 
         ConstraintSetPtr_t set = state->configConstraint();
         value_type guessThreshold = this->problem().getParameter ("SplineGradientBased/guessThreshold", value_type(-1));
-        Eigen::RowBlockIndexes select =
+        Eigen::RowBlockIndices select =
           this->computeActiveParameters (path, set->configProjector()->solver(), guessThreshold, true);
 
         const size_type rDof = this->robot_->numberDof(),
                         col  = idxSpline * Spline::NbCoeffs * rDof,
                         row = lc.J.rows(),
-                        nOutVar = select.nbIndexes();
+                        nOutVar = select.nbIndices();
 
         // Add nOutVar constraints
         lc.addRows(nOutVar);
@@ -209,13 +209,13 @@ namespace hpp {
 
         // ConstraintSetPtr_t set = state->configConstraint();
         // value_type guessThreshold = this->problem().getParameter ("SplineGradientBased/guessThreshold", value_type(-1));
-        // Eigen::RowBlockIndexes select =
+        // Eigen::RowBlockIndices select =
           // this->computeActiveParameters (path, set->configProjector()->solver(), guessThreshold);
 
         const size_type rDof = this->robot_->numberDof(),
                         col  = idxSpline * Spline::NbCoeffs * rDof,
                         row = lc.J.rows(),
-                        // nOutVar = select.nbIndexes();
+                        // nOutVar = select.nbIndices();
                         nOutVar = rDof;
 
         // Add nOutVar constraints