diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
index 96fd7b81a143bddd60c37886f7d9223044af28c5..fb9c8c7c1ddfb1177b52860a1e8c01f44015919b 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
@@ -182,7 +182,6 @@ Eigen::MatrixXd AbstractManipulabilityTracking::getJointsLimitsWeightMatrix(cons
             weights(i) = 1.;
         }
         else {
-            // TODO add condition on velocities
             gradient(i) = std::pow(jointLimitsHigh(i) - jointLimitsLow(i), 2) * (2*jointAngles(i) - jointLimitsHigh(i) - jointLimitsLow(i)) /
                                 ( 4 * std::pow(jointLimitsHigh(i) - jointAngles(i), 2) * std::pow(jointAngles(i) - jointLimitsLow(i), 2) );
 
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
index 468f87822b2633005627a34977cdf788413e5a11..6fc9b3118ee3047436c6a55165fa33644675448e 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
@@ -81,14 +81,22 @@ Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen
     Eigen::Tensor<double, 3> manipJ_sub = subCube(manipJ);
 
     Eigen::MatrixXd matManipJ = computeManipulabilityJacobianMandelNotation(manipJ_sub);
+    Eigen::MatrixXd dampedLSI;
     if (jointLimitAvoidance)
     {
         // Compute weighted manipulability Jacobian
         Eigen::MatrixXd jointsWeightMatrix = getJointsLimitsWeightMatrix(manipulability->getJointAngles(), manipulability->getJointLimitsLow(), manipulability->getJointLimitsHigh());
         matManipJ = matManipJ * jointsWeightMatrix;
+        // Compute pseudo-inverse of weighted manipulability Jacobian
+        double dampingFactor = getDamping(matManipJ);
+        dampedLSI = jointsWeightMatrix * dampedLeastSquaresInverse(matManipJ, dampingFactor);
     }
-    double dampingFactor = getDamping(matManipJ);
-    Eigen::MatrixXd dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+    else{
+        // Compute pseudo-inverse of manipulability Jacobian
+        double dampingFactor = getDamping(matManipJ);
+        dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+    }
+    
     Eigen::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
     
     Eigen::VectorXd dq = dampedLSI * (gainMatrix.rows() == 0 ? getDefaultGainMatrix() : gainMatrix) * manipulability_mandel;