make aborting behavior configurable

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

@@ -79,21 +79,24 @@ namespace armarx
             const Eigen::Vector2f currentJointAngles(_rtYawNode->getJointValue(), _rtPitchNode->getJointValue());
             const Eigen::Vector3f targetPoint = _currentTarget.targetPoint->toRootEigen(_rtRobot);
             const float h = _rtCameraNode->getPositionInRootFrame().z();
-            const float yaw = -std::atan2(targetPoint.x(), targetPoint.y());
-            const float pitch = std::atan2(h - targetPoint.z(), targetPoint.y());
+            float yaw = -std::atan2(targetPoint.x(), targetPoint.y());
+            float pitch = std::atan2(h - targetPoint.z(), targetPoint.y());
+            const bool targetReachable = _rtYawNode->checkJointLimits(yaw)
+                                         && _rtPitchNode->checkJointLimits(pitch);
+            if (!targetReachable)
+            {
+                //limit joint ranges to avoid damage
+                yaw = std::clamp(yaw, _rtYawNode->getJointLimitLo(), _rtYawNode->getJointLimitHi());
+                pitch = std::clamp(pitch, _rtPitchNode->getJointLimitLo(), _rtPitchNode->getJointLimitHi());
+            }
             const Eigen::Vector2f targetJointAngles(yaw, pitch);
 
             _pid->update(timeSinceLastIteration.toSecondsDouble(), currentJointAngles, targetJointAngles);
             Eigen::Vector2f ctrlVel = _pid->getControlValue();
 
-            //check end conditions
-            const bool targetReachable = _rtYawNode->checkJointLimits(targetJointAngles.x())
-                                         && _rtPitchNode->checkJointLimits(targetJointAngles.y());
-            const Eigen::Vector2f angleDiff = targetJointAngles - currentJointAngles;
-            const bool targetReached = std::abs(angleDiff.x()) < _config->yawAngleTolerance &&
-                                       std::abs(angleDiff.y()) < _config->pitchAngleTolerance;
-            if (!targetReachable)
+            if (!targetReachable && _config->abortIfUnreachable)
             {
+                //abort the target
                 _currentTarget.abortedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
                 _tripRt2NonRt.getWriteBuffer() = _currentTarget;
                 _tripRt2NonRt.commitWrite();
@@ -101,9 +104,12 @@ namespace armarx
                 _pid->reset();
                 _controlActive = false;
             }
-            else if (targetReached)
+            const Eigen::Vector2f angleDiff = targetJointAngles - currentJointAngles;
+            const bool targetReached = std::abs(angleDiff.x()) < _config->yawAngleTolerance &&
+                                       std::abs(angleDiff.y()) < _config->pitchAngleTolerance;
+            if (targetReached)
             {
-                if (!_currentTarget.isReached())
+                if (targetReachable && !_currentTarget.isReached())
                 {
                     _currentTarget.reachedTimestampMilliSeconds = sensorValuesTimestamp.toMilliSeconds();
                     _tripRt2NonRt.getWriteBuffer() = _currentTarget;

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

@@ -44,6 +44,8 @@ module armarx
 
         float yawAngleTolerance = 0.005;
         float pitchAngleTolerance = 0.005;
+
+        bool abortIfUnreachable = true;
     };
 
     interface GazeControllerInterface extends NJointControllerInterface