added function to calculate the length of a trajectory over all dimensions

source/RobotAPI/libraries/core/Trajectory.cpp

@@ -472,82 +472,118 @@ namespace armarx
         }
     }
 
-    double Trajectory::getSpaceLength(size_t dimension, size_t stateDim) const
+    double Trajectory::getSpaceLength(size_t derivation) const
     {
-        return getSquaredSpaceLength(dimension, stateDim, begin()->getTimestamp(), rbegin()->getTimestamp());
+        return getSpaceLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
     }
 
-    double Trajectory::getSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    double Trajectory::getSpaceLength(size_t derivation, double startTime, double endTime) const
     {
         double length = 0.0;
         const ordered_view& ordv = dataMap.get<TagOrdered>();
-        //    timestamp_view::iterator it = dataMap.find(startTime);
-        //    Ice::DoubleSeq data = getDimensionData(0);
-        //    timestamp_view::iterator itEnd= dataMap.find(endTime);
         ordered_view::iterator itO = ordv.lower_bound(startTime);
         ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
         if (itO == ordv.end())
         {
             return 0.0;
         }
-        double prevValue = itO->getDeriv(dimension, stateDim);
+        size_t dimensions = dim();
+        Ice::DoubleSeq prevValue(dimensions);
+        for (size_t d = 0; d < dimensions; ++d)
+        {
+            prevValue[d] = itO->getDeriv(d, derivation);
+        }
+        itO++;
+        for (;
+             itO != itOEnd;
+             itO++
+            )
+        {
+            double segmentLength = 0;
+            double value;
+            for (size_t d = 0; d < dimensions; ++d)
+            {
+                value = itO->getDeriv(d, derivation);
+                segmentLength += pow(prevValue[d] - value, 2);
+                prevValue[d] = value;
+            }
+            length = sqrt(segmentLength);
+
+        }
+
+        return length;
+    }
+
+    double Trajectory::getSpaceLength(size_t dimension, size_t derivation) const
+    {
+        return getSpaceLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+    }
+
+    double Trajectory::getSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const
+    {
+        double length = 0.0;
+        const ordered_view& ordv = dataMap.get<TagOrdered>();
+        ordered_view::iterator itO = ordv.lower_bound(startTime);
+        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+        if (itO == ordv.end())
+        {
+            return 0.0;
+        }
+        double prevValue = itO->getDeriv(dimension, derivation);
 
         for (;
              itO != itOEnd;
              itO++
             )
         {
-            length += fabs(prevValue - itO->getDeriv(dimension, stateDim));
-            prevValue = itO->getDeriv(dimension, stateDim);
+            length += fabs(prevValue - itO->getDeriv(dimension, derivation));
+            prevValue = itO->getDeriv(dimension, derivation);
         }
 
         return length;
     }
 
-    double Trajectory::getSquaredSpaceLength(size_t dimension, size_t stateDim) const
+    double Trajectory::getSquaredSpaceLength(size_t dimension, size_t derivation) const
     {
-        return getSquaredSpaceLength(dimension, stateDim, begin()->getTimestamp(), rbegin()->getTimestamp());
+        return getSquaredSpaceLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
     }
 
-    double Trajectory::getSquaredSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    double Trajectory::getSquaredSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
         double length = 0.0;
         const ordered_view& ordv = dataMap.get<TagOrdered>();
-        //    timestamp_view::iterator it = dataMap.find(startTime);
-        //    Ice::DoubleSeq data = getDimensionData(0);
-        //    timestamp_view::iterator itEnd= dataMap.find(endTime);
         ordered_view::iterator itO = ordv.lower_bound(startTime);
         ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
         if (itO == ordv.end())
         {
             return 0.0;
         }
-        //    double prevValue = itO->data[dimension]->at(stateDim);
-        double prevValue = itO->getDeriv(dimension, stateDim);
+
+        double prevValue = itO->getDeriv(dimension, derivation);
 
         for (;
              itO != itOEnd;
              itO++
             )
         {
-            length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, stateDim), 2.0));
-            prevValue = itO->getDeriv(dimension, stateDim);
+            length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, derivation), 2.0));
+            prevValue = itO->getDeriv(dimension, derivation);
         }
 
         return length;
     }
 
-    double Trajectory::getMax(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    double Trajectory::getMax(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
-        return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, stateDim, startTime, endTime);
+        return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
     }
 
-    double Trajectory::getMin(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    double Trajectory::getMin(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
-        return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, stateDim, startTime, endTime);
+        return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
     }
 
-    double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t stateDim, double startTime, double endTime) const
+    double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const
     {
         double bestValue = initValue;
         const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -563,18 +599,18 @@ namespace armarx
              itO++
             )
         {
-            bestValue = foo(bestValue, itO->getDeriv(dimension, stateDim));
+            bestValue = foo(bestValue, itO->getDeriv(dimension, derivation));
         }
 
         return bestValue;
     }
 
-    double Trajectory::getTrajectorySpace(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    double Trajectory::getTrajectorySpace(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
-        return getMax(dimension, stateDim, startTime, endTime) - getMin(dimension, stateDim, startTime, endTime);
+        return getMax(dimension, derivation, startTime, endTime) - getMin(dimension, derivation, startTime, endTime);
     }
 
-    Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
         Ice::DoubleSeq result;
         const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -585,20 +621,20 @@ namespace armarx
         {
             return result;
         }
-        double preValue = itO->getDeriv(dimension, stateDim);
+        double preValue = itO->getDeriv(dimension, derivation);
         for (;
              itO != itOEnd;
 
             )
         {
             //            double t = itO->getTimestamp();
-            double cur = itO->getDeriv(dimension, stateDim);
+            double cur = itO->getDeriv(dimension, derivation);
             itO++;
             if (itO == ordv.end())
             {
                 break;
             }
-            double next = itO->getDeriv(dimension, stateDim);
+            double next = itO->getDeriv(dimension, derivation);
             if (cur <= preValue && cur <= next)
             {
                 result.push_back(cur);
@@ -609,7 +645,7 @@ namespace armarx
         return result;
     }
 
-    Ice::DoubleSeq Trajectory::getMinimaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    Ice::DoubleSeq Trajectory::getMinimaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
         Ice::DoubleSeq result;
         const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -620,20 +656,20 @@ namespace armarx
         {
             return result;
         }
-        double preValue = itO->getDeriv(dimension, stateDim);
+        double preValue = itO->getDeriv(dimension, derivation);
         for (;
              itO != itOEnd;
 
             )
         {
             //            double t = itO->getTimestamp();
-            double cur = itO->getDeriv(dimension, stateDim);
+            double cur = itO->getDeriv(dimension, derivation);
             itO++;
             if (itO == ordv.end())
             {
                 break;
             }
-            double next = itO->getDeriv(dimension, stateDim);
+            double next = itO->getDeriv(dimension, derivation);
             if (cur <= preValue && cur <= next)
             {
                 result.push_back(itO->getTimestamp());
@@ -644,7 +680,7 @@ namespace armarx
         return result;
     }
 
-    Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
         Ice::DoubleSeq result;
         const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -655,19 +691,19 @@ namespace armarx
         {
             return result;
         }
-        double preValue = itO->getDeriv(dimension, stateDim);
+        double preValue = itO->getDeriv(dimension, derivation);
         for (;
              itO != itOEnd;
 
             )
         {
-            double cur = itO->getDeriv(dimension, stateDim);
+            double cur = itO->getDeriv(dimension, derivation);
             itO++;
             if (itO == ordv.end())
             {
                 break;
             }
-            double next = itO->getDeriv(dimension, stateDim);
+            double next = itO->getDeriv(dimension, derivation);
             if (cur >= preValue && cur >= next)
             {
                 result.push_back(cur);
@@ -678,7 +714,7 @@ namespace armarx
         return result;
     }
 
-    Ice::DoubleSeq Trajectory::getMaximaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const
+    Ice::DoubleSeq Trajectory::getMaximaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const
     {
         Ice::DoubleSeq result;
         const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -689,19 +725,19 @@ namespace armarx
         {
             return result;
         }
-        double preValue = itO->getDeriv(dimension, stateDim);
+        double preValue = itO->getDeriv(dimension, derivation);
         for (;
              itO != itOEnd;
 
             )
         {
-            double cur = itO->getDeriv(dimension, stateDim);
+            double cur = itO->getDeriv(dimension, derivation);
             itO++;
             if (itO == ordv.end())
             {
                 break;
             }
-            double next = itO->getDeriv(dimension, stateDim);
+            double next = itO->getDeriv(dimension, derivation);
 
             if (cur >= preValue && cur >= next)
             {
@@ -1254,7 +1290,7 @@ namespace armarx
         double previousValue = valueAtFirstTimestamp;
         double previousTimestamp = it->timestamp;
 
-        //        size_t stateDim = double().size();
+        //        size_t derivation = double().size();
         for (it++; it != ordv.end(); it++)
         {
             double slope;
@@ -1593,7 +1629,10 @@ namespace armarx
         __addDimension();
 
         addPositionsToDimension(newDim - 1, x, y);
-        dimensionNames.push_back(name);
+        if(dimensionNames.size() > newDim-1)
+            dimensionNames.at(newDim-1) = name;
+        else
+            dimensionNames.push_back(name);
         return newDim - 1;
     }
 
@@ -1742,4 +1781,6 @@ namespace armarx
     }
 
 
+
+
 } // namespace armarx

source/RobotAPI/libraries/core/Trajectory.h

@@ -239,7 +239,6 @@ namespace armarx
          * @param dimension
          * @return
          */
-        //        Ice::DoubleSeq getDimensionData(size_t dimension = 0) const;
         Ice::DoubleSeq getDimensionData(size_t dimension, size_t deriv = 0) const;
         Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation) const;
         Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const;
@@ -253,18 +252,20 @@ namespace armarx
         size_t dim() const;
         size_t size() const;
         double getTimeLength() const;
-        double getSpaceLength(size_t dimension, size_t stateDim) const;
-        double getSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        double getSquaredSpaceLength(size_t dimension, size_t stateDim) const;
-        double getSquaredSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        double getMax(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        double getMin(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        double getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        double getTrajectorySpace(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        Ice::DoubleSeq getMinima(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        Ice::DoubleSeq getMaxima(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        Ice::DoubleSeq getMinimaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const;
-        Ice::DoubleSeq getMaximaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+        double getSpaceLength(size_t derivation = 0) const;
+        double getSpaceLength(size_t derivation, double startTime, double endTime) const;
+        double getSpaceLength(size_t dimension, size_t derivation) const;
+        double getSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        double getSquaredSpaceLength(size_t dimension, size_t derivation) const;
+        double getSquaredSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        double getMax(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        double getMin(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        double getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const;
+        double getTrajectorySpace(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        Ice::DoubleSeq getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        Ice::DoubleSeq getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        Ice::DoubleSeq getMinimaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const;
+        Ice::DoubleSeq getMaximaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const;
 
         // calculations
 

source/RobotAPI/libraries/core/test/TrajectoryTest.cpp

@@ -177,14 +177,17 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
     // Load the trajectory
     Ice::DoubleSeq timestamps;
     int count = 1000;
-    for (int i = 0; i < count; ++i) {
+    for (int i = 0; i < count; ++i)
+    {
         timestamps.push_back(rand());
     }
     Trajectory traj;
-    for (int var = 0; var < 8; ++var) {
+    for (int var = 0; var < 8; ++var)
+    {
         Ice::DoubleSeq p;
         p.reserve(count);
-        for (int i = 0; i < count; ++i) {
+        for (int i = 0; i < count; ++i)
+        {
             p.push_back(rand());
         }
         traj.addDimension(timestamps, p);
@@ -199,8 +202,12 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
     ARMARX_INFO_S << "Cleared";
     traj2->ice_postUnmarshal();
     ARMARX_INFO_S << "Unmarshalled";
-
+    traj.getSpaceLength();
+    ARMARX_INFO_S << "Length calculated";
     BOOST_CHECK_EQUAL(traj.getState(0), traj2->getState(0));
+    JSONObjectPtr json = new JSONObject();
+    traj.serialize(json);
+    ARMARX_INFO_S << json->asString(true);
 }
 
 
@@ -215,8 +222,19 @@ BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
     Trajectory traj2;
     traj2.deserialize(json);
     BOOST_CHECK_CLOSE(traj.getState(0), traj2.getState(0), 0.01);
-    BOOST_CHECK_CLOSE(traj.getState(0.4,1), traj2.getState(0.4,1), 0.01);
+    BOOST_CHECK_CLOSE(traj.getState(0.4, 1), traj2.getState(0.4, 1), 0.01);
     BOOST_CHECK_EQUAL(traj.getDimensionName(0), traj2.getDimensionName(0));
 }
 
+BOOST_AUTO_TEST_CASE(TrajectoryLengthTest)
+{
+    // Load the trajectory
+    FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+    Trajectory traj(positions, {}, {"joint1", "joint2"});
+
+    BOOST_CHECK_EQUAL(traj.getSpaceLength(), (Eigen::Vector3f(positions[0].data())- Eigen::Vector3f(positions[1].data())).norm());
+}
+
+
+