From 451add79533d65b3120c0cfd750d2e19b64dd094 Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Tue, 23 Jun 2020 18:59:00 +0200
Subject: [PATCH] fixed the error of zero division
---
.../NJointBimanualObjLevelController.cpp | 22 +++++++++++++++++--
1 file changed, 20 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 37cf71bfc..eb8d9b4dd 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -10,6 +10,8 @@ namespace armarx
NJointBimanualObjLevelController::NJointBimanualObjLevelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
ARMARX_INFO << "Preparing ... bimanual ";
+ ARMARX_INFO << "I am here";
+
useSynchronizedRtRobot();
cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
// ARMARX_CHECK_EXPRESSION(prov);
@@ -391,8 +393,23 @@ namespace armarx
deltaPoseForWrenchControl.setZero(12);
for (size_t i = 0; i < 12; ++i)
{
- deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
- // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
+ if (KpImpedance(i) == 0)
+ {
+ deltaPoseForWrenchControl(i) = 0;
+ }
+ else
+ {
+ deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
+ if (deltaPoseForWrenchControl(i) > 0.1)
+ {
+ deltaPoseForWrenchControl(i) = 0.1;
+ }
+ else if (deltaPoseForWrenchControl(i) < -0.1)
+ {
+ deltaPoseForWrenchControl(i) = -0.1;
+ }
+ // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
+ }
}
// ------------------------------------------- current tcp pose -------------------------------------------
@@ -787,6 +804,7 @@ namespace armarx
void NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
{
+
Eigen::VectorXf setpoint;
setpoint.setZero(value.size());
ARMARX_CHECK_EQUAL(value.size(), 12);
--
GitLab