Add setting to waypoint controller to not skip to the closest waypoint

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp

@@ -182,7 +182,10 @@ namespace armarx
                 _publishWpsNum = r.wps.size();
                 _rtHasWps = !r.wps.empty();
                 _rtWpController->swapWaypoints(r.wps);
-                _rtWpController->skipToClosestWaypoint(r.skipRad2mmFactor);
+                if (r.cfg.skipToClosestWaypoint)
+                {
+                    _rtWpController->skipToClosestWaypoint(r.skipRad2mmFactor);
+                }
             }
             _publishIsAtForceLimit = false;
         }

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h

@@ -77,6 +77,7 @@ namespace armarx
             float skipRad2mmFactor = 500;
             bool wpsUpdated = false;
             bool cfgUpdated = false;
+            bool skipToClosestWaypoint = true;
         };
 
         struct RtToNonRtData

source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice

@@ -36,6 +36,7 @@ module armarx
         float forceThreshold = -1; // < 0 -> no limit
         bool forceThresholdInRobotRootZ = true;
         bool optimizeNullspaceIfTargetWasReached = false;
+        bool skipToClosestWaypoint = true;
     };
 
     class NJointCartesianWaypointControllerConfig extends NJointControllerConfig