fixed race condition when setting control mode on kinematicunit and setting a...

fixed race condition when setting control mode on kinematicunit and setting a target immediately after

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

@@ -63,6 +63,11 @@ namespace armarx
             *target = control;
         }
         inline virtual void rtPreActivateController() override
+        {
+
+        }
+
+        void reset()
         {
             control = (*sensor) * sensorToControlOnActivateFactor;
             if (std::abs(control) < resetZeroThreshold)

source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp

@@ -219,7 +219,7 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
                                << "'. (ignoring this device)";
                 continue;
             }
-            auto ctrl = devs.at(name).getController(mode);
+            NJointKinematicUnitPassThroughControllerPtr ctrl = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(devs.at(name).getController(mode));
             if (!ctrl)
             {
                 ARMARX_WARNING << "requested unsupported mode " << mode << " for device '" << name
@@ -230,6 +230,7 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
             {
                 continue;
             }
+            ctrl->reset(); // reset immediately instead of preActivate() to avoid race condition
             toActivate[name] = std::move(ctrl);
         }
         const auto printToActivate = ARMARX_STREAM_PRINTER