From 704fba38bae9d9a1c6b06362bac1c7c8dcddc151 Mon Sep 17 00:00:00 2001
From: Fabian Paus <fabian.paus@kit.edu>
Date: Thu, 29 Nov 2018 11:33:36 +0100
Subject: [PATCH] Prevent division by zero in carteisian velocity controller
---
.../libraries/core/CartesianPositionController.cpp | 3 ++-
.../libraries/core/CartesianVelocityController.cpp | 9 +++++++--
2 files changed, 9 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 3ffd16046..4559e139c 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -41,7 +41,8 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
if (posLen)
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+ Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+ Eigen::Vector3f errPos = targetPos - currentPos;
Eigen::Vector3f posVel = errPos * KpPos;
if (maxPosVel > 0)
{
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 40dad3107..3ec8849c6 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -69,7 +69,12 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
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();
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ }
}
// Eigen::VectorXf nsv = nullspace * nullspaceVel;
@@ -178,7 +183,7 @@ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobo
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;
}
--
GitLab