From f08f2a340d959c8ea98795c774955a5b967b7cb0 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Thu, 9 Sep 2021 11:05:24 +0200 Subject: [PATCH] motion_planning: In VelovitySmoothing, mark const functions const --- src/lib/motion_planning/VelocitySmoothing.cpp | 18 +++++++++--------- src/lib/motion_planning/VelocitySmoothing.hpp | 18 +++++++++--------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/lib/motion_planning/VelocitySmoothing.cpp b/src/lib/motion_planning/VelocitySmoothing.cpp index 7dfd951e2e..9a29e6a1a7 100644 --- a/src/lib/motion_planning/VelocitySmoothing.cpp +++ b/src/lib/motion_planning/VelocitySmoothing.cpp @@ -55,7 +55,7 @@ void VelocitySmoothing::reset(float accel, float vel, float pos) _state_init = _state; } -float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max) +float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max) const { /* Check maximum acceleration, saturate and recompute T1 if needed */ float accel_T1 = a0 + j_max * T1; @@ -71,7 +71,7 @@ float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, flo return T1_new; } -float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) +float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) const { float delta = 2.f * a0 * a0 + 4.f * j_max * v3; @@ -101,7 +101,7 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) return math::max(T1, 0.f); } -float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max) +float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max) const { float a = -j_max; float b = j_max * T123 - a0; @@ -138,7 +138,7 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, } -float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float j_max) +float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float j_max) const { float T2 = 0.f; @@ -151,13 +151,13 @@ float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float return math::max(T2, 0.f); } -float VelocitySmoothing::computeT2(float T123, float T1, float T3) +float VelocitySmoothing::computeT2(float T123, float T1, float T3) const { float T2 = T123 - T1 - T3; return math::max(T2, 0.f); } -float VelocitySmoothing::computeT3(float T1, float a0, float j_max) +float VelocitySmoothing::computeT3(float T1, float a0, float j_max) const { float T3 = a0 / j_max + T1; return math::max(T3, 0.f); @@ -179,7 +179,7 @@ void VelocitySmoothing::updateDurations(float vel_setpoint) } } -int VelocitySmoothing::computeDirection() +int VelocitySmoothing::computeDirection() const { // Compute the velocity at which the trajectory will be // when the acceleration will be zero @@ -197,7 +197,7 @@ int VelocitySmoothing::computeDirection() return direction; } -float VelocitySmoothing::computeVelAtZeroAcc() +float VelocitySmoothing::computeVelAtZeroAcc() const { float vel_zero_acc = _state.v; @@ -225,7 +225,7 @@ void VelocitySmoothing::updateDurationsMinimizeTotalTime() _T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1); } -Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) +Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const { Trajectory traj; float jt = d * j; diff --git a/src/lib/motion_planning/VelocitySmoothing.hpp b/src/lib/motion_planning/VelocitySmoothing.hpp index ba817a8b98..25799d1ce9 100644 --- a/src/lib/motion_planning/VelocitySmoothing.hpp +++ b/src/lib/motion_planning/VelocitySmoothing.hpp @@ -141,43 +141,43 @@ private: * Compute the direction of the jerk to be applied in order to drive the current state * to the desired one */ - int computeDirection(); + int computeDirection() const; /** * Compute the velocity at which the trajectory will be if the maximum jerk is applied * during the time required to cancel the current acceleration */ - float computeVelAtZeroAcc(); + float computeVelAtZeroAcc() const; /** * Compute increasing acceleration time */ - inline float computeT1(float a0, float v3, float j_max, float a_max); + inline float computeT1(float a0, float v3, float j_max, float a_max) const; /** * Compute increasing acceleration time using total time constraint */ - inline float computeT1(float T123, float a0, float v3, float j_max, float a_max); + inline float computeT1(float T123, float a0, float v3, float j_max, float a_max) const; /** * Saturate T1 in order to respect the maximum acceleration constraint */ - inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max); + inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max) const; /** * Compute constant acceleration time */ - inline float computeT2(float T1, float T3, float a0, float v3, float j_max); + inline float computeT2(float T1, float T3, float a0, float v3, float j_max) const; /** * Compute constant acceleration time using total time constraint */ - inline float computeT2(float T123, float T1, float T3); + inline float computeT2(float T123, float T1, float T3) const; /** * Compute decreasing acceleration time */ - inline float computeT3(float T1, float a0, float j_max); + inline float computeT3(float T1, float a0, float j_max) const; /** * Compute the jerk, acceleration, velocity and position @@ -189,7 +189,7 @@ private: * @param t current time * @param d direction */ - inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d); + inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const; /* Input */ float _vel_sp{0.0f};