Add steeringMethod::EndEffectorTrajectory::makePiecewiseLinearTrajectory

include/hpp/manipulation/steering-method/end-effector-trajectory.hh

@@ -46,6 +46,10 @@ namespace hpp {
             return ptr;
           }
 
+          /// Build a trajectory in SE(3).
+          /// \param points a Nx7 matrix whose rows corresponds to a pose.
+          static PathPtr_t makePiecewiseLinearTrajectory (matrixIn_t points);
+
           /// Set the constraint whose right hand side will vary.
           void trajectoryConstraint (const constraints::ImplicitPtr_t& ic);
 

src/steering-method/end-effector-trajectory.cc

@@ -27,6 +27,7 @@
 
 #include <hpp/core/config-projector.hh>
 #include <hpp/core/path.hh>
+#include <hpp/core/path-vector.hh>
 #include <hpp/core/problem.hh>
 #include <hpp/core/straight-path.hh>
 
@@ -80,6 +81,29 @@ namespace hpp {
         };
       }
 
+      PathPtr_t EndEffectorTrajectory::makePiecewiseLinearTrajectory (matrixIn_t points)
+      {
+        if (points.cols() != 7)
+          throw std::invalid_argument("The input matrix should have 7 columns");
+        LiegroupSpacePtr_t se3 = LiegroupSpace::SE3();
+        if (points.rows() == 1)
+          return core::StraightPath::create (se3,
+              points.row(0),
+              points.row(0), interval_t(0,0));
+
+        core::PathVectorPtr_t path = core::PathVector::create(7, 6);
+        for (size_type i = 1; i < points.rows(); ++i) {
+          value_type d = (
+              se3->elementConstRef(points.row(i)) - se3->elementConstRef(points.row(i-1))
+              ).norm();
+          path->appendPath (core::StraightPath::create (se3,
+                points.row(i-1),
+                points.row(i),
+                interval_t(0, d)));
+        }
+        return path;
+      }
+
       void EndEffectorTrajectory::trajectoryConstraint (const constraints::ImplicitPtr_t& ic)
       {
         constraint_ = ic;