2323#ifndef MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
2424#define MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
2525
26- #include < cmath>
26+ #include < limits>
27+
28+ #include " control_toolbox/rate_limiter.hpp"
2729
2830namespace mecanum_drive_controller
2931{
@@ -37,16 +39,62 @@ class SpeedLimiter
3739 * \param [in] has_jerk_limits if true, applies jerk limits
3840 * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
3941 * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
40- * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
42+ * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0
4143 * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
4244 * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
4345 * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
4446 */
45- SpeedLimiter (
46- bool has_velocity_limits = false , bool has_acceleration_limits = false ,
47- bool has_jerk_limits = false , double min_velocity = NAN, double max_velocity = NAN,
48- double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN,
49- double max_jerk = NAN);
47+ [[deprecated]] explicit SpeedLimiter (
48+ bool has_velocity_limits = true , bool has_acceleration_limits = true ,
49+ bool has_jerk_limits = true , double min_velocity = std::numeric_limits<double >::quiet_NaN(),
50+ double max_velocity = std::numeric_limits<double>::quiet_NaN(),
51+ double max_deceleration = std::numeric_limits<double>::quiet_NaN(),
52+ double max_acceleration = std::numeric_limits<double>::quiet_NaN(),
53+ double min_jerk = std::numeric_limits<double>::quiet_NaN(),
54+ double max_jerk = std::numeric_limits<double>::quiet_NaN())
55+ {
56+ if (!has_velocity_limits) {
57+ min_velocity = max_velocity = std::numeric_limits<double >::quiet_NaN ();
58+ }
59+ if (!has_acceleration_limits) {
60+ max_deceleration = max_acceleration = std::numeric_limits<double >::quiet_NaN ();
61+ }
62+ if (!has_jerk_limits) {
63+ min_jerk = max_jerk = std::numeric_limits<double >::quiet_NaN ();
64+ }
65+ speed_limiter_ = control_toolbox::RateLimiter<double >(
66+ min_velocity, max_velocity, max_deceleration, max_acceleration,
67+ std::numeric_limits<double >::quiet_NaN (), std::numeric_limits<double >::quiet_NaN (), min_jerk,
68+ max_jerk);
69+ }
70+
71+ /* *
72+ * \brief Constructor
73+ * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
74+ * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
75+ * \param [in] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually
76+ * <= 0
77+ * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
78+ * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0
79+ * \param [in] max_deceleration_reverse Maximum deceleration in reverse direction [m/s^2], usually
80+ * >= 0
81+ * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
82+ * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
83+ *
84+ * \note
85+ * If max_* values are NAN, the respective limit is deactivated
86+ * If min_* values are NAN (unspecified), defaults to -max
87+ * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used
88+ */
89+ explicit SpeedLimiter (
90+ double min_velocity, double max_velocity, double max_acceleration_reverse,
91+ double max_acceleration, double max_deceleration, double max_deceleration_reverse,
92+ double min_jerk, double max_jerk)
93+ {
94+ speed_limiter_ = control_toolbox::RateLimiter<double >(
95+ min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration,
96+ max_deceleration_reverse, min_jerk, max_jerk);
97+ }
5098
5199 /* *
52100 * \brief Limit the velocity and acceleration
@@ -56,14 +104,17 @@ class SpeedLimiter
56104 * \param [in] dt Time step [s]
57105 * \return Limiting factor (1.0 if none)
58106 */
59- double limit (double & v, double v0, double v1, double dt);
107+ double limit (double & v, double v0, double v1, double dt)
108+ {
109+ return speed_limiter_.limit (v, v0, v1, dt);
110+ }
60111
61112 /* *
62113 * \brief Limit the velocity
63114 * \param [in, out] v Velocity [m/s]
64115 * \return Limiting factor (1.0 if none)
65116 */
66- double limit_velocity (double & v);
117+ double limit_velocity (double & v) { return speed_limiter_. limit_value (v); }
67118
68119 /* *
69120 * \brief Limit the acceleration
@@ -72,7 +123,10 @@ class SpeedLimiter
72123 * \param [in] dt Time step [s]
73124 * \return Limiting factor (1.0 if none)
74125 */
75- double limit_acceleration (double & v, double v0, double dt);
126+ double limit_acceleration (double & v, double v0, double dt)
127+ {
128+ return speed_limiter_.limit_first_derivative (v, v0, dt);
129+ }
76130
77131 /* *
78132 * \brief Limit the jerk
@@ -83,25 +137,13 @@ class SpeedLimiter
83137 * \return Limiting factor (1.0 if none)
84138 * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
85139 */
86- double limit_jerk (double & v, double v0, double v1, double dt);
140+ double limit_jerk (double & v, double v0, double v1, double dt)
141+ {
142+ return speed_limiter_.limit_second_derivative (v, v0, v1, dt);
143+ }
87144
88145private:
89- // Enable/Disable velocity/acceleration/jerk limits:
90- bool has_velocity_limits_;
91- bool has_acceleration_limits_;
92- bool has_jerk_limits_;
93-
94- // Velocity limits:
95- double min_velocity_;
96- double max_velocity_;
97-
98- // Acceleration limits:
99- double min_acceleration_;
100- double max_acceleration_;
101-
102- // Jerk limits:
103- double min_jerk_;
104- double max_jerk_;
146+ control_toolbox::RateLimiter<double > speed_limiter_; // Instance of the new RateLimiter
105147};
106148
107149} // namespace mecanum_drive_controller
0 commit comments