From 69d53d065dd8fc48c8d3422d80a08cd1b5b6eab9 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Fri, 16 Nov 2018 11:37:23 +0100
Subject: [PATCH] added support for limitless joints fixed joint limit check:
not only near joint limit is considered a violation but also exceeding the
joint limit
---
.../libraries/core/CartesianVelocityController.cpp | 13 +++++++------
1 file changed, 7 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index c3cd6fb23..40dad3107 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -174,21 +174,22 @@ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobo
for (size_t i = 0; i < size; ++i)
{
- auto& node = rns->getNode(i);
+ auto& node = rns->getNode(static_cast<int>(i));
- if (std::abs(jointVel(i)) < 0.001f)
+ if (node->isLimitless() || // limitless joint cannot be out of limits
+ std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
+ )
{
continue;
}
- if ((std::abs(node->getJointValue() - node->getJointLimitHigh()) < jointLimitCheckAccuracy && jointVel(i) > 0)
- || (std::abs(node->getJointValue() - node->getJointLimitLow()) < jointLimitCheckAccuracy && jointVel(i) < 0))
+ if ((node->getJointValue() >= node->getJointLimitHigh() - jointLimitCheckAccuracy && jointVel(i) > 0)
+ || (node->getJointValue() <= node->getJointLimitLow() + jointLimitCheckAccuracy && jointVel(i) < 0))
{
- for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
+ for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
{
jacobi(k, i) = 0.0f;
}
- // ARMARX_INFO << deactivateSpam(0.5, node->getName()) << " joint " << node->getName() << " is at limit -\n" << inv << "\n initial inv jacobi:\n" << initialInvJac << "\n target cart vel:\n" << cartesianVel << "\njacobi:\n" << jacobi;
modifiedJacobi = true;
}
}
--
GitLab