From f3ffff900d68c1202700368f8fb92c36eb8d6f27 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Tue, 17 Mar 2026 03:49:28 +0000 Subject: [PATCH 1/6] Reversing needs to choose the right signs for articulation as acos is multivalued and we didn't properly account for the sign there. For bicycle controller we should be using tan i nstead of atan2 as we want to be between -pi/2 and pi/2 and not in all 4 quadrants with atan2 --- derivations/articulated_model.md | 5 +-- derivations/bicycle_model.md | 5 ++- src/articulated_model.cpp | 14 +++++++-- src/bicycle_model.cpp | 9 ++++-- test/test_articulated_model.cpp | 54 ++++++++++++++++++++++++++++++++ test/test_bicycle_model.cpp | 52 ++++++++++++++++++++++++++++++ test/test_python_bindings.py | 34 ++++++++++++++++++++ 7 files changed, 162 insertions(+), 11 deletions(-) diff --git a/derivations/articulated_model.md b/derivations/articulated_model.md index 265d2bb..206efb7 100644 --- a/derivations/articulated_model.md +++ b/derivations/articulated_model.md @@ -130,12 +130,13 @@ $$\cos(\gamma + \phi) = \frac{L_r(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + $$\gamma + \phi = \arccos\!\left(\frac{L_r(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right)$$ -$$\boxed{\gamma = \arccos\!\left(\frac{L_r\,(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f)} \tag{13}$$ +$$\boxed{\gamma = \operatorname{sign}(v)\cdot\arccos\!\left(\frac{L_r\,(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f)} \tag{13}$$ With $\dot{\gamma} = 0$ (current implementation): -$$\gamma = \arccos\!\left(\frac{-L_r\,\omega}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f) \tag{14}$$ +$$\gamma = \operatorname{sign}(v)\cdot\arccos\!\left(\frac{-L_r\,\omega}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f) \tag{14}$$ +> **Branch selection:** The cosine inversion $\cos(\gamma+\phi) = c$ has two solutions $\gamma = \pm\arccos(c) - \phi$. For $v > 0$, $\phi = \operatorname{atan2}(v, \omega L_f) > 0$ and the $+$ branch yields the small physical angle. For $v < 0$, $\phi$ is negative and the $+$ branch pushes $\gamma$ near $\pm\pi$; the $-$ branch recovers the correct small angle. Hence $\operatorname{sign}(v)$ selects the appropriate branch in both directions. --- ## Turning radii diff --git a/derivations/bicycle_model.md b/derivations/bicycle_model.md index 2e8b1b5..87fc16c 100644 --- a/derivations/bicycle_model.md +++ b/derivations/bicycle_model.md @@ -31,9 +31,9 @@ This follows from the geometry: the rear axle center traces a circle of radius $ Invert the forward relation: -$$\delta = \operatorname{atan2}(\omega \cdot L,\; v)$$ +$$\delta = \arctan\!\left(\frac{\omega \cdot L}{v}\right)$$ -$\operatorname{atan2}$ is used instead of $\arctan(\omega L / v)$ to handle all quadrants correctly, including reverse motion. +$\arctan$ (not $\operatorname{atan2}$) is used intentionally: the result must lie in $(-\pi/2,\, \pi/2)$, which is the physically valid range for a steering angle. $\operatorname{atan2}(\omega L, v)$ would add $\pm\pi$ when $v < 0$, placing $\delta$ in the wrong quadrant during reverse motion. ### Turning radius @@ -90,5 +90,4 @@ $$\omega_{\text{fr}} = \frac{\omega \cdot \operatorname{copysign}\!\left(\sqrt{( | $\|R\| < W/2$ | ICR between rear wheels. Inner wheel reverses direction (handled by $\operatorname{copysign}$) | ## Future Work - Add diagrams to the readme for visualization diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index 7ce6c2c..1e4e884 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -52,10 +52,18 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( std::numeric_limits::infinity()}; } + // The harmonic-addition inversion of eq. (12) yields two solutions: + // gamma + phi = +/-arccos(Lr*(gamma_dot - omega) / R) + // For v > 0, phi = atan2(v, omega*Lf) > 0 and the + branch gives the + // small physical angle. For v < 0, phi is negative and the + branch + // pushes gamma toward +/-pi. copysign selects + for forward, - for reverse. double articulation_angle = - std::acos( - articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / - std::sqrt(square(angular_velocity_rad_s) * square(articulation_to_front_axle_m_) + square(linear_velocity_m_s))) - + std::copysign( + std::acos( + articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / + std::sqrt( + square(angular_velocity_rad_s) * square(articulation_to_front_axle_m_) + square(linear_velocity_m_s))), + linear_velocity_m_s) - std::atan2(linear_velocity_m_s, angular_velocity_rad_s * articulation_to_front_axle_m_); double cos_articulation = std::cos(articulation_angle); diff --git a/src/bicycle_model.cpp b/src/bicycle_model.cpp index 8b60268..53d73b1 100644 --- a/src/bicycle_model.cpp +++ b/src/bicycle_model.cpp @@ -47,9 +47,12 @@ BicycleSteeringState BicycleModel::bodyVelocityToSteering(double linear_velocity wheel_rad_s}; } - // steering_angle = atan2(omega * L, v) inverts the forward kinematic - // relation omega = v * tan(steering_angle) / L - double steering_angle = std::atan2(angular_velocity * wheelbase_m_, linear_velocity); + // steering_angle = atan(omega * L / v) inverts the forward kinematic + // relation omega = v * tan(steering_angle) / L. + // atan (not atan2) is intentional: the result must stay in (-pi/2, pi/2), + // which is the valid physical range for a steering angle. atan2 would add + // +/-pi when v < 0, placing the result in the wrong quadrant for reverse. + double steering_angle = std::atan(angular_velocity * wheelbase_m_ / linear_velocity); double turning_radius = turningRadius(steering_angle); double half_track = track_width_m_ / 2.0; diff --git a/test/test_articulated_model.cpp b/test/test_articulated_model.cpp index fc7506b..70d1b18 100644 --- a/test/test_articulated_model.cpp +++ b/test/test_articulated_model.cpp @@ -151,4 +151,58 @@ TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - straight line (v, 0) no CHECK(std::isinf(result.rear_axle_turning_radius_m)); } +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - reverse left turn") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + // Reversing (v < 0) + CCW yaw (omega > 0): articulation angle must be negative (joint bends right) + // |gamma| matches the forward case by symmetry: gamma = -0.6435011088 + auto result = model.bodyVelocityToVehicleState(-2.0, 0.5); + + CHECK(result.linear_velocity_m_s == Approx(-2.0)); + CHECK(result.articulation_angle_rad == Approx(-0.6435011088)); + CHECK(result.front_axle_turning_radius_m == Approx(-4.0)); + CHECK(result.rear_axle_turning_radius_m == Approx(-4.1)); + + // All wheel speeds are negative (reversing) + CHECK(result.front_right_wheel_speed_rad_s < 0.0); + CHECK(result.front_left_wheel_speed_rad_s < 0.0); + CHECK(result.rear_right_wheel_speed_rad_s < 0.0); + CHECK(result.rear_left_wheel_speed_rad_s < 0.0); + + // Right wheels are closer to ICR (smaller magnitude) than left wheels + CHECK(std::abs(result.front_right_wheel_speed_rad_s) < std::abs(result.front_left_wheel_speed_rad_s)); + CHECK(std::abs(result.rear_right_wheel_speed_rad_s) < std::abs(result.rear_left_wheel_speed_rad_s)); +} + +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - reverse right turn") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + // Reversing (v < 0) + CW yaw (omega < 0): articulation angle must be positive (joint bends left) + auto result = model.bodyVelocityToVehicleState(-2.0, -0.5); + + CHECK(result.linear_velocity_m_s == Approx(-2.0)); + CHECK(result.articulation_angle_rad == Approx(0.6435011088)); + + // All wheel speeds are negative (reversing) + CHECK(result.front_right_wheel_speed_rad_s < 0.0); + CHECK(result.front_left_wheel_speed_rad_s < 0.0); + CHECK(result.rear_right_wheel_speed_rad_s < 0.0); + CHECK(result.rear_left_wheel_speed_rad_s < 0.0); +} + +TEST_CASE("ArticulatedModel roundtrip reverse - bodyVelocityToVehicleState to articulationToAxleVelocities") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + double linear_velocity = -2.0; + double angular_velocity = 0.5; + + auto vehicle_state = model.bodyVelocityToVehicleState(linear_velocity, angular_velocity); + auto axle_vel = model.articulationToAxleVelocities(linear_velocity, vehicle_state.articulation_angle_rad); + + CHECK(axle_vel.front_axle_turning_velocity_rad_s == Approx(angular_velocity).margin(1e-6)); +} + } // namespace polymath::kinematics diff --git a/test/test_bicycle_model.cpp b/test/test_bicycle_model.cpp index 2ca2278..e1db681 100644 --- a/test/test_bicycle_model.cpp +++ b/test/test_bicycle_model.cpp @@ -277,4 +277,56 @@ TEST_CASE("BicycleModel roundtrip - negative steering angle") CHECK(steering.steering_angle_rad == Approx(steering_angle)); } +TEST_CASE("BicycleModel bodyVelocityToSteering - reverse left turn") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Reversing (v < 0) + CCW body rotation (omega > 0): front wheels must steer RIGHT (delta < 0) + // delta = atan(2.0 * 2.5 / -5.0) = atan(-1) = -pi/4 + auto result = model.bodyVelocityToSteering(-5.0, 2.0); + + CHECK(result.velocity_m_s == Approx(-5.0)); + CHECK(result.steering_angle_rad == Approx(-M_PI / 4)); + CHECK(result.turning_radius_m == Approx(-2.5)); + + // All wheels move backward + CHECK(result.rear_left_wheel_rad_s < 0.0); + CHECK(result.rear_right_wheel_rad_s < 0.0); + CHECK(result.front_left_wheel_rad_s < 0.0); + CHECK(result.front_right_wheel_rad_s < 0.0); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - reverse right turn") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Reversing (v < 0) + CW body rotation (omega < 0): front wheels must steer LEFT (delta > 0) + // delta = atan(-2.0 * 2.5 / -5.0) = atan(1) = pi/4 + auto result = model.bodyVelocityToSteering(-5.0, -2.0); + + CHECK(result.velocity_m_s == Approx(-5.0)); + CHECK(result.steering_angle_rad == Approx(M_PI / 4)); + CHECK(result.turning_radius_m == Approx(2.5)); + + // All wheels move backward + CHECK(result.rear_left_wheel_rad_s < 0.0); + CHECK(result.rear_right_wheel_rad_s < 0.0); + CHECK(result.front_left_wheel_rad_s < 0.0); + CHECK(result.front_right_wheel_rad_s < 0.0); +} + +TEST_CASE("BicycleModel roundtrip - reverse velocity") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double velocity = -3.0; + double steering_angle = -0.3; // Right steer while reversing + + auto body_vel = model.steeringToBodyVelocity(velocity, steering_angle); + auto steering = model.bodyVelocityToSteering(body_vel.linear_velocity_m_s, body_vel.angular_velocity_rad_s); + + CHECK(steering.velocity_m_s == Approx(velocity)); + CHECK(steering.steering_angle_rad == Approx(steering_angle)); +} + } // namespace polymath::kinematics diff --git a/test/test_python_bindings.py b/test/test_python_bindings.py index 0123958..1cf1880 100644 --- a/test/test_python_bindings.py +++ b/test/test_python_bindings.py @@ -82,6 +82,24 @@ def test_roundtrip(self): assert steering.velocity_m_s == pytest.approx(velocity) assert steering.steering_angle_rad == pytest.approx(steering_angle) + def test_reverse_turning(self): + model = BicycleModel(2.5, 1.5, 0.3) + # Reversing with CCW body rotation → front wheels steer RIGHT (negative delta) + result = model.body_velocity_to_steering(-5.0, 2.0) + assert result.velocity_m_s == pytest.approx(-5.0) + assert result.steering_angle_rad == pytest.approx(-math.pi / 4) + + def test_reverse_roundtrip(self): + model = BicycleModel(2.5, 1.5, 0.3) + velocity = -3.0 + steering_angle = -0.3 + + body_vel = model.steering_to_body_velocity(velocity, steering_angle) + steering = model.body_velocity_to_steering(body_vel.linear_velocity_m_s, body_vel.angular_velocity_rad_s) + + assert steering.velocity_m_s == pytest.approx(velocity) + assert steering.steering_angle_rad == pytest.approx(steering_angle) + class TestArticulatedModel: def test_construction(self): @@ -107,3 +125,19 @@ def test_axle_velocities(self): assert result.linear_velocity_m_s == pytest.approx(2.0) assert result.front_axle_turning_velocity_rad_s == pytest.approx(0.2244737375) + + def test_reverse_turning(self): + model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5) + # Reversing (v < 0) + CCW yaw → articulation angle negative (mirrors forward case) + result = model.body_velocity_to_vehicle_state(-2.0, 0.5) + assert result.articulation_angle_rad == pytest.approx(-0.6435011088) + + def test_reverse_roundtrip(self): + model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5) + linear_velocity = -2.0 + angular_velocity = 0.5 + + vehicle_state = model.body_velocity_to_vehicle_state(linear_velocity, angular_velocity) + axle_vel = model.articulation_to_axle_velocities(linear_velocity, vehicle_state.articulation_angle_rad) + + assert axle_vel.front_axle_turning_velocity_rad_s == pytest.approx(angular_velocity, abs=1e-6) From 1a3912fa9f445ce5941ba669d3512cd5dfde6fd5 Mon Sep 17 00:00:00 2001 From: Zeerek Ahmad Date: Wed, 18 Mar 2026 06:31:28 +0000 Subject: [PATCH 2/6] Clean up comments and documentation --- derivations/articulated_model.md | 2 +- src/articulated_model.cpp | 6 +----- src/bicycle_model.cpp | 21 ++++++--------------- 3 files changed, 8 insertions(+), 21 deletions(-) diff --git a/derivations/articulated_model.md b/derivations/articulated_model.md index 206efb7..ab58d49 100644 --- a/derivations/articulated_model.md +++ b/derivations/articulated_model.md @@ -136,7 +136,7 @@ With $\dot{\gamma} = 0$ (current implementation): $$\gamma = \operatorname{sign}(v)\cdot\arccos\!\left(\frac{-L_r\,\omega}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f) \tag{14}$$ -> **Branch selection:** The cosine inversion $\cos(\gamma+\phi) = c$ has two solutions $\gamma = \pm\arccos(c) - \phi$. For $v > 0$, $\phi = \operatorname{atan2}(v, \omega L_f) > 0$ and the $+$ branch yields the small physical angle. For $v < 0$, $\phi$ is negative and the $+$ branch pushes $\gamma$ near $\pm\pi$; the $-$ branch recovers the correct small angle. Hence $\operatorname{sign}(v)$ selects the appropriate branch in both directions. +> **Branch selection:** The cosine inversion $\cos(\gamma+\phi) = c$ has two solutions $\gamma = \pm\arccos(c) - \phi$. $\operatorname{sign}(v)$ selects the appropriate branch in both directions. --- ## Turning radii diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index 1e4e884..e7bc050 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -52,11 +52,7 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( std::numeric_limits::infinity()}; } - // The harmonic-addition inversion of eq. (12) yields two solutions: - // gamma + phi = +/-arccos(Lr*(gamma_dot - omega) / R) - // For v > 0, phi = atan2(v, omega*Lf) > 0 and the + branch gives the - // small physical angle. For v < 0, phi is negative and the + branch - // pushes gamma toward +/-pi. copysign selects + for forward, - for reverse. + // copysign selects + for forward, - for reverse. double articulation_angle = std::copysign( std::acos( diff --git a/src/bicycle_model.cpp b/src/bicycle_model.cpp index 53d73b1..13b6a03 100644 --- a/src/bicycle_model.cpp +++ b/src/bicycle_model.cpp @@ -50,35 +50,26 @@ BicycleSteeringState BicycleModel::bodyVelocityToSteering(double linear_velocity // steering_angle = atan(omega * L / v) inverts the forward kinematic // relation omega = v * tan(steering_angle) / L. // atan (not atan2) is intentional: the result must stay in (-pi/2, pi/2), - // which is the valid physical range for a steering angle. atan2 would add - // +/-pi when v < 0, placing the result in the wrong quadrant for reverse. double steering_angle = std::atan(angular_velocity * wheelbase_m_ / linear_velocity); double turning_radius = turningRadius(steering_angle); double half_track = track_width_m_ / 2.0; // Signed lateral distance from ICR to each rear wheel along the rear axle. - // ICR lies on the rear axle line, so the distance is purely lateral. - // Positive = wheel is on the outer side of the turn. double rear_left_r = turning_radius - half_track; double rear_right_r = turning_radius + half_track; - // Signed distance from ICR to each front wheel. The front wheels are offset - // by the wheelbase longitudinally, so the distance is the hypotenuse of - // (lateral_offset, wheelbase). copysign preserves the sign of the lateral - // offset so that each wheel's speed has the correct direction — this matters - // when the ICR falls between the rear wheels (|R| < half_track), where the - // inner and outer wheels rotate in opposite directions. + // Signed distance from ICR to each front wheel double front_left_r = std::copysign(std::hypot(rear_left_r, wheelbase_m_), rear_left_r); double front_right_r = std::copysign(std::hypot(rear_right_r, wheelbase_m_), rear_right_r); // Each wheel's ground speed = omega * distance_to_ICR (rigid body kinematics). // Convert to wheel angular velocity by dividing by wheel radius. - double rl = angular_velocity * rear_left_r / wheel_radius_m_; - double rr = angular_velocity * rear_right_r / wheel_radius_m_; - double fl = angular_velocity * front_left_r / wheel_radius_m_; - double fr = angular_velocity * front_right_r / wheel_radius_m_; + double rear_left_rad_s = angular_velocity * rear_left_r / wheel_radius_m_; + double rear_right_rad_s = angular_velocity * rear_right_r / wheel_radius_m_; + double front_left_rad_s = angular_velocity * front_left_r / wheel_radius_m_; + double front_right_rad_s = angular_velocity * front_right_r / wheel_radius_m_; - return BicycleSteeringState{linear_velocity, steering_angle, turning_radius, fr, fl, rr, rl}; + return BicycleSteeringState{linear_velocity, steering_angle, turning_radius, front_right_rad_s, front_left_rad_s, rear_right_rad_s, rear_left_rad_s}; } double BicycleModel::turningRadius(double steering_angle) From 854aab1abf450238c40f38a59997ece1d9bba69c Mon Sep 17 00:00:00 2001 From: Zeerek Ahmad Date: Wed, 18 Mar 2026 06:31:42 +0000 Subject: [PATCH 3/6] Pre-commit --- src/bicycle_model.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/bicycle_model.cpp b/src/bicycle_model.cpp index 13b6a03..e005f06 100644 --- a/src/bicycle_model.cpp +++ b/src/bicycle_model.cpp @@ -69,7 +69,14 @@ BicycleSteeringState BicycleModel::bodyVelocityToSteering(double linear_velocity double front_left_rad_s = angular_velocity * front_left_r / wheel_radius_m_; double front_right_rad_s = angular_velocity * front_right_r / wheel_radius_m_; - return BicycleSteeringState{linear_velocity, steering_angle, turning_radius, front_right_rad_s, front_left_rad_s, rear_right_rad_s, rear_left_rad_s}; + return BicycleSteeringState{ + linear_velocity, + steering_angle, + turning_radius, + front_right_rad_s, + front_left_rad_s, + rear_right_rad_s, + rear_left_rad_s}; } double BicycleModel::turningRadius(double steering_angle) From c7478e807e27e198249c625f4ee26a78fe119c85 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Wed, 18 Mar 2026 20:01:01 +0000 Subject: [PATCH 4/6] Make it much easier to read the math from the derivations --- src/articulated_model.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index e7bc050..4ad0a5b 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -53,6 +53,10 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( } // copysign selects + for forward, - for reverse. + double acos_calculation = std::acos( + articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / + std::hypot(angular_velocity_rad_s * articulation_to_front_axle_m_, linear_velocity_m_s)); + double articulation_angle = std::copysign( std::acos( From e4dbf46e0384a9aae2f1404e16640c0d22e174c7 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Wed, 18 Mar 2026 20:21:26 +0000 Subject: [PATCH 5/6] Pushed incomplete changes --- src/articulated_model.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index 4ad0a5b..84c4c72 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -57,14 +57,8 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / std::hypot(angular_velocity_rad_s * articulation_to_front_axle_m_, linear_velocity_m_s)); - double articulation_angle = - std::copysign( - std::acos( - articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / - std::sqrt( - square(angular_velocity_rad_s) * square(articulation_to_front_axle_m_) + square(linear_velocity_m_s))), - linear_velocity_m_s) - - std::atan2(linear_velocity_m_s, angular_velocity_rad_s * articulation_to_front_axle_m_); + double atan2_calculation = std::atan2(linear_velocity_m_s, angular_velocity_rad_s * articulation_to_front_axle_m_); + double articulation_angle = std::copysign(acos_calculation, linear_velocity_m_s) - atan2_calculation; double cos_articulation = std::cos(articulation_angle); double sin_articulation = std::sin(articulation_angle); From 4095da3db79832a53604530a495388530c50bded Mon Sep 17 00:00:00 2001 From: Zeerek Date: Wed, 18 Mar 2026 20:21:55 +0000 Subject: [PATCH 6/6] Delete unused square --- src/articulated_model.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index 84c4c72..26b678b 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -20,11 +20,6 @@ namespace polymath::kinematics { -constexpr inline double square(double x) -{ - return x * x; -} - ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( double linear_velocity_m_s, double angular_velocity_rad_s) {