From 7d2e3db2a35cf2897ad3e9c8ec76bde8f10b18aa Mon Sep 17 00:00:00 2001
From: Noemie Jaquier <noemie.jaquier@gmail.com>
Date: Sun, 7 Mar 2021 13:47:41 +0100
Subject: [PATCH] Fixed weight multiplication
---
.../AbstractManipulabilityTracking.cpp | 1 -
.../SingleChainManipulabilityTracking.cpp | 12 ++++++++++--
2 files changed, 10 insertions(+), 3 deletions(-)
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
index 96fd7b81a..fb9c8c7c1 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
@@ -182,7 +182,6 @@ Eigen::MatrixXd AbstractManipulabilityTracking::getJointsLimitsWeightMatrix(cons
weights(i) = 1.;
}
else {
- // TODO add condition on velocities
gradient(i) = std::pow(jointLimitsHigh(i) - jointLimitsLow(i), 2) * (2*jointAngles(i) - jointLimitsHigh(i) - jointLimitsLow(i)) /
( 4 * std::pow(jointLimitsHigh(i) - jointAngles(i), 2) * std::pow(jointAngles(i) - jointLimitsLow(i), 2) );
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
index 468f87822..6fc9b3118 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
@@ -81,14 +81,22 @@ Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen
Eigen::Tensor<double, 3> manipJ_sub = subCube(manipJ);
Eigen::MatrixXd matManipJ = computeManipulabilityJacobianMandelNotation(manipJ_sub);
+ Eigen::MatrixXd dampedLSI;
if (jointLimitAvoidance)
{
// Compute weighted manipulability Jacobian
Eigen::MatrixXd jointsWeightMatrix = getJointsLimitsWeightMatrix(manipulability->getJointAngles(), manipulability->getJointLimitsLow(), manipulability->getJointLimitsHigh());
matManipJ = matManipJ * jointsWeightMatrix;
+ // Compute pseudo-inverse of weighted manipulability Jacobian
+ double dampingFactor = getDamping(matManipJ);
+ dampedLSI = jointsWeightMatrix * dampedLeastSquaresInverse(matManipJ, dampingFactor);
}
- double dampingFactor = getDamping(matManipJ);
- Eigen::MatrixXd dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+ else{
+ // Compute pseudo-inverse of manipulability Jacobian
+ double dampingFactor = getDamping(matManipJ);
+ dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+ }
+
Eigen::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
Eigen::VectorXd dq = dampedLSI * (gainMatrix.rows() == 0 ? getDefaultGainMatrix() : gainMatrix) * manipulability_mandel;
--
GitLab