From 2284f5e712b965c079ca1df1470a403791b51d5f Mon Sep 17 00:00:00 2001
From: Sonja Marahrens <sonja.marahrens>
Date: Tue, 3 Jul 2018 13:52:57 +0200
Subject: [PATCH] fixed nullspace calculation
---
.../core/CartesianVelocityController.cpp | 18 ++++++++--------
.../CartesianVelocityControllerWithRamp.cpp | 21 ++++++-------------
2 files changed, 15 insertions(+), 24 deletions(-)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 62e5740e4..5ba9cd5d3 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/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();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 4708d2ac4..e1d5154f4 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/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)
--
GitLab