From 45d8e5fa701dba108160ae526071bc9747ec4b17 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit.edu>
Date: Tue, 8 Jun 2021 19:03:19 +0200
Subject: [PATCH] Merge branch 'master' of gitlab.com:ArmarX/RobotAPI; fixed
the controller with damping and dealing with limitless joints.
---
.../NJointTaskSpaceImpedanceDMPController.cpp | 35 ++++++++++++++++---
.../core/CartesianVelocityController.cpp | 10 ++++++
2 files changed, 40 insertions(+), 5 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 91f65179c..74c1e94c4 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -296,13 +296,38 @@ namespace armarx
Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
+ Eigen::VectorXf diffJoints = desiredNullSpaceJointValues;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless() && std::abs(diffJoints(i) - qpos(i)) > M_PI)
+ {
+ if (diffJoints(i) > qpos(i))
+ {
+ diffJoints(i) = -(2 * M_PI - std::abs(diffJoints(i) - qpos(i)));
+ }
+ else
+ {
+ diffJoints(i) = 2 * M_PI - std::abs(diffJoints(i) - qpos(i));
+ }
+ }
+ else
+ {
+ diffJoints(i) -= qpos(i);
+ }
+ }
+ Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(diffJoints) - dnull.cwiseProduct(qvel);
+ Eigen::MatrixXf jtp = jacobi.transpose();
+ auto svd = Eigen::JacobiSVD(jtp, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sv = svd.singularValues();
+ double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+ double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+ ik->setDampedSvdLambda(damping);
+ Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jtp, 2.0);
+ Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
// torque limit
ARMARX_CHECK_EXPRESSION(!targets.empty());
@@ -407,8 +432,8 @@ namespace armarx
goal.at(i) = traj.rbegin()->getPosition(i);
currentJointState.at(i).pos = currentJVS.at(i);
currentJointState.at(i).vel = 0;
+ ARMARX_INFO << goal.at(i);
}
-
trajs.push_back(traj);
nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index f140f9512..9527835a6 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -57,6 +57,11 @@ void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::Carte
_jacobiWithCosts(r, c) = jacobi(r, c) / _jointCosts(c);
}
}
+ auto svd = Eigen::JacobiSVD(_jacobiWithCosts, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sv = svd.singularValues();
+ double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+ double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+ ik->setDampedSvdLambda(damping);
_inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
}
@@ -267,6 +272,11 @@ bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolve
}
if (modifiedJacobi)
{
+ auto svd = Eigen::JacobiSVD(jacobi, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sv = svd.singularValues();
+ double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
+ double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
+ ik->setDampedSvdLambda(damping);
inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
}
--
GitLab