diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 9a9ec7038de7169ced6e8725ce4215016e3a3d5e..ec9a18f74a1ace2886938492d36b390c9b345e3b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/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)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index cdb5fc3666a89f134f59772f13a0b65c2bb99c66..62e5740e433786088c18dd6a1a6b9d7c6fcc63cf 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/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)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 264db7d88a2101edcf3d674e3ccfa4abbfae8734..4708d2ac4ef23821ca246d4991458191b1eb6fb5 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/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)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
index b5ed838bd322bd78581b68c52328454d3556dcb9..21cc73a781df9261d33afe050728680e7d88103d 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
+++ b/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;
}
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
index f0488c9e3e4c743b1add06c1e9bbe13cf7a92ebc..33128cf1f5f1f360cc163ad07aff60cf99f4afdb 100644
--- a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
+++ b/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;
}
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index 0e889f0c0efec591d6e1caf6a1b2d20fe0ef6f11..f50dedc362145b2ef1aeb812f98233ba0f6da8d4 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/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);
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
index 487018abde075ae58f1eca8696b84a76685e002a..5ffd4e0aed928a3e9292cabbb25087cf91758b6f 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
+++ b/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);