diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp index fb1dcf83cf6e9ce00401bbcef75d0518f7226612..ee47310699ea73207a266a6db7319dd2fa09a70d 100644 --- a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp +++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp @@ -128,4 +128,5 @@ void armarx::TopicTimingClient::reportSmall(const topic_timing::SmallData& data, void armarx::TopicTimingClient::reportBig(const topic_timing::BigData& data, const Ice::Current&) { IceUtil::Time sentTime = IceUtil::Time::microSeconds(data.sentTimestamp); + (void) sentTime; } diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp index f779b851b17ecfdeb4316013234b62b7696e8cea..94c2946f758d1a387f49000c15d38082ab633877 100644 --- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp +++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp @@ -952,14 +952,21 @@ namespace armarx const double newAcceleration = (newState == State::Keep) ? currentAcc : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc); result.acceleration = newAcceleration; result.jerk = usedJerk; + const double usedDeceleration = hardBrakingNeeded ? -std::abs(currentV * currentV / 2.f / math::MathUtils::LimitTo(positionError, 0.0001)) : newAcceleration; + (void) usedDeceleration; + const bool decelerate = std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV] hardBrakingNeeded || sign(posErrorIfBrakingNow) != signV; // we are moving away from the target + (void) decelerate; + const double deltaVel = ctrlutil::v(useddt, 0, newAcceleration, usedJerk);//signV * newAcceleration * useddt; + (void) deltaVel; + double newTargetVelRampCtrl = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);//boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV); newTargetVelRampCtrl = std::clamp(newTargetVelRampCtrl, -maxV, maxV); bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl); //|| @@ -1052,6 +1059,7 @@ namespace armarx const double posWhenBrakingWithCustomJerk = ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk); // const double brakingDistanceWithCustomJerk = ctrlutil::s(tmp_t, 0, currentV, tmp_acc, curVDir * tmp_jerk); const double posWhenBrakingWithFixedJerk = ctrlutil::s(brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk); + (void) calculatedJerk, (void) posWhenBrakingWithCustomJerk, (void) posWhenBrakingWithFixedJerk; ARMARX_INFO << VAROUT((int)state) << VAROUT(distance) << VAROUT(brakingDistance);//VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk); @@ -1100,12 +1108,15 @@ namespace armarx double accAfterBraking = ctrlutil::a(t_to_0, currentAcc, curVDir * newJerk); const double posWhenBrakingNow = ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk); const double simpleBrakingDistance = ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk); + (void) tmpV, (void) vAfterBraking, (void) accAfterBraking, (void) posWhenBrakingNow, (void) simpleBrakingDistance; double acc_at_t = ctrlutil::a(t_to_0, std::abs(currentAcc), -newJerk); std::tie(t, a0, newJerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV); double at = ctrlutil::a(t, a0, newJerk); double vt = ctrlutil::v(t, currentV, a0, newJerk); double st = ctrlutil::s(t, targetPosition - currentPosition, currentV, a0, newJerk); + (void) at, (void) vt, (void) st; + if (this->state <= State::Keep) { newState = State::DecAccIncJerk; @@ -1295,6 +1306,7 @@ namespace armarx VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAccelerationAndPositionBounds::run() const { const double useddt = std::min(std::abs(dt), std::abs(maxDt)); + (void) useddt; // if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard) // { @@ -1513,6 +1525,7 @@ namespace armarx double newAcc = currentAcc + jerk * dt; double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk); double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk); // assumes that fixed jerk would be used, which is not the case here + (void) accAtEnd; std::vector<State> states; std::vector<State> states2;