added support for limitless joints

fixed joint limit check: not only near joint limit is considered a violation but also exceeding the joint limit

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -174,21 +174,22 @@ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobo
 
     for (size_t i = 0; i < size; ++i)
     {
-        auto& node = rns->getNode(i);
+        auto& node = rns->getNode(static_cast<int>(i));
 
-        if (std::abs(jointVel(i)) < 0.001f)
+        if (node->isLimitless() || // limitless joint cannot be out of limits
+            std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
+            )
         {
             continue;
         }
 
-        if ((std::abs(node->getJointValue() - node->getJointLimitHigh()) < jointLimitCheckAccuracy && jointVel(i) > 0)
-            || (std::abs(node->getJointValue() - node->getJointLimitLow()) < jointLimitCheckAccuracy && jointVel(i) < 0))
+        if ((node->getJointValue() >= node->getJointLimitHigh() - jointLimitCheckAccuracy && jointVel(i) > 0)
+            || (node->getJointValue() <= node->getJointLimitLow() + jointLimitCheckAccuracy && jointVel(i) < 0))
         {
-            for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free  resetting of column
+            for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
             {
                 jacobi(k, i) = 0.0f;
             }
-            //                ARMARX_INFO << deactivateSpam(0.5, node->getName()) << " joint " << node->getName() << " is at limit -\n" << inv << "\n initial inv jacobi:\n" << initialInvJac << "\n target cart vel:\n" << cartesianVel  << "\njacobi:\n" << jacobi;
             modifiedJacobi = true;
         }
     }