fixed nullspace calculation

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -58,21 +58,21 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
 
     //    ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
-    //    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
     //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
     //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
-    //    Eigen::MatrixXf nullspace = lu_decomp.kernel();
+    Eigen::MatrixXf nullspace = lu_decomp.kernel();
 
 
-    Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
+    //    Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
 
-    //    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();
-    //    }
+    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();
+    }
 
-    Eigen::VectorXf nsv = nullspace * nullspaceVel;
+    //    Eigen::VectorXf nsv = nullspace * nullspaceVel;
 
     //nsv /= kernel.cols();
 

source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp

@@ -45,22 +45,13 @@ void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::V
     Eigen::MatrixXf nullspace = lu_decomp.kernel();
     Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
 
-
-    //        int r = lu_decomp.rank();
-
-    //        Eigen::MatrixXf nullspace = Eigen::MatrixXf::Zero(r, V.cols());
-    //        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
-
-
-    //    for (int i = 0; i < nullspace.cols(); i++)
-    //    {
-    //        nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
-    //    }
-
+    for (int i = 0; i < nullspace.cols(); i++)
+    {
+        nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
+    }
     cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
-    //    nullSpaceVelocityRamp.setState(nsv);
-    nullSpaceVelocityRamp.setState(currentJointVelocity - inv * jacobi * currentJointVelocity);
-    //    ARMARX_IMPORTANT << "nsv " << currentJointVelocity - inv* jacobi* currentJointVelocity;
+    nullSpaceVelocityRamp.setState(nsv);
+
 }
 
 Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)