trajectory controller: make use of new multidim pid controller

source/RobotAPI/libraries/core/TrajectoryController.cpp

@@ -24,6 +24,7 @@
 #include "TrajectoryController.h"
 #include <RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h>
 #include <RobotAPI/libraries/core/math/MathUtils.h>
+
 namespace armarx
 {
 
@@ -36,6 +37,7 @@ namespace armarx
             limitless.push_back(ls.enabled);
         }
         pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe, limitless));
+        pid->preallocate(traj->dim());
         ARMARX_CHECK_EXPRESSION(traj);
         currentTimestamp = traj->begin()->getTimestamp();
         //for (size_t i = 0; i < traj->dim(); i++)
@@ -51,22 +53,36 @@ namespace armarx
     {
         ARMARX_CHECK_EXPRESSION(pid);
         ARMARX_CHECK_EXPRESSION(traj);
+        ARMARX_CHECK_EXPRESSION(traj->size() > 0);
         ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
         size_t dim = traj->dim();
         currentTimestamp  = currentTimestamp + deltaT;
-
+        const Trajectory& traj = *this->traj;
         if (currentTimestamp < 0.0)
         {
             currentTimestamp = 0.0;
         }
+        if (currentTimestamp <= traj.rbegin()->getTimestamp())
+        {
+            for (size_t i = 0; i < dim; ++i)
+            {
 
-        for (size_t i = 0; i < dim; ++i)
+                positions(i) = traj.getState(currentTimestamp, i, 0);
+                veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj.getState(currentTimestamp, i, 1);
+                //pids.at(i)->update(std::abs(deltaT), currentPosition(i), positions(i));
+                //veloctities(i) += pids.at(i)->getControlValue();
+            }
+        }
+        else // hold position in the end
         {
-            positions(i) = traj->getState(currentTimestamp, i, 0);
-            veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj->getState(currentTimestamp, i, 1);
-            //pids.at(i)->update(std::abs(deltaT), currentPosition(i), positions(i));
-            //veloctities(i) += pids.at(i)->getControlValue();
+            for (size_t i = 0; i < dim; ++i)
+            {
+                positions(i) = traj.rbegin()->getPosition(i);
+                veloctities(i) = 0;
+            }
         }
+        currentError = positions - currentPosition;
+
         pid->update(std::abs(deltaT), currentPosition, positions);
         veloctities += pid->getControlValue();
         return veloctities;
@@ -154,5 +170,15 @@ namespace armarx
         }
     }
 
+    const Eigen::VectorXf& TrajectoryController::getCurrentError() const
+    {
+        return currentError;
+    }
+
+    const Eigen::VectorXf& TrajectoryController::getPositions() const
+    {
+        return positions;
+    }
+
 
 } // namespace armarx

source/RobotAPI/libraries/core/TrajectoryController.h

@@ -44,6 +44,10 @@ namespace armarx
         static void UnfoldLimitlessJointPositions(TrajectoryPtr traj);
         static void FoldLimitlessJointPositions(TrajectoryPtr traj);
 
+        const Eigen::VectorXf& getCurrentError() const;
+
+        const Eigen::VectorXf& getPositions() const;
+
     protected:
         TrajectoryPtr traj;
         MultiDimPIDControllerPtr pid;
@@ -51,6 +55,7 @@ namespace armarx
         double currentTimestamp;
         Eigen::VectorXf positions;
         Eigen::VectorXf veloctities;
+        Eigen::VectorXf currentError;
 
     };
     typedef std::shared_ptr<TrajectoryController> TrajectoryControllerPtr;