From 06ce27514e4235b1e686f48861038a535eb3738e Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Thu, 5 Sep 2019 12:20:33 +0200
Subject: [PATCH] minor tweaks to ramped-acceleration velocity controller
---
.../components/units/RobotUnit/BasicControllers.cpp | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 5be317760..0ec81ab38 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -371,9 +371,9 @@ namespace armarx
const double curverror = clampedTargetV - currentV;
if (std::abs(curverror) < directSetVLimit)
{
- double nextAcc = (clampedTargetV - currentV) / useddt;
- double nextJerk = (nextAcc - currentAcc) / useddt;
- Output result {clampedTargetV, nextAcc, nextJerk};
+ // double nextAcc = (clampedTargetV - currentV) / useddt; // calculated acc is unstable!!!
+ // double nextJerk = (nextAcc - currentAcc) / useddt;
+ Output result {clampedTargetV, 0, 0};
return result;
}
@@ -1336,7 +1336,7 @@ namespace armarx
const double dec = std::abs(vsquared / 2.0 / wayToGo);
const double vel = currentV - sign(currentV) * dec * upperDt;
nextv = boost::algorithm::clamp(vel, -maxV, maxV);
- // ARMARX_INFO /*<< deactivateSpam(0.1) */ << "clamped new Vel: " << VAROUT(nextv) << VAROUT(currentV);
+ // ARMARX_INFO /*<< deactivateSpam(0.1)*/ << "clamped new Vel: " << VAROUT(nextv) << VAROUT(currentV);
if (sign(currentV) != sign(nextv))
{
// ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping"; //stop now
@@ -1357,8 +1357,8 @@ namespace armarx
// ARMARX_INFO << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
nextv = 0;
}
- double nextAcc = (nextv - currentV) / useddt; // doesnt work with calculated acc
- double nextJerk = (nextAcc - currentAcc) / useddt;
+ // double nextAcc = (nextv - currentV) / useddt; // doesnt work with calculated acc!!
+ // double nextJerk = (nextAcc - currentAcc) / useddt;
//the next velocity will not violate the pos bounds harder than they are already
return Output {nextv, 0, 0};
}
--
GitLab