optimized trajectory controller class

source/RobotAPI/libraries/core/TrajectoryController.cpp

@@ -47,12 +47,12 @@ namespace armarx
         veloctities.resize(traj->dim(), 1);
     }
 
-    Eigen::VectorXf TrajectoryController::update(double deltaT, const Eigen::VectorXf currentPosition)
+    const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
     {
         ARMARX_CHECK_EXPRESSION(pid);
         ARMARX_CHECK_EXPRESSION(traj);
         ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
-        int dim = traj->dim();
+        size_t dim = traj->dim();
         currentTimestamp  = currentTimestamp + deltaT;
 
         if (currentTimestamp < 0.0)
@@ -60,7 +60,7 @@ namespace armarx
             currentTimestamp = 0.0;
         }
 
-        for (int 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);

source/RobotAPI/libraries/core/TrajectoryController.h

@@ -33,7 +33,7 @@ namespace armarx
     {
     public:
         TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true);
-        Eigen::VectorXf update(double deltaT, const Eigen::VectorXf currentPosition);
+        const Eigen::VectorXf& update(double deltaT, const Eigen::VectorXf& currentPosition);
         //const MultiDimPIDControllerPtr& getPid() const;
         //void setPid(const MultiDimPIDControllerPtr& value);