diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp index 5be317760916ca7c3138c10019d4ec5c7c14349a..0ec81ab3895bc1037141ee6798c01effcbee45eb 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}; }