diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
index ff379ac4ee8daa0235be44f7584867877f09c813..daacae417637d89a1a4d23a79a6a4668d196b69d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/GazeController.cpp
+++ b/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;
diff --git a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
index d8dbc1b2a803fa4bb5b6be334c0f0c416947960c..04fe7aa27e7b8b77c48edaaf102f23b9653f0022 100644
--- a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
+++ b/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
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
index e54134fbae50c5547dcddb0ef06bb8d57531c284..8a519d1f28d70fc5f2539bce293096effd7426e0 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -120,6 +120,25 @@ namespace armarx::aron::codegenerator::cpp
         }
     }
 
+    std::string Generator::getFullCppTypenameGenerator() const
+    {
+        switch (getType().getMaybe())
+        {
+            case type::Maybe::eNone:
+                return getCoreCppTypename();
+            case type::Maybe::eOptional:
+                return "std::make_optional<" + getCoreCppTypename() + ">";
+            case type::Maybe::eRawPointer:
+                return getCoreCppTypename() + "*";
+            case type::Maybe::eSharedPointer:
+                return "std::make_shared<" + getCoreCppTypename() + ">";
+            case type::Maybe::eUniquePointer:
+                return "std::make_unique<" + getCoreCppTypename() + ">";
+            default:
+                throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received unknown maybe enum", std::to_string((int) getType().getMaybe()), getType().getPath());
+        }
+    }
+
     CppCtorPtr Generator::toCtor(const std::string& name) const
     {
         CppCtorPtr c = CppCtorPtr(new CppCtor(name + "()"));
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h
index 3978677fdc29f54fc1041d97b970780ecc37894a..8ea8f0f0b90812c0c9b78fd3c4edb950536ff386 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h
@@ -108,6 +108,7 @@ namespace armarx::aron::codegenerator::cpp
         // public member methods
         std::string getCoreCppTypename() const;
         std::string getFullCppTypename() const;
+        std::string getFullCppTypenameGenerator() const;
 
         CppMethodPtr toSpecializedDataWriterMethod(const WriterInfo& info) const;
         CppMethodPtr toSpecializedDataReaderMethod(const ReaderInfo& info) const;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
index c0fbed1127a93cbd4e6ba77a035375eba34245c5..54b58ec531ea7eedfd3630b7ad3ac630c68ffb19 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
@@ -55,7 +55,7 @@ namespace armarx::aron::codegenerator::cpp::generator
     CppBlockPtr PointCloud::getResetSoftBlock(const std::string& cppAccessor) const
     {
         CppBlockPtr block_if_data = std::make_shared<CppBlock>();
-        block_if_data->addLine(cppAccessor + " = " + getFullCppTypename() + "(" + cppAccessor + nextEl() + "width, " + cppAccessor + nextEl() + "height);");
+        block_if_data->addLine(cppAccessor + " = " + getFullCppTypenameGenerator() + "(" + cppAccessor + nextEl() + "width, " + cppAccessor + nextEl() + "height);");
         return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
     }
 
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
index 73b75913805426a660ba0dddd3cf70d7738b137d..2736bfff337a33de477c224e34e63a81daf4b048 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
@@ -60,6 +60,7 @@ namespace armarx
         public:
             virtual void init(Parameters& params) = 0;
             virtual Eigen::VectorXf getGradient(Parameters& params) = 0;
+            virtual ~NullspaceGradient() = default;
             float kP = 1;
         };
         typedef std::shared_ptr<class NullspaceGradient> NullspaceGradientPtr;