@@ -94,30 +94,57 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
9494 }
9595}
9696
97+ /* *
98+ * Returns a zero vector if the given vector is within the specified
99+ * distance from the origin. The remaining distance between the deadband and the
100+ * maximum distance is scaled from the origin to the maximum distance.
101+ *
102+ * @param value Value to clip.
103+ * @param deadband Distance from origin.
104+ * @param maxMagnitude The maximum distance from the origin of the input
105+ * (defaults to 1). Can be infinite.
106+ * @return The value after the deadband is applied.
107+ */
108+ template <typename T, int N>
109+ requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
110+ Eigen::Vector<T, N> ApplyDeadband (const Eigen::Vector<T, N>& value, T deadband,
111+ T maxMagnitude = T{1.0 }) {
112+ if constexpr (std::is_arithmetic_v<T>) {
113+ if (value.norm () < T{1e-9 }) {
114+ return Eigen::Vector<T, N>::Zero ();
115+ }
116+ return value.normalized () *
117+ ApplyDeadband (value.norm (), deadband, maxMagnitude);
118+ } else {
119+ const Eigen::Vector<double , N> asDouble = value.template cast <double >();
120+ const Eigen::Vector<double , N> processed =
121+ ApplyDeadband (asDouble, deadband.value (), maxMagnitude.value ());
122+ return processed.template cast <T>();
123+ }
124+ }
125+
97126/* *
98127 * Raises the input to the power of the given exponent while preserving its
99128 * sign.
100129 *
101130 * The function normalizes the input value to the range [0, 1] based on the
102- * maximum magnitude, raises it to the power of the exponent, then scales the
103- * result back to the original range and copying the sign. This keeps the value
104- * in the original range and gives consistent curve behavior regardless of the
105- * input value's scale.
131+ * maximum magnitude so that the output stays in the range.
106132 *
107133 * This is useful for applying smoother or more aggressive control response
108134 * curves (e.g. joystick input shaping).
109135 *
110136 * @param value The input value to transform.
111137 * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared
112138 * curve). Must be positive.
113- * @param maxMagnitude The maximum expected absolute value of input. Must be
114- * positive.
139+ * @param maxMagnitude The maximum expected absolute value of input (defaults to
140+ * 1). Must be positive.
115141 * @return The transformed value with the same sign and scaled to the input
116142 * range.
117143 */
118144template <typename T>
119145 requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
120- constexpr T CopySignPow (T value, double exponent, T maxMagnitude = T{1.0 }) {
146+ constexpr T CopyDirectionPow (T value, double exponent,
147+ T maxMagnitude = T{1.0 }) {
121148 if constexpr (std::is_arithmetic_v<T>) {
122149 return gcem::copysign (
123150 gcem::pow (gcem::abs (value) / maxMagnitude, exponent) * maxMagnitude,
@@ -130,6 +157,42 @@ constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) {
130157 }
131158}
132159
160+ /* *
161+ * Raises the norm of the input to the power of the given exponent while
162+ * preserving its direction.
163+ *
164+ * The function normalizes the input value to the range [0, 1] based on the
165+ * maximum magnitude so that the output stays in the range.
166+ *
167+ * This is useful for applying smoother or more aggressive control response
168+ * curves (e.g. joystick input shaping).
169+ *
170+ * @param value The input vector to transform.
171+ * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared
172+ * curve). Must be positive.
173+ * @param maxMagnitude The maximum expected distance from origin of input
174+ * (defaults to 1). Must be positive.
175+ * @return The transformed value with the same direction and norm scaled to
176+ * the input range.
177+ */
178+ template <typename T, int N>
179+ requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
180+ Eigen::Vector<T, N> CopyDirectionPow (const Eigen::Vector<T, N>& value,
181+ double exponent, T maxMagnitude = T{1.0 }) {
182+ if constexpr (std::is_arithmetic_v<T>) {
183+ if (value.norm () < T{1e-9 }) {
184+ return Eigen::Vector<T, N>::Zero ();
185+ }
186+ return value.normalized () *
187+ CopyDirectionPow (value.norm (), exponent, maxMagnitude);
188+ } else {
189+ const Eigen::Vector<double , N> asDouble = value.template cast <double >();
190+ const Eigen::Vector<double , N> processed =
191+ CopyDirectionPow (asDouble, exponent, maxMagnitude.value ());
192+ return processed.template cast <T>();
193+ }
194+ }
195+
133196/* *
134197 * Returns modulus of input.
135198 *
@@ -279,6 +342,7 @@ constexpr Translation2d SlewRateLimit(const Translation2d& current,
279342 }
280343 if (dist > maxVelocity * dt) {
281344 // Move maximum allowed amount in direction of the difference
345+ // NOLINTNEXTLINE(bugprone-integer-division)
282346 return current + diff * (maxVelocity * dt / dist);
283347 }
284348 return next;
@@ -309,6 +373,7 @@ constexpr Translation3d SlewRateLimit(const Translation3d& current,
309373 }
310374 if (dist > maxVelocity * dt) {
311375 // Move maximum allowed amount in direction of the difference
376+ // NOLINTNEXTLINE(bugprone-integer-division)
312377 return current + diff * (maxVelocity * dt / dist);
313378 }
314379 return next;
0 commit comments