added limitless joint support for trajectory controller

source/RobotAPI/libraries/core/TrajectoryController.cpp

@@ -48,6 +48,7 @@ namespace armarx
         //}
         positions.resize(traj->dim(), 1);
         veloctities.resize(traj->dim(), 1);
+        currentError.resize(traj->dim(), 1);
     }
 
     const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
@@ -82,7 +83,18 @@ namespace armarx
                 veloctities(i) = 0;
             }
         }
-        currentError = positions - currentPosition;
+        for (size_t i = 0; i < dim; ++i)
+        {
+            if (pid->limitless.size() > i && pid->limitless.at(i))
+            {
+                currentError(i) = math::MathUtils::AngleDelta(currentPosition(i), positions(i));
+            }
+            else
+            {
+                currentError(i) = positions(i) - currentPosition(i);
+            }
+
+        }
 
         pid->update(std::abs(deltaT), currentPosition, positions);
         veloctities += pid->getControlValue();