diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index f9d565b7d3f1164def1ecbc4b9bd906eb5f1de2b..6593562f053e509d5fdb5cb27d67eac51486027a 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -48,7 +48,10 @@ namespace hpp {
 
           /// Build a trajectory in SE(3).
           /// \param points a Nx7 matrix whose rows corresponds to a pose.
-          static PathPtr_t makePiecewiseLinearTrajectory (matrixIn_t points);
+          /// \param weights a 6D vector, weights to be applied when computing
+          ///        the distance between two SE3 points.
+          static PathPtr_t makePiecewiseLinearTrajectory (matrixIn_t points,
+              vectorIn_t weights);
 
           /// Set the constraint whose right hand side will vary.
           void trajectoryConstraint (const constraints::ImplicitPtr_t& ic);
diff --git a/src/steering-method/end-effector-trajectory.cc b/src/steering-method/end-effector-trajectory.cc
index 62138543c54a3cf78ebc6f6df5f8f59011459e3c..634075bc121673807bfa7301dbf2aab706ee4cbd 100644
--- a/src/steering-method/end-effector-trajectory.cc
+++ b/src/steering-method/end-effector-trajectory.cc
@@ -82,26 +82,31 @@ namespace hpp {
         };
       }
 
-      PathPtr_t EndEffectorTrajectory::makePiecewiseLinearTrajectory (matrixIn_t points)
+      PathPtr_t EndEffectorTrajectory::makePiecewiseLinearTrajectory (
+          matrixIn_t points,
+          vectorIn_t weights)
       {
         if (points.cols() != 7)
           throw std::invalid_argument("The input matrix should have 7 columns");
+        if (weights.size() != 6)
+          throw std::invalid_argument("The weights vector should have 6 elements");
         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();
+        if (points.rows() == 1)
           path->appendPath (core::StraightPath::create (se3,
-                points.row(i-1),
-                points.row(i),
-                interval_t(0, d)));
-        }
+              points.row(0),
+              points.row(0), interval_t(0,0)));
+        else
+          for (size_type i = 1; i < points.rows(); ++i) {
+            value_type d =
+              ( se3->elementConstRef(points.row(i))
+                - se3->elementConstRef(points.row(i-1))
+              ).cwiseProduct(weights).norm();
+            path->appendPath (core::StraightPath::create (se3,
+                  points.row(i-1),
+                  points.row(i),
+                  interval_t(0, d)));
+          }
         return path;
       }