diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 91f65179c440efe816c1e4d6f22f4f517e0ff2ea..74c1e94c43a4af671f56e4a636aa829f69eb7daa 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -296,13 +296,38 @@ namespace armarx
 
         Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
 
-        Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
-        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
-        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
+        Eigen::VectorXf diffJoints = desiredNullSpaceJointValues;
 
+        for (size_t i = 0; i < rns->getSize(); i++)
+        {
+            VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+            if (rn->isLimitless() && std::abs(diffJoints(i) - qpos(i)) > M_PI)
+            {
+                if (diffJoints(i) > qpos(i))
+                {
+                    diffJoints(i) = -(2 * M_PI - std::abs(diffJoints(i) - qpos(i)));
+                }
+                else
+                {
+                    diffJoints(i) = 2 * M_PI - std::abs(diffJoints(i) - qpos(i));
+                }
+            }
+            else
+            {
+                diffJoints(i) -= qpos(i);
+            }
+        }
 
+        Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(diffJoints) - dnull.cwiseProduct(qvel);
+        Eigen::MatrixXf jtp = jacobi.transpose();
 
+        auto svd = Eigen::JacobiSVD(jtp, Eigen::ComputeThinU | Eigen::ComputeThinV);
+        auto sv = svd.singularValues();
+        double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+        double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+        ik->setDampedSvdLambda(damping);
+        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jtp, 2.0);
+        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
 
         // torque limit
         ARMARX_CHECK_EXPRESSION(!targets.empty());
@@ -407,8 +432,8 @@ namespace armarx
             goal.at(i) = traj.rbegin()->getPosition(i);
             currentJointState.at(i).pos = currentJVS.at(i);
             currentJointState.at(i).vel = 0;
+            ARMARX_INFO << goal.at(i);
         }
-
         trajs.push_back(traj);
         nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
 
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index f140f9512bcb990a82e50ed3eb271536817d4d88..9527835a6dd48107f5e11846ec882d30b056c54a 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -57,6 +57,11 @@ void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::Carte
             _jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
         }
     }
+    auto svd = Eigen::JacobiSVD(_jacobiWithCosts, Eigen::ComputeThinU | Eigen::ComputeThinV);
+    auto sv = svd.singularValues();
+    double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+    double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+    ik->setDampedSvdLambda(damping);
     _inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
 }
 
@@ -267,6 +272,11 @@ bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolve
     }
     if (modifiedJacobi)
     {
+        auto svd = Eigen::JacobiSVD(jacobi, Eigen::ComputeThinU | Eigen::ComputeThinV);
+        auto sv = svd.singularValues();
+        double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+        double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+        ik->setDampedSvdLambda(damping);
         inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
     }