trajectory interpolation fixes

source/RobotAPI/libraries/core/Trajectory.cpp

@@ -211,38 +211,53 @@ namespace armarx
 
         typename timestamp_view::iterator it = dataMap.find(t);
 
-        if (it == dataMap.end() || !it->data.at(dim))
+        if (it == dataMap.end() || dim >= it->data.size() || !it->data.at(dim) || it->data.at(dim)->size() <= derivation)
         {
+            //            ARMARX_INFO << "interpolating for " << VAROUT(t) << VAROUT(dim);
             __fillBaseDataAtTimestamp(t);// interpolates and retrieves
             it = dataMap.find(t);
         }
 
+        if (it->data.size() <= dim)
+        {
+            //            ARMARX_ERROR << "FAILED!";
+            //            ARMARX_INFO << VAROUT(t) << VAROUT(dim) << VAROUT(it->data.size()) << this->output();
+            throw LocalException() << "std::vector ptr is not the correct size!? " << VAROUT(dim) << VAROUT(it->data.size());
+        }
+
         if (!it->data.at(dim))
             //        it->data.at(dim).reset(new Ice::DoubleSeq());
         {
             throw LocalException() << "std::vector ptr is NULL!?";
         }
 
-        std::vector<DoubleSeqPtr>& vec = it->data;
 
+
+        std::vector<DoubleSeqPtr>& vec = it->data;
+        ARMARX_CHECK_GREATER(vec.size(), dim);
         if (derivation != 0 && vec.at(dim)->size() <= derivation)
         {
             //resize and calculate derivations
-            size_t curDeriv = it->data.at(dim)->size();
-            it->data.at(dim)->resize(derivation + 1);
+            ARMARX_CHECK_GREATER(vec.size(), dim);
+            ARMARX_CHECK_EXPRESSION(vec.at(dim));
+
+            size_t curDeriv = vec.at(dim)->size();
+            //            ARMARX_INFO << VAROUT(curDeriv) << VAROUT(dim);
+            vec.at(dim)->resize(derivation + 1);
 
             while (curDeriv <= derivation)
             {
                 double derivValue = getDiscretDifferentiationForDimAtT(t, dim, curDeriv);
                 checkValue(curDeriv);
-                it->data.at(dim)->at(curDeriv) = derivValue;
+                vec.at(dim)->at(curDeriv) = derivValue;
                 curDeriv++;
             }
 
         }
 
+        ARMARX_CHECK_GREATER_W_HINT(vec.at(dim)->size(), derivation, VAROUT(t) << VAROUT(dim) << VAROUT(*this));
         //    std::cout << "dimensions: " <<it->data.size() << " derivations: " <<  it->data.at(dim)->size() << std::endl;
-        double result = it->data.at(dim)->at(derivation);
+        double result = vec.at(dim)->at(derivation);
         //    checkValue(result);
         return result;
     }
@@ -795,6 +810,7 @@ namespace armarx
 
     std::vector<DoubleSeqPtr> Trajectory::__calcBaseDataAtTimestamp(const double& t) const
     {
+        //        ARMARX_INFO << "calcBaseDataAtTimestamp for " << t;
         //    typename timestamp_view::const_iterator it = dataMap.find(t);
         //    if(it != dataMap.end())
         //        return it->data;
@@ -831,20 +847,22 @@ namespace armarx
     {
         typename timestamp_view::const_iterator it = dataMap.find(t);
 
-        if (it != dataMap.end())
+        if (it != dataMap.end() && it->data.size() == dim())
         {
             bool foundEmpty = false;
 
             for (size_t i = 0; i < it->data.size(); i++)
             {
-                if (!it->data.at(i))
+                if (!it->data.at(i) || it->data.at(i)->empty())
                 {
                     foundEmpty = true;
+                    break;
                 }
             }
 
             if (!foundEmpty)
             {
+                //                ARMARX_INFO << "Was not empty for " << t;
                 return it;
             }
         }
@@ -855,7 +873,7 @@ namespace armarx
         dataMap.insert(entry);
         it = dataMap.find(t);
 
-        assert(it != dataMap.end());
+        ARMARX_CHECK_EXPRESSION(it != dataMap.end());
         //        const ordered_view& ordv = dataMap.get<TagOrdered>();
         //        typename ordered_view::iterator itOrdered = ordv.iterator_to(*it);
         it->data = __calcBaseDataAtTimestamp(t);
@@ -1252,7 +1270,7 @@ namespace armarx
 
         if (itNext == itPrev)
         {
-            throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << VAROUT(size());
+            throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << " " << VAROUT(size());
         }
 
         //        double diff = itNext->data[trajDimension]->at(derivation-1) - itPrev->data[trajDimension]->at(derivation-1);
@@ -1452,24 +1470,28 @@ namespace armarx
         double next = 0;
 
         // find previous SystemState that exists for that dimension
-        while (itPrev != ordView.end() && (itPrev->data.at(dimension) == NULL /*|| itPrev->data[dimension]->size() <= derivation*/))
+        while (itPrev != ordView.end() && (itPrev->data.at(dimension) == NULL || itPrev->data.at(dimension)->size() <= derivation))
         {
             itPrev--;
         }
 
         if (itPrev != ordView.end())
         {
+            //            ARMARX_INFO << "Found prev state at " << itPrev->timestamp;
+            ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itPrev->timestamp, VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this));
             previous = getState(itPrev->timestamp, dimension, derivation);
         }
 
         // find next SystemState that exists for that dimension
-        while (itNext != ordView.end() && (itNext->data.at(dimension) == NULL /*|| itNext->data[dimension]->size() <= derivation*/))
+        while (itNext != ordView.end() && (!itNext->data.at(dimension) || itNext->data.at(dimension)->size() <= derivation))
         {
             itNext++;
         }
 
         if (itNext != ordView.end())
         {
+            //            ARMARX_INFO << "Found next state at " << itNext->timestamp;
+            ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itNext->timestamp, VAROUT(t) << VAROUT(itNext->timestamp));
             next = getState(itNext->timestamp, dimension, derivation);
         }
 
@@ -1482,10 +1504,12 @@ namespace armarx
 
         if (itNext == ordView.end())
         {
+            //            ARMARX_INFO << "Extrapolating to the right from " << itPrev->timestamp;
             return getState(itPrev->timestamp, dimension, derivation) + getState(itPrev->timestamp, dimension, derivation + 1) * (t - itPrev->timestamp);
         }
         else if (itPrev == ordView.end())
         {
+            //            ARMARX_INFO << "Extrapolating to the left from " << itNext->timestamp;
             return getState(itNext->timestamp, dimension, derivation) - getState(itNext->timestamp, dimension, derivation + 1) * (itNext->timestamp - t);
         }
         else
@@ -1870,7 +1894,8 @@ namespace armarx
 
         for (; itMap != ordv.end(); itMap++)
         {
-            //itMap->timestamp -= shift;
+            // since all values are shifted this operation is OK
+            *const_cast<double*>(&(itMap->timestamp)) += shift;
         }
     }