Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp

@@ -86,14 +86,13 @@ namespace armarx
 
     void NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
     {
-        ARMARX_IMPORTANT << "rtPreActivateController start";
         Eigen::VectorXf currentJointVelocities(velocitySensors.size());
         for (size_t i = 0; i < velocitySensors.size(); i++)
         {
             currentJointVelocities(i) = *velocitySensors.at(i);
         }
         controller->setCurrentJointVelocity(currentJointVelocities);
-        ARMARX_IMPORTANT << "rtPreActivateController end";
+        ARMARX_IMPORTANT << "initial joint velocities: " << currentJointVelocities.transpose();
     }
 
     void NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -57,19 +57,28 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     //ARMARX_IMPORTANT << jacobi;
     inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
 
-    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+    //    ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
+    //    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::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::MatrixXf nullspace = lu_decomp.kernel();
+
+
+    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 = nullspace * nullspaceVel;
 
     //nsv /= kernel.cols();
 
+
     Eigen::VectorXf jointVel = inv * cartesianVel;
+
     jointVel += nsv;
 
     return jointVel;
@@ -98,7 +107,9 @@ Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Ei
 {
     Eigen::VectorXf regularization = calculateRegularization(mode);
     Eigen::VectorXf vel = cartesianVel * regularization;
+
     return KpScale * vel.norm() * calculateJointLimitAvoidance();
+
 }
 
 void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)

source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp

@@ -23,6 +23,8 @@
 
 #include "CartesianVelocityControllerWithRamp.h"
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 using namespace armarx;
 
 CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
@@ -36,17 +38,29 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
 void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
 {
     Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
+    Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
+
 
     Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
     Eigen::MatrixXf nullspace = lu_decomp.kernel();
     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();
-    }
+
+
+    //        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();
+    //    }
 
     cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
-    nullSpaceVelocityRamp.setState(nsv);
+    //    nullSpaceVelocityRamp.setState(nsv);
+    nullSpaceVelocityRamp.setState(currentJointVelocity - inv * jacobi * currentJointVelocity);
+    //    ARMARX_IMPORTANT << "nsv " << currentJointVelocity - inv* jacobi* currentJointVelocity;
 }
 
 Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)

source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp

@@ -23,6 +23,8 @@
 
 #include "CartesianVelocityRamp.h"
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 using namespace armarx;
 
 CartesianVelocityRamp::CartesianVelocityRamp()
@@ -60,6 +62,7 @@ Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, flo
     }
 
     state += delta / factor;
+    //    ARMARX_IMPORTANT << "CartRamp state " << state;
     return state;
 }
 

source/RobotAPI/libraries/core/JointVelocityRamp.cpp

@@ -22,6 +22,7 @@
  */
 
 #include "JointVelocityRamp.h"
+#include <ArmarXCore/core/logging/Logging.h>
 
 using namespace armarx;
 
@@ -36,13 +37,18 @@ void JointVelocityRamp::setState(const Eigen::VectorXf& state)
 
 Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
 {
+    if (dt <= 0)
+    {
+        return state;
+    }
 
     Eigen::VectorXf delta = target - state;
 
-    float factor = delta.norm() / maxAcceleration;
+    float factor = delta.norm() / maxAcceleration / dt;
     factor = std::max(factor, 1.f);
 
-    state += delta / factor * dt;
+    state += delta / factor;
+    //    ARMARX_IMPORTANT << "JointRamp state " << state;
     return state;
 
 }

source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp

@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE(test1)
     jointVel << 1, 1, 1, 1, 1, 1, 1, 1;
 
     Eigen::VectorXf targetTcpVel(6);
-    targetTcpVel << 1, 0, 0, 0, 0, 0;
+    targetTcpVel << 0, 0, 0, 0, 0, 0;
 
     ARMARX_IMPORTANT << rns->getJointValues();
     Eigen::VectorXf vel = h->calculate(targetTcpVel, jointVel, VirtualRobot::IKSolver::CartesianSelection::All);

source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp

@@ -50,7 +50,7 @@ BOOST_AUTO_TEST_CASE(CartesianVelocityRampTest)
     for (size_t i = 0; i < 30; i++)
     {
         state = c.update(target, dt);
-        ARMARX_IMPORTANT << "state:" << state;
+        ARMARX_IMPORTANT << "state:" << state.transpose();
     }
 
 }
@@ -77,20 +77,20 @@ BOOST_AUTO_TEST_CASE(test1)
 
 
     Eigen::VectorXf currentjointVel(8);
-    currentjointVel << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
+    currentjointVel << 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0;
 
 
     Eigen::VectorXf targetTcpVel(6);
-    targetTcpVel << 20, 0, 0, 0, 0, 0;
+    targetTcpVel << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
 
-    CartesianVelocityControllerWithRampPtr h(new CartesianVelocityControllerWithRamp(rns, currentjointVel, VirtualRobot::IKSolver::CartesianSelection::All, 100, 1, 1));
+    CartesianVelocityControllerWithRampPtr h(new CartesianVelocityControllerWithRamp(rns, currentjointVel, VirtualRobot::IKSolver::CartesianSelection::All, 100, 1, 0.1));
     Eigen::MatrixXf jacobi = h->controller.ik->getJacobianMatrix(rns->getTCP(), VirtualRobot::IKSolver::CartesianSelection::All);
     ARMARX_IMPORTANT << "Initial TCP Vel: " << (jacobi * currentjointVel).transpose();
 
 
-    float dt = 0.1f;
+    float dt = 0.01f;
 
-    for (size_t n = 0; n < 10; n++)
+    for (size_t n = 0; n < 300; n++)
     {
         //ARMARX_IMPORTANT << rns->getJointValues();
         Eigen::VectorXf vel = h->calculate(targetTcpVel, 2, dt);