diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index 10080879e0cb59e0af96a4a3d4c7b4e29f650bb9..a2cd02a19b867888a00994a075c9d87f8f6067ab 100644
--- a/include/hpp/manipulation/device.hh
+++ b/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,
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 9cf66ac570efa2562d74b43679691be872d24e45..c076ca844aeef8129d9cad97e29a963db246726a 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/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);
diff --git a/src/device.cc b/src/device.cc
index 9cbf53c4c72bd1b87452bd66ae8e20a5a2ff5068..b641f991bae504f0dd36f052f3537e3a12ebeb0a 100644
--- a/src/device.cc
+++ b/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);
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index 1e5dbe780bcd6661cdea91c9beed981f34e539c7..8d7004bf8c24cc01d981917a0428385c20220315 100644
--- a/src/graph-steering-method.cc
+++ b/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>
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 53f8c73891108a6b570268fdb971a78d9808c2e2..13762cd3dbe311f2926491d4433dd720348d9888 100644
--- a/src/graph/helper.cc
+++ b/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);
}
diff --git a/src/handle.cc b/src/handle.cc
index 1ef224ab3d5db2271f0fe64be724800330bb3863..d7cd8718be022bb8fa74e615988e943458c38874 100644
--- a/src/handle.cc
+++ b/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 {}
};
}
diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc
index 2bd91eab23f740c4385b03c222d3dea26158c5ea..5fa3a0ff1da3152d30badae64ee2d59de760256d 100644
--- a/src/path-optimization/spline-gradient-based.cc
+++ b/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