Prevent division by zero in carteisian velocity controller

source/RobotAPI/libraries/core/CartesianPositionController.cpp

@@ -41,7 +41,8 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
     if (posLen)
     {
         Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
-        Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+        Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+        Eigen::Vector3f errPos = targetPos - currentPos;
         Eigen::Vector3f posVel =  errPos * KpPos;
         if (maxPosVel > 0)
         {

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -69,7 +69,12 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
     for (int i = 0; i < nullspace.cols(); i++)
     {
-        nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+        float squaredNorm = nullspace.col(i).squaredNorm();
+        // Prevent division by zero
+        if (squaredNorm > 1.0e-32f)
+        {
+            nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+        }
     }
 
     //    Eigen::VectorXf nsv = nullspace * nullspaceVel;
@@ -178,7 +183,7 @@ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobo
 
         if (node->isLimitless() || // limitless joint cannot be out of limits
             std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
-            )
+           )
         {
             continue;
         }