From 958c561822b84a279aaf94d0e590afd1b92ebed9 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 13 Aug 2026 13:38:38 -0700 Subject: [PATCH] add forward projectors for all three kinematic models Introduces BicycleProjector, ArticulatedProjector and DifferentialDriveProjector: forward simulation that ramps the actuator command toward a target under a rate or acceleration limit, clamps it to the configured range, and integrates pose with Euler. Each exposes step() for one time step and project() for a full horizon, returning a timestamped state sequence whose element 0 is the initial state. Shared 2D geometry lands in pose2d.hpp: Pose2D, Point2D, Footprint (a CCW, unclosed polygon of any vertex count), normalizeAngle, transformFootprint, offsetAlongHeading and rectangleFootprint. Projectors optionally carry a body-frame polygon and emit it transformed into the world frame per sample, so the kinematic models stay dimension-free. AxleReference selects which axle the bicycle and articulated projectors measure poses and footprints from. Motion is still integrated where each model is naturally defined (the rear axle) and converted on input and output, so no integration accuracy is traded for the choice. ArticulatedModel gains an explicit articulation turning velocity (gamma-dot) on bodyVelocityToVehicleState and articulationToAxleVelocities, replacing the hardcoded zero constant. The two-argument forms remain as overloads assuming steady articulation, so existing callers are unaffected. Those overloads make the existing bindings ambiguous: kinematics_pybind.cpp passed bare member-function pointers, which no longer resolve once a second overload exists. Both bindings move to py::overload_cast on the three-argument form and expose gamma-dot as a keyword argument defaulting to 0.0, so Python callers keep the two-argument call and gain the rate. catch_discover_tests switches to DISCOVERY_MODE PRE_TEST. The default POST_BUILD runs each test binary during the build, where an older installed libpolymath_kinematics.so wins on LD_LIBRARY_PATH. Co-Authored-By: Claude Opus 5 (1M context) --- CMakeLists.txt | 14 +- .../polymath_kinematics/articulated_model.hpp | 24 +- .../articulated_projector.hpp | 170 +++++++++ .../polymath_kinematics/bicycle_projector.hpp | 148 ++++++++ .../differential_drive_projector.hpp | 154 ++++++++ include/polymath_kinematics/pose2d.hpp | 97 ++++++ src/articulated_model.cpp | 18 +- src/articulated_projector.cpp | 145 ++++++++ src/bicycle_projector.cpp | 129 +++++++ src/differential_drive_projector.cpp | 129 +++++++ src/kinematics_pybind.cpp | 10 +- test/test_articulated_model.cpp | 37 ++ test/test_articulated_projector.cpp | 328 ++++++++++++++++++ test/test_bicycle_projector.cpp | 256 ++++++++++++++ test/test_differential_drive_projector.cpp | 185 ++++++++++ 15 files changed, 1829 insertions(+), 15 deletions(-) create mode 100644 include/polymath_kinematics/articulated_projector.hpp create mode 100644 include/polymath_kinematics/bicycle_projector.hpp create mode 100644 include/polymath_kinematics/differential_drive_projector.hpp create mode 100644 include/polymath_kinematics/pose2d.hpp create mode 100644 src/articulated_projector.cpp create mode 100644 src/bicycle_projector.cpp create mode 100644 src/differential_drive_projector.cpp create mode 100644 test/test_articulated_projector.cpp create mode 100644 test/test_bicycle_projector.cpp create mode 100644 test/test_differential_drive_projector.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6624f4c..708f9e5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,8 +53,11 @@ find_package(pybind11 REQUIRED) # C++ library with all kinematic models add_library(polymath_kinematics SHARED src/articulated_model.cpp - src/differential_drive_model.cpp + src/articulated_projector.cpp src/bicycle_model.cpp + src/bicycle_projector.cpp + src/differential_drive_model.cpp + src/differential_drive_projector.cpp ) target_include_directories(polymath_kinematics PUBLIC $ @@ -109,15 +112,20 @@ if(BUILD_TESTING) target_link_libraries(${TEST_NAME} PRIVATE polymath_kinematics Catch2::Catch2WithMain) target_include_directories(${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test) if(COMMAND catch_discover_tests) - catch_discover_tests(${TEST_NAME}) + # PRE_TEST enumerates at ctest time. The default POST_BUILD runs each test binary during the + # build, where an older installed libpolymath_kinematics.so wins on LD_LIBRARY_PATH. + catch_discover_tests(${TEST_NAME} DISCOVERY_MODE PRE_TEST) else() add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME}) endif() endfunction() - add_kinematics_catch2_test(test_differential_drive test/test_differential_drive.cpp) add_kinematics_catch2_test(test_bicycle_model test/test_bicycle_model.cpp) add_kinematics_catch2_test(test_articulated_model test/test_articulated_model.cpp) + add_kinematics_catch2_test(test_differential_drive test/test_differential_drive.cpp) + add_kinematics_catch2_test(test_bicycle_projector test/test_bicycle_projector.cpp) + add_kinematics_catch2_test(test_articulated_projector test/test_articulated_projector.cpp) + add_kinematics_catch2_test(test_differential_drive_projector test/test_differential_drive_projector.cpp) ament_add_pytest_test(test_python_bindings test) endif() diff --git a/include/polymath_kinematics/articulated_model.hpp b/include/polymath_kinematics/articulated_model.hpp index 8969f47..490631a 100644 --- a/include/polymath_kinematics/articulated_model.hpp +++ b/include/polymath_kinematics/articulated_model.hpp @@ -40,7 +40,6 @@ struct ArticulatedAxleVelocities }; /// @brief Kinematic model for articulated vehicles (e.g., wheel loaders, articulated dump trucks) -/// @note TODO: (Zeerek) We want to add articulated angle turning velocity into our estimations class ArticulatedModel { public: @@ -68,17 +67,36 @@ class ArticulatedModel ~ArticulatedModel() = default; /// @brief Convert body velocity to vehicle state (articulation angle and wheel speeds) + /// Convenience overload that assumes a zero articulation turning velocity (steady articulation). /// @param linear_velocity_m_s Desired linear velocity in m/s /// @param angular_velocity_rad_s Desired angular velocity in rad/s /// @return Vehicle state including required articulation angle and wheel speeds ArticulatedVehicleState bodyVelocityToVehicleState(double linear_velocity_m_s, double angular_velocity_rad_s); + /// @brief Convert body velocity to vehicle state, including the effect of the articulation joint + /// turning velocity (gamma-dot). + /// @param linear_velocity_m_s Desired linear velocity in m/s + /// @param angular_velocity_rad_s Desired angular velocity in rad/s + /// @param articulation_turning_velocity_rad_s Rate of change of the articulation angle in rad/s + /// @return Vehicle state including required articulation angle and wheel speeds + ArticulatedVehicleState bodyVelocityToVehicleState( + double linear_velocity_m_s, double angular_velocity_rad_s, double articulation_turning_velocity_rad_s); + /// @brief Convert articulation state to axle turning velocities + /// Convenience overload that assumes a zero articulation turning velocity (steady articulation). /// @param linear_velocity_m_s Current linear velocity in m/s /// @param articulation_angle_rad Current articulation angle in radians /// @return Axle turning velocities for front and rear axles ArticulatedAxleVelocities articulationToAxleVelocities(double linear_velocity_m_s, double articulation_angle_rad); + /// @brief Convert articulation state to axle turning velocities, including gamma-dot + /// @param linear_velocity_m_s Current linear velocity in m/s + /// @param articulation_angle_rad Current articulation angle in radians + /// @param articulation_turning_velocity_rad_s Rate of change of the articulation angle in rad/s + /// @return Axle turning velocities for front and rear axles + ArticulatedAxleVelocities articulationToAxleVelocities( + double linear_velocity_m_s, double articulation_angle_rad, double articulation_turning_velocity_rad_s); + double get_articulation_to_front_axle_m() const { return articulation_to_front_axle_m_; @@ -117,10 +135,6 @@ class ArticulatedModel double front_wheel_radius_m_; double rear_wheel_radius_m_; - /// @brief Articulation turning velocity once calculated or available - /// TODO: (Zeerek) Add ability to pass this in when generating estimations - static constexpr double articulation_turning_velocity_rad_s_ = 0.0; - /// @brief Threshold below which a velocity is treated as zero to avoid numerical /// singularities (e.g. 0/0 in acos, or inf*0 in wheel speed calculations) static constexpr double ZERO_VELOCITY_THRESHOLD = 1e-9; diff --git a/include/polymath_kinematics/articulated_projector.hpp b/include/polymath_kinematics/articulated_projector.hpp new file mode 100644 index 0000000..b2d5ca6 --- /dev/null +++ b/include/polymath_kinematics/articulated_projector.hpp @@ -0,0 +1,170 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POLYMATH_KINEMATICS__ARTICULATED_PROJECTOR_HPP__ +#define POLYMATH_KINEMATICS__ARTICULATED_PROJECTOR_HPP__ + +#include +#include + +#include "polymath_kinematics/articulated_model.hpp" +#include "polymath_kinematics/pose2d.hpp" + +namespace polymath::kinematics +{ + +/// @brief One sample of an articulated-model projection. `pose` sits at the projector's reference +/// axle, with theta the heading of the body that axle belongs to. Motion is integrated at the rear +/// axle internally. +struct ArticulatedProjectedState +{ + double time_s; ///< Elapsed time from the start of the projection + Pose2D pose; ///< Reference-axle pose; theta = heading of that axle's body + double articulation_angle_rad; ///< Post-ramp articulation angle (gamma) in radians + double linear_velocity_m_s; ///< Commanded linear velocity in m/s + double angular_velocity_rad_s; ///< Rear-axle turning rate used for theta integration + ArticulatedVehicleState vehicle_state; ///< Full kinematic snapshot (wheel speeds + turning radii) + Pose2D joint_pose; ///< Articulation-joint (base_link) pose; theta = rear-body heading + Footprint front_footprint; ///< World-frame front-body polygon; empty if none was set + Footprint rear_footprint; ///< World-frame rear-body polygon; empty if none was set +}; + +/// @brief Forward-projection wrapper around ArticulatedModel. Ramps articulation angle (gamma) +/// toward a target at a bounded rate (gamma-dot), clamping the target to [min, max] first. +/// +/// Poses are measured at the axle named by `axle_reference`, with theta the heading of the body +/// that axle belongs to (rear-body heading for REAR, front-body heading = theta_rear + gamma for +/// FRONT). Motion is always integrated at the rear axle; the reference conversion is applied on +/// input and output. The articulation-joint pose is reported alongside on every sample. +class ArticulatedProjector +{ +public: + /// @brief Construct a projector with articulation-angle limits and (optionally) body footprints. + /// + /// Each body carries its own arbitrary polygon, expressed in the frame of **its own** axle: the + /// front polygon about the front axle with +x along the front-body heading, the rear polygon + /// about the rear axle with +x along the rear-body heading. This is the only anchoring that stays + /// rigid as the joint articulates. Footprints are owned by the projector, so the kinematic model + /// stays dimension-free. An empty polygon means "unset": that body's footprint is emitted empty + /// and projection proceeds normally (never throws). Use `rectangleFootprint()` for boxy bodies. + /// @param model Articulated kinematics model (stored by value) + /// @param min_articulation_angle_rad Minimum allowed articulation angle (typically negative) + /// @param max_articulation_angle_rad Maximum allowed articulation angle (typically positive) + /// @param axle_reference Axle that reported poses are measured from + /// @param front_footprint Front-body polygon in the front-axle frame + /// @param rear_footprint Rear-body polygon in the rear-axle frame + ArticulatedProjector( + ArticulatedModel model, + double min_articulation_angle_rad, + double max_articulation_angle_rad, + AxleReference axle_reference = AxleReference::REAR, + Footprint front_footprint = {}, + Footprint rear_footprint = {}) + : model_(model) + , min_articulation_angle_rad_(min_articulation_angle_rad) + , max_articulation_angle_rad_(max_articulation_angle_rad) + , axle_reference_(axle_reference) + , front_footprint_(std::move(front_footprint)) + , rear_footprint_(std::move(rear_footprint)) + {} + + ~ArticulatedProjector() = default; + + /// @brief Advance the vehicle one time step. + /// The articulation angle ramps from the current value toward clamp(target, min, max) at + /// |articulation_rate_rad_s| per second, never overshooting. Pose is integrated with Euler + /// using the post-ramp angle. + /// @param dt_s Step duration in seconds (must be > 0) + /// @param current_pose Reference-axle pose at the start of the step + /// @param current_articulation_angle_rad Articulation angle at the start of the step + /// @param target_articulation_angle_rad Desired articulation angle (clamped to [min, max] internally) + /// @param articulation_rate_rad_s Magnitude of the ramp rate in rad/s (sign is ignored) + /// @param linear_velocity_m_s Commanded linear velocity in m/s + /// @return Projected state at the end of the step (time_s = dt_s) + ArticulatedProjectedState step( + double dt_s, + const Pose2D & current_pose, + double current_articulation_angle_rad, + double target_articulation_angle_rad, + double articulation_rate_rad_s, + double linear_velocity_m_s); + + /// @brief Project a trajectory forward over `horizon_s` at `dt_s` steps. + /// Element 0 is the initial state (time_s=0); element N is the final state. + /// Trajectory length is ceil(horizon_s / dt_s) + 1. + /// @return Sequence of timestamped states (initial state included as element 0) + std::vector project( + double horizon_s, + double dt_s, + const Pose2D & initial_pose, + double initial_articulation_angle_rad, + double target_articulation_angle_rad, + double articulation_rate_rad_s, + double linear_velocity_m_s); + + const ArticulatedModel & get_model() const + { + return model_; + } + + double get_min_articulation_angle_rad() const + { + return min_articulation_angle_rad_; + } + + double get_max_articulation_angle_rad() const + { + return max_articulation_angle_rad_; + } + + AxleReference get_axle_reference() const + { + return axle_reference_; + } + + const Footprint & get_front_footprint() const + { + return front_footprint_; + } + + const Footprint & get_rear_footprint() const + { + return rear_footprint_; + } + +private: + /// @brief Convert a pose at the reference axle to the rear axle, where motion is integrated. + Pose2D toRearAxle(const Pose2D & reference_pose, double articulation_angle_rad) const; + + /// @brief Rear-axle pose -> articulation-joint pose (theta unchanged, still the rear heading). + Pose2D jointFromRearAxle(const Pose2D & rear_axle_pose) const; + + /// @brief Rear-axle pose -> front-axle pose, whose theta is the front-body heading. + Pose2D frontAxleFromRearAxle(const Pose2D & rear_axle_pose, double articulation_angle_rad) const; + + /// @brief Populate pose / joint_pose / both footprints from a rear-axle pose and gamma. + void fillPosesAndFootprints( + ArticulatedProjectedState & state, const Pose2D & rear_axle_pose, double articulation_angle_rad) const; + + ArticulatedModel model_; + double min_articulation_angle_rad_; + double max_articulation_angle_rad_; + AxleReference axle_reference_; + Footprint front_footprint_; + Footprint rear_footprint_; +}; + +} // namespace polymath::kinematics + +#endif // POLYMATH_KINEMATICS__ARTICULATED_PROJECTOR_HPP__ diff --git a/include/polymath_kinematics/bicycle_projector.hpp b/include/polymath_kinematics/bicycle_projector.hpp new file mode 100644 index 0000000..e041cd8 --- /dev/null +++ b/include/polymath_kinematics/bicycle_projector.hpp @@ -0,0 +1,148 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POLYMATH_KINEMATICS__BICYCLE_PROJECTOR_HPP__ +#define POLYMATH_KINEMATICS__BICYCLE_PROJECTOR_HPP__ + +#include +#include + +#include "polymath_kinematics/bicycle_model.hpp" +#include "polymath_kinematics/pose2d.hpp" + +namespace polymath::kinematics +{ + +/// @brief One sample of a bicycle-model projection: pose + steering + wheel speeds at a point in time. +struct BicycleProjectedState +{ + double time_s; ///< Elapsed time from the start of the projection + Pose2D pose; ///< Body pose at the projector's reference axle; theta is the chassis heading + double steering_angle_rad; ///< Post-ramp steering angle in radians + double linear_velocity_m_s; ///< Commanded linear velocity in m/s + double angular_velocity_rad_s; ///< Body angular velocity used for theta integration + BicycleSteeringState steering_state; ///< Full kinematic snapshot (wheel speeds + turning radius) + Footprint footprint; ///< World-frame body polygon; empty if no footprint was set +}; + +/// @brief Forward-projection wrapper around BicycleModel that ramps steering toward a target at +/// a bounded rate, clamping the target to [min, max] before ramping, and integrates pose with Euler. +/// +/// Poses and the body-frame footprint are both measured from the axle named by `axle_reference`. +/// Motion is always integrated at the rear axle (where the bicycle model is defined); a FRONT +/// reference is a rigid wheelbase offset along the chassis heading, applied on input and output. +class BicycleProjector +{ +public: + /// @brief Construct a projector with steering-angle limits and (optionally) a body footprint. + /// + /// The footprint is an arbitrary polygon in the body frame of `axle_reference`: +x along the + /// chassis heading, +y to the left, origin at that axle. It is owned by the projector, so the + /// kinematic model stays dimension-free. An empty footprint means "unset": projected states + /// carry an empty footprint and projection proceeds normally (never throws). Use + /// `rectangleFootprint()` for a boxy vehicle. + /// @param model Bicycle kinematics model (stored by value) + /// @param min_steering_angle_rad Minimum allowed steering angle (typically negative) + /// @param max_steering_angle_rad Maximum allowed steering angle (typically positive) + /// @param axle_reference Axle that poses and the footprint are measured from + /// @param footprint Body-frame body polygon; empty disables footprint output + BicycleProjector( + BicycleModel model, + double min_steering_angle_rad, + double max_steering_angle_rad, + AxleReference axle_reference = AxleReference::REAR, + Footprint footprint = {}) + : model_(model) + , min_steering_angle_rad_(min_steering_angle_rad) + , max_steering_angle_rad_(max_steering_angle_rad) + , axle_reference_(axle_reference) + , footprint_(std::move(footprint)) + {} + + ~BicycleProjector() = default; + + /// @brief Advance the vehicle one time step. + /// Steering ramps from current_steering_angle_rad toward clamp(target, min, max) at + /// |steering_rate_rad_s| per second, never overshooting. Pose is integrated with Euler + /// using the post-ramp angle. + /// @param dt_s Step duration in seconds (must be > 0) + /// @param current_pose Pose at the start of the step + /// @param current_steering_angle_rad Steering angle at the start of the step + /// @param target_steering_angle_rad Desired steering angle (clamped to [min, max] internally) + /// @param steering_rate_rad_s Magnitude of the ramp rate in rad/s (sign is ignored) + /// @param linear_velocity_m_s Commanded linear velocity in m/s + /// @return Projected state at the end of the step (time_s = dt_s) + BicycleProjectedState step( + double dt_s, + const Pose2D & current_pose, + double current_steering_angle_rad, + double target_steering_angle_rad, + double steering_rate_rad_s, + double linear_velocity_m_s); + + /// @brief Project a trajectory forward over `horizon_s` at `dt_s` steps. + /// Element 0 is the initial state (time_s=0); element N is the final state. + /// Trajectory length is ceil(horizon_s / dt_s) + 1. + /// @return Sequence of timestamped states (initial state included as element 0) + std::vector project( + double horizon_s, + double dt_s, + const Pose2D & initial_pose, + double initial_steering_angle_rad, + double target_steering_angle_rad, + double steering_rate_rad_s, + double linear_velocity_m_s); + + const BicycleModel & get_model() const + { + return model_; + } + + double get_min_steering_angle_rad() const + { + return min_steering_angle_rad_; + } + + double get_max_steering_angle_rad() const + { + return max_steering_angle_rad_; + } + + AxleReference get_axle_reference() const + { + return axle_reference_; + } + + const Footprint & get_footprint() const + { + return footprint_; + } + +private: + /// @brief Convert a pose at the reference axle to the rear axle, where motion is integrated. + Pose2D toRearAxle(const Pose2D & reference_pose) const; + + /// @brief Convert a rear-axle pose back out to the reference axle. + Pose2D fromRearAxle(const Pose2D & rear_axle_pose) const; + + BicycleModel model_; + double min_steering_angle_rad_; + double max_steering_angle_rad_; + AxleReference axle_reference_; + Footprint footprint_; +}; + +} // namespace polymath::kinematics + +#endif // POLYMATH_KINEMATICS__BICYCLE_PROJECTOR_HPP__ diff --git a/include/polymath_kinematics/differential_drive_projector.hpp b/include/polymath_kinematics/differential_drive_projector.hpp new file mode 100644 index 0000000..e1c221b --- /dev/null +++ b/include/polymath_kinematics/differential_drive_projector.hpp @@ -0,0 +1,154 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POLYMATH_KINEMATICS__DIFFERENTIAL_DRIVE_PROJECTOR_HPP__ +#define POLYMATH_KINEMATICS__DIFFERENTIAL_DRIVE_PROJECTOR_HPP__ + +#include +#include + +#include "polymath_kinematics/differential_drive_model.hpp" +#include "polymath_kinematics/pose2d.hpp" + +namespace polymath::kinematics +{ + +/// @brief One sample of a differential-drive-model projection. +struct DifferentialDriveProjectedState +{ + double time_s; ///< Elapsed time from the start of the projection + Pose2D pose; ///< Body pose at this sample + double linear_velocity_m_s; ///< Post-ramp body linear velocity in m/s + double angular_velocity_rad_s; ///< Post-ramp body angular velocity in rad/s + DifferentialDriveWheelVelocities wheel_velocities; ///< Wheel speeds derived from the body command + Footprint footprint; ///< World-frame body polygon; empty if no footprint was set +}; + +/// @brief Forward-projection wrapper around DifferentialDriveModel that ramps the body command +/// (linear_velocity, angular_velocity) toward targets at bounded accelerations, clamping each +/// to its [min, max] range before ramping. Pose is integrated with Euler. +/// +/// A differential drive has a single axle, so there is no axle reference to choose: poses and the +/// body-frame footprint are both measured from the body centre. +class DifferentialDriveProjector +{ +public: + /// @brief Construct a projector with body-velocity limits. + /// @param model Differential drive kinematics model (stored by value) + /// @param min_linear_velocity_m_s Minimum allowed linear velocity (typically negative for reverse) + /// @param max_linear_velocity_m_s Maximum allowed linear velocity + /// @param min_angular_velocity_rad_s Minimum allowed angular velocity (typically negative) + /// @param max_angular_velocity_rad_s Maximum allowed angular velocity + /// + /// The footprint is an arbitrary polygon in the body frame: +x along the heading, +y to the + /// left, origin at the body centre. It is owned by the projector, so the kinematic model stays + /// dimension-free. An empty footprint means "unset": projected states carry an empty footprint + /// and projection proceeds normally (never throws). Use `rectangleFootprint()` for a boxy body. + /// @param footprint Body-frame body polygon; empty disables footprint output + DifferentialDriveProjector( + DifferentialDriveModel model, + double min_linear_velocity_m_s, + double max_linear_velocity_m_s, + double min_angular_velocity_rad_s, + double max_angular_velocity_rad_s, + Footprint footprint = {}) + : model_(model) + , min_linear_velocity_m_s_(min_linear_velocity_m_s) + , max_linear_velocity_m_s_(max_linear_velocity_m_s) + , min_angular_velocity_rad_s_(min_angular_velocity_rad_s) + , max_angular_velocity_rad_s_(max_angular_velocity_rad_s) + , footprint_(std::move(footprint)) + {} + + ~DifferentialDriveProjector() = default; + + /// @brief Advance the vehicle one time step. + /// Linear and angular velocities ramp from their current values toward + /// clamp(target, min, max) at the corresponding |acceleration| per second, + /// never overshooting. Pose is integrated with Euler using the post-ramp body command. + /// @param dt_s Step duration in seconds (must be > 0) + /// @param current_pose Pose at the start of the step + /// @param current_linear_velocity_m_s Linear velocity at the start of the step + /// @param current_angular_velocity_rad_s Angular velocity at the start of the step + /// @param target_linear_velocity_m_s Desired linear velocity (clamped internally) + /// @param target_angular_velocity_rad_s Desired angular velocity (clamped internally) + /// @param linear_acceleration_m_s2 Magnitude of the linear ramp rate (sign is ignored) + /// @param angular_acceleration_rad_s2 Magnitude of the angular ramp rate (sign is ignored) + /// @return Projected state at the end of the step (time_s = dt_s) + DifferentialDriveProjectedState step( + double dt_s, + const Pose2D & current_pose, + double current_linear_velocity_m_s, + double current_angular_velocity_rad_s, + double target_linear_velocity_m_s, + double target_angular_velocity_rad_s, + double linear_acceleration_m_s2, + double angular_acceleration_rad_s2); + + /// @brief Project a trajectory forward over `horizon_s` at `dt_s` steps. + /// Element 0 is the initial state (time_s=0); element N is the final state. + /// Trajectory length is ceil(horizon_s / dt_s) + 1. + std::vector project( + double horizon_s, + double dt_s, + const Pose2D & initial_pose, + double initial_linear_velocity_m_s, + double initial_angular_velocity_rad_s, + double target_linear_velocity_m_s, + double target_angular_velocity_rad_s, + double linear_acceleration_m_s2, + double angular_acceleration_rad_s2); + + const DifferentialDriveModel & get_model() const + { + return model_; + } + + double get_min_linear_velocity_m_s() const + { + return min_linear_velocity_m_s_; + } + + double get_max_linear_velocity_m_s() const + { + return max_linear_velocity_m_s_; + } + + double get_min_angular_velocity_rad_s() const + { + return min_angular_velocity_rad_s_; + } + + double get_max_angular_velocity_rad_s() const + { + return max_angular_velocity_rad_s_; + } + + const Footprint & get_footprint() const + { + return footprint_; + } + +private: + DifferentialDriveModel model_; + double min_linear_velocity_m_s_; + double max_linear_velocity_m_s_; + double min_angular_velocity_rad_s_; + double max_angular_velocity_rad_s_; + Footprint footprint_; +}; + +} // namespace polymath::kinematics + +#endif // POLYMATH_KINEMATICS__DIFFERENTIAL_DRIVE_PROJECTOR_HPP__ diff --git a/include/polymath_kinematics/pose2d.hpp b/include/polymath_kinematics/pose2d.hpp new file mode 100644 index 0000000..36e8e9c --- /dev/null +++ b/include/polymath_kinematics/pose2d.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POLYMATH_KINEMATICS__POSE2D_HPP__ +#define POLYMATH_KINEMATICS__POSE2D_HPP__ + +#include +#include + +namespace polymath::kinematics +{ + +/// @brief Planar pose: position (x, y) and heading theta (radians, CCW from +x). +struct Pose2D +{ + double x; + double y; + double theta; +}; + +/// @brief A planar point (world frame, metres). +struct Point2D +{ + double x; + double y; +}; + +/// @brief A vehicle footprint as a polygon of arbitrary vertex count, counter-clockwise and NOT +/// closed (the first vertex is not repeated; consumers close it if needed). Used both for the +/// body-frame outline handed to a projector and for the world-frame result it emits. An empty +/// footprint means "not set / not computed". +using Footprint = std::vector; + +/// @brief Which axle a projector's poses and body-frame footprint are measured from. +enum class AxleReference +{ + FRONT, + REAR +}; + +/// @brief Build a rectangular body-frame footprint, the common case for a boxy vehicle. +/// @param front_m Distance from the reference axle forward to the front edge (may be negative) +/// @param rear_m Distance from the reference axle back to the rear edge (positive = behind) +/// @param width_m Total body width; <= 0 returns an empty footprint +/// @return Corners CCW and open: rear-right, front-right, front-left, rear-left +inline Footprint rectangleFootprint(double front_m, double rear_m, double width_m) +{ + if (width_m <= 0.0) { + return Footprint{}; + } + const double half_w = width_m / 2.0; + return Footprint{ + Point2D{-rear_m, -half_w}, Point2D{front_m, -half_w}, Point2D{front_m, half_w}, Point2D{-rear_m, half_w}}; +} + +/// @brief Transform a body-frame footprint into the world frame by `pose`. +inline Footprint transformFootprint(const Footprint & body_frame, const Pose2D & pose) +{ + Footprint world; + world.reserve(body_frame.size()); + const double cos_t = std::cos(pose.theta); + const double sin_t = std::sin(pose.theta); + for (const Point2D & vertex : body_frame) { + world.push_back( + Point2D{pose.x + cos_t * vertex.x - sin_t * vertex.y, pose.y + sin_t * vertex.x + cos_t * vertex.y}); + } + return world; +} + +/// @brief Offset a pose along its own heading; negative `distance_m` moves backward. +inline Pose2D offsetAlongHeading(const Pose2D & pose, double distance_m) +{ + return Pose2D{pose.x + distance_m * std::cos(pose.theta), pose.y + distance_m * std::sin(pose.theta), pose.theta}; +} + +/// @brief Wrap an angle to [-pi, pi]. +/// std::remainder(angle, 2*pi) maps to the [-pi, pi] interval directly (note: exactly +pi +/// maps to +pi, where the old fmod-based implementation returned -pi; both denote the same angle). +inline double normalizeAngle(double angle) +{ + return std::remainder(angle, 2.0 * M_PI); +} + +} // namespace polymath::kinematics + +#endif // POLYMATH_KINEMATICS__POSE2D_HPP__ diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index 0b04a59..c446261 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -23,6 +23,12 @@ namespace polymath::kinematics ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( double linear_velocity_m_s, double angular_velocity_rad_s) +{ + return bodyVelocityToVehicleState(linear_velocity_m_s, angular_velocity_rad_s, 0.0); +} + +ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( + double linear_velocity_m_s, double angular_velocity_rad_s, double articulation_turning_velocity_rad_s) { // Guard: fully stationary — sqrt denominator collapses to 0, producing 0/0 = NaN if ( @@ -51,7 +57,7 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( // Clamp keeps the arccos argument in [-1, 1] for over-tight commands; see the "Feasibility of // the arccos argument (clamping)" section in derivations/articulated_model.md. const double acos_argument = std::clamp( - articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / + 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), -1.0, 1.0); @@ -92,13 +98,19 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( ArticulatedAxleVelocities ArticulatedModel::articulationToAxleVelocities( double linear_velocity_m_s, double articulation_angle_rad) +{ + return articulationToAxleVelocities(linear_velocity_m_s, articulation_angle_rad, 0.0); +} + +ArticulatedAxleVelocities ArticulatedModel::articulationToAxleVelocities( + double linear_velocity_m_s, double articulation_angle_rad, double articulation_turning_velocity_rad_s) { double front_axle_turning_velocity = (linear_velocity_m_s * std::sin(articulation_angle_rad) + - articulation_to_rear_axle_m_ * articulation_turning_velocity_rad_s_) / + articulation_to_rear_axle_m_ * articulation_turning_velocity_rad_s) / (articulation_to_front_axle_m_ * std::cos(articulation_angle_rad) + articulation_to_rear_axle_m_); - double rear_axle_turning_velocity = front_axle_turning_velocity - articulation_turning_velocity_rad_s_; + double rear_axle_turning_velocity = front_axle_turning_velocity - articulation_turning_velocity_rad_s; return ArticulatedAxleVelocities{linear_velocity_m_s, front_axle_turning_velocity, rear_axle_turning_velocity}; } diff --git a/src/articulated_projector.cpp b/src/articulated_projector.cpp new file mode 100644 index 0000000..f8b2e3d --- /dev/null +++ b/src/articulated_projector.cpp @@ -0,0 +1,145 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics/articulated_projector.hpp" + +#include +#include +#include + +namespace polymath::kinematics +{ + +Pose2D ArticulatedProjector::jointFromRearAxle(const Pose2D & rear_axle_pose) const +{ + return offsetAlongHeading(rear_axle_pose, model_.get_articulation_to_rear_axle_m()); +} + +Pose2D ArticulatedProjector::frontAxleFromRearAxle(const Pose2D & rear_axle_pose, double articulation_angle_rad) const +{ + // Walk rear axle -> joint along the rear heading, then joint -> front axle along the front + // heading (theta_front = theta_rear + gamma). + const Pose2D joint = jointFromRearAxle(rear_axle_pose); + const Pose2D front_heading{joint.x, joint.y, normalizeAngle(joint.theta + articulation_angle_rad)}; + return offsetAlongHeading(front_heading, model_.get_articulation_to_front_axle_m()); +} + +Pose2D ArticulatedProjector::toRearAxle(const Pose2D & reference_pose, double articulation_angle_rad) const +{ + if (axle_reference_ == AxleReference::REAR) { + return reference_pose; + } + // Invert frontAxleFromRearAxle: theta here is the front-body heading. + const Pose2D joint = offsetAlongHeading(reference_pose, -model_.get_articulation_to_front_axle_m()); + const Pose2D rear_heading{joint.x, joint.y, normalizeAngle(joint.theta - articulation_angle_rad)}; + return offsetAlongHeading(rear_heading, -model_.get_articulation_to_rear_axle_m()); +} + +void ArticulatedProjector::fillPosesAndFootprints( + ArticulatedProjectedState & state, const Pose2D & rear_axle_pose, double articulation_angle_rad) const +{ + const Pose2D front_axle_pose = frontAxleFromRearAxle(rear_axle_pose, articulation_angle_rad); + state.joint_pose = jointFromRearAxle(rear_axle_pose); + state.pose = axle_reference_ == AxleReference::REAR ? rear_axle_pose : front_axle_pose; + // Each body's polygon is anchored at its own axle, so it stays rigid as the joint articulates. + state.front_footprint = transformFootprint(front_footprint_, front_axle_pose); + state.rear_footprint = transformFootprint(rear_footprint_, rear_axle_pose); +} + +ArticulatedProjectedState ArticulatedProjector::step( + double dt_s, + const Pose2D & current_pose, + double current_articulation_angle_rad, + double target_articulation_angle_rad, + double articulation_rate_rad_s, + double linear_velocity_m_s) +{ + // Clamp target into the joint's mechanical bounds before ramping. + double clamped_target = + std::clamp(target_articulation_angle_rad, min_articulation_angle_rad_, max_articulation_angle_rad_); + + // Slew toward the clamped target at |rate| per second, never overshooting. + double max_delta = std::abs(articulation_rate_rad_s) * dt_s; + double delta = clamped_target - current_articulation_angle_rad; + if (std::abs(delta) > max_delta) { + delta = std::copysign(max_delta, delta); + } + double new_articulation_angle_rad = current_articulation_angle_rad + delta; + // Actual gamma-dot realized during this step (zero once the angle pins at clamped_target). + double actual_articulation_rate_rad_s = delta / dt_s; + + // Rear-axle turning velocity drives theta integration; feed the realized gamma-dot in. + ArticulatedAxleVelocities axle = model_.articulationToAxleVelocities( + linear_velocity_m_s, new_articulation_angle_rad, actual_articulation_rate_rad_s); + double angular_velocity_rad_s = axle.rear_axle_turning_velocity_rad_s; + + // Full vehicle state (wheel speeds + turning radii) for the snapshot. + ArticulatedVehicleState inner = + model_.bodyVelocityToVehicleState(linear_velocity_m_s, angular_velocity_rad_s, actual_articulation_rate_rad_s); + + // Motion is integrated at the rear axle; convert in from the reference axle and back out again. + const Pose2D rear_pose = toRearAxle(current_pose, current_articulation_angle_rad); + const Pose2D new_rear_pose{ + rear_pose.x + linear_velocity_m_s * std::cos(rear_pose.theta) * dt_s, + rear_pose.y + linear_velocity_m_s * std::sin(rear_pose.theta) * dt_s, + normalizeAngle(rear_pose.theta + angular_velocity_rad_s * dt_s)}; + + ArticulatedProjectedState state{ + dt_s, {}, new_articulation_angle_rad, linear_velocity_m_s, angular_velocity_rad_s, inner, {}, {}, {}}; + fillPosesAndFootprints(state, new_rear_pose, new_articulation_angle_rad); + return state; +} + +std::vector ArticulatedProjector::project( + double horizon_s, + double dt_s, + const Pose2D & initial_pose, + double initial_articulation_angle_rad, + double target_articulation_angle_rad, + double articulation_rate_rad_s, + double linear_velocity_m_s) +{ + std::vector trajectory; + if (dt_s <= 0.0 || horizon_s < 0.0) { + return trajectory; + } + + // Seed element 0 with the initial state. + ArticulatedAxleVelocities initial_axle = + model_.articulationToAxleVelocities(linear_velocity_m_s, initial_articulation_angle_rad); + double initial_omega = initial_axle.rear_axle_turning_velocity_rad_s; + ArticulatedVehicleState initial_inner = model_.bodyVelocityToVehicleState(linear_velocity_m_s, initial_omega); + ArticulatedProjectedState initial_state{ + 0.0, {}, initial_articulation_angle_rad, linear_velocity_m_s, initial_omega, initial_inner, {}, {}, {}}; + fillPosesAndFootprints( + initial_state, toRearAxle(initial_pose, initial_articulation_angle_rad), initial_articulation_angle_rad); + trajectory.push_back(initial_state); + + std::size_t n_steps = static_cast(std::ceil(horizon_s / dt_s)); + trajectory.reserve(n_steps + 1); + + Pose2D pose = initial_pose; + double articulation_angle = initial_articulation_angle_rad; + for (std::size_t i = 0; i < n_steps; ++i) { + ArticulatedProjectedState s = + step(dt_s, pose, articulation_angle, target_articulation_angle_rad, articulation_rate_rad_s, linear_velocity_m_s); + s.time_s = static_cast(i + 1) * dt_s; + pose = s.pose; + articulation_angle = s.articulation_angle_rad; + trajectory.push_back(s); + } + return trajectory; +} + +} // namespace polymath::kinematics diff --git a/src/bicycle_projector.cpp b/src/bicycle_projector.cpp new file mode 100644 index 0000000..bf44ff6 --- /dev/null +++ b/src/bicycle_projector.cpp @@ -0,0 +1,129 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics/bicycle_projector.hpp" + +#include +#include +#include + +namespace polymath::kinematics +{ + +Pose2D BicycleProjector::toRearAxle(const Pose2D & reference_pose) const +{ + if (axle_reference_ == AxleReference::REAR) { + return reference_pose; + } + return offsetAlongHeading(reference_pose, -model_.get_wheelbase_m()); +} + +Pose2D BicycleProjector::fromRearAxle(const Pose2D & rear_axle_pose) const +{ + if (axle_reference_ == AxleReference::REAR) { + return rear_axle_pose; + } + return offsetAlongHeading(rear_axle_pose, model_.get_wheelbase_m()); +} + +BicycleProjectedState BicycleProjector::step( + double dt_s, + const Pose2D & current_pose, + double current_steering_angle_rad, + double target_steering_angle_rad, + double steering_rate_rad_s, + double linear_velocity_m_s) +{ + // Clamp target into the actuator's bounds before ramping, so out-of-range commands + // saturate at the limit rather than oscillating or overshooting. + double clamped_target = std::clamp(target_steering_angle_rad, min_steering_angle_rad_, max_steering_angle_rad_); + + // Slew toward the clamped target at |rate| per second, never overshooting. + double max_delta = std::abs(steering_rate_rad_s) * dt_s; + double delta = clamped_target - current_steering_angle_rad; + if (std::abs(delta) > max_delta) { + delta = std::copysign(max_delta, delta); + } + double new_steering_angle_rad = current_steering_angle_rad + delta; + + // Forward kinematics with the post-ramp angle gives the body omega used for theta integration. + BicycleBodyVelocity body_vel = model_.steeringToBodyVelocity(linear_velocity_m_s, new_steering_angle_rad); + + // Inverse kinematics populates wheel speeds + turning radius for the snapshot. + BicycleSteeringState inner = model_.bodyVelocityToSteering(linear_velocity_m_s, body_vel.angular_velocity_rad_s); + + // Euler pose update at the rear axle (heading taken at start of step), then back out to the + // reference axle. + const Pose2D rear_pose = toRearAxle(current_pose); + Pose2D new_rear_pose{ + rear_pose.x + linear_velocity_m_s * std::cos(rear_pose.theta) * dt_s, + rear_pose.y + linear_velocity_m_s * std::sin(rear_pose.theta) * dt_s, + normalizeAngle(rear_pose.theta + body_vel.angular_velocity_rad_s * dt_s)}; + const Pose2D new_pose = fromRearAxle(new_rear_pose); + + return BicycleProjectedState{ + dt_s, + new_pose, + new_steering_angle_rad, + linear_velocity_m_s, + body_vel.angular_velocity_rad_s, + inner, + transformFootprint(footprint_, new_pose)}; +} + +std::vector BicycleProjector::project( + double horizon_s, + double dt_s, + const Pose2D & initial_pose, + double initial_steering_angle_rad, + double target_steering_angle_rad, + double steering_rate_rad_s, + double linear_velocity_m_s) +{ + std::vector trajectory; + if (dt_s <= 0.0 || horizon_s < 0.0) { + return trajectory; + } + + // Seed element 0 with the initial state so plots have a clean t=0 anchor. + // Populate the steering_state field by running inverse kinematics on the implied omega. + BicycleBodyVelocity initial_body = model_.steeringToBodyVelocity(linear_velocity_m_s, initial_steering_angle_rad); + BicycleSteeringState initial_inner = + model_.bodyVelocityToSteering(linear_velocity_m_s, initial_body.angular_velocity_rad_s); + trajectory.push_back(BicycleProjectedState{ + 0.0, + initial_pose, + initial_steering_angle_rad, + linear_velocity_m_s, + initial_body.angular_velocity_rad_s, + initial_inner, + transformFootprint(footprint_, initial_pose)}); + + std::size_t n_steps = static_cast(std::ceil(horizon_s / dt_s)); + trajectory.reserve(n_steps + 1); + + Pose2D pose = initial_pose; + double steering_angle = initial_steering_angle_rad; + for (std::size_t i = 0; i < n_steps; ++i) { + BicycleProjectedState s = + step(dt_s, pose, steering_angle, target_steering_angle_rad, steering_rate_rad_s, linear_velocity_m_s); + s.time_s = static_cast(i + 1) * dt_s; + pose = s.pose; + steering_angle = s.steering_angle_rad; + trajectory.push_back(s); + } + return trajectory; +} + +} // namespace polymath::kinematics diff --git a/src/differential_drive_projector.cpp b/src/differential_drive_projector.cpp new file mode 100644 index 0000000..dd7b32b --- /dev/null +++ b/src/differential_drive_projector.cpp @@ -0,0 +1,129 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics/differential_drive_projector.hpp" + +#include +#include +#include + +namespace polymath::kinematics +{ + +namespace +{ + +/// @brief Ramp `current` toward `clamped_target` by at most |rate| * dt, never overshooting. +double rampedAdvance(double current, double clamped_target, double rate, double dt_s) +{ + double max_delta = std::abs(rate) * dt_s; + double delta = clamped_target - current; + if (std::abs(delta) > max_delta) { + delta = std::copysign(max_delta, delta); + } + return current + delta; +} + +} // namespace + +DifferentialDriveProjectedState DifferentialDriveProjector::step( + double dt_s, + const Pose2D & current_pose, + double current_linear_velocity_m_s, + double current_angular_velocity_rad_s, + double target_linear_velocity_m_s, + double target_angular_velocity_rad_s, + double linear_acceleration_m_s2, + double angular_acceleration_rad_s2) +{ + // Clamp targets to actuator bounds before ramping. + double clamped_target_v = std::clamp(target_linear_velocity_m_s, min_linear_velocity_m_s_, max_linear_velocity_m_s_); + double clamped_target_omega = + std::clamp(target_angular_velocity_rad_s, min_angular_velocity_rad_s_, max_angular_velocity_rad_s_); + + double new_linear_velocity_m_s = + rampedAdvance(current_linear_velocity_m_s, clamped_target_v, linear_acceleration_m_s2, dt_s); + double new_angular_velocity_rad_s = + rampedAdvance(current_angular_velocity_rad_s, clamped_target_omega, angular_acceleration_rad_s2, dt_s); + + DifferentialDriveWheelVelocities wheels = + model_.bodyVelocityToWheelVelocities(new_linear_velocity_m_s, new_angular_velocity_rad_s); + + // Euler pose update (heading taken at start of step, matches bicycle/articulated projectors). + Pose2D new_pose{ + current_pose.x + new_linear_velocity_m_s * std::cos(current_pose.theta) * dt_s, + current_pose.y + new_linear_velocity_m_s * std::sin(current_pose.theta) * dt_s, + normalizeAngle(current_pose.theta + new_angular_velocity_rad_s * dt_s)}; + + return DifferentialDriveProjectedState{ + dt_s, + new_pose, + new_linear_velocity_m_s, + new_angular_velocity_rad_s, + wheels, + transformFootprint(footprint_, new_pose)}; +} + +std::vector DifferentialDriveProjector::project( + double horizon_s, + double dt_s, + const Pose2D & initial_pose, + double initial_linear_velocity_m_s, + double initial_angular_velocity_rad_s, + double target_linear_velocity_m_s, + double target_angular_velocity_rad_s, + double linear_acceleration_m_s2, + double angular_acceleration_rad_s2) +{ + std::vector trajectory; + if (dt_s <= 0.0 || horizon_s < 0.0) { + return trajectory; + } + + DifferentialDriveWheelVelocities initial_wheels = + model_.bodyVelocityToWheelVelocities(initial_linear_velocity_m_s, initial_angular_velocity_rad_s); + trajectory.push_back(DifferentialDriveProjectedState{ + 0.0, + initial_pose, + initial_linear_velocity_m_s, + initial_angular_velocity_rad_s, + initial_wheels, + transformFootprint(footprint_, initial_pose)}); + + std::size_t n_steps = static_cast(std::ceil(horizon_s / dt_s)); + trajectory.reserve(n_steps + 1); + + Pose2D pose = initial_pose; + double linear_velocity_m_s = initial_linear_velocity_m_s; + double angular_velocity_rad_s = initial_angular_velocity_rad_s; + for (std::size_t i = 0; i < n_steps; ++i) { + DifferentialDriveProjectedState s = step( + dt_s, + pose, + linear_velocity_m_s, + angular_velocity_rad_s, + target_linear_velocity_m_s, + target_angular_velocity_rad_s, + linear_acceleration_m_s2, + angular_acceleration_rad_s2); + s.time_s = static_cast(i + 1) * dt_s; + pose = s.pose; + linear_velocity_m_s = s.linear_velocity_m_s; + angular_velocity_rad_s = s.angular_velocity_rad_s; + trajectory.push_back(s); + } + return trajectory; +} + +} // namespace polymath::kinematics diff --git a/src/kinematics_pybind.cpp b/src/kinematics_pybind.cpp index 0079e67..b2ecf9d 100644 --- a/src/kinematics_pybind.cpp +++ b/src/kinematics_pybind.cpp @@ -55,14 +55,16 @@ PYBIND11_MODULE(polymath_kinematics_cpp, m) py::arg("rear_wheel_radius_m")) .def( "body_velocity_to_vehicle_state", - &ArticulatedModel::bodyVelocityToVehicleState, + py::overload_cast(&ArticulatedModel::bodyVelocityToVehicleState), py::arg("linear_velocity_m_s"), - py::arg("angular_velocity_rad_s")) + py::arg("angular_velocity_rad_s"), + py::arg("articulation_turning_velocity_rad_s") = 0.0) .def( "articulation_to_axle_velocities", - &ArticulatedModel::articulationToAxleVelocities, + py::overload_cast(&ArticulatedModel::articulationToAxleVelocities), py::arg("linear_velocity_m_s"), - py::arg("articulation_angle_rad")) + py::arg("articulation_angle_rad"), + py::arg("articulation_turning_velocity_rad_s") = 0.0) .def_property_readonly("articulation_to_front_axle", &ArticulatedModel::get_articulation_to_front_axle_m) .def_property_readonly("articulation_to_rear_axle", &ArticulatedModel::get_articulation_to_rear_axle_m) .def_property_readonly("front_track_width", &ArticulatedModel::get_front_track_width_m) diff --git a/test/test_articulated_model.cpp b/test/test_articulated_model.cpp index 564bb95..063d0c7 100644 --- a/test/test_articulated_model.cpp +++ b/test/test_articulated_model.cpp @@ -222,4 +222,41 @@ TEST_CASE("ArticulatedModel roundtrip reverse - bodyVelocityToVehicleState to ar CHECK(axle_vel.front_axle_turning_velocity_rad_s == Approx(angular_velocity).margin(1e-6)); } +TEST_CASE("ArticulatedModel articulationToAxleVelocities - 2-arg delegates to 3-arg with rate=0") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + auto two_arg = model.articulationToAxleVelocities(2.0, 0.3); + auto three_arg_zero = model.articulationToAxleVelocities(2.0, 0.3, 0.0); + + CHECK(two_arg.front_axle_turning_velocity_rad_s == Approx(three_arg_zero.front_axle_turning_velocity_rad_s)); + CHECK(two_arg.rear_axle_turning_velocity_rad_s == Approx(three_arg_zero.rear_axle_turning_velocity_rad_s)); +} + +TEST_CASE("ArticulatedModel articulationToAxleVelocities - nonzero rate adds gamma-dot contribution") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + // From the derivation: omega_rear = omega_front - gamma_dot. So passing a non-zero rate must + // shift the rear-axle turning velocity by exactly -gamma_dot compared to the rate=0 case + // (the front-axle term changes too, but the rear-front difference is exactly gamma_dot). + double gamma_dot = 0.2; + auto rest = model.articulationToAxleVelocities(2.0, 0.3, 0.0); + auto with_rate = model.articulationToAxleVelocities(2.0, 0.3, gamma_dot); + + CHECK(with_rate.front_axle_turning_velocity_rad_s - with_rate.rear_axle_turning_velocity_rad_s == Approx(gamma_dot)); + CHECK(with_rate.front_axle_turning_velocity_rad_s != Approx(rest.front_axle_turning_velocity_rad_s)); +} + +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - 2-arg delegates to 3-arg with rate=0") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + auto two_arg = model.bodyVelocityToVehicleState(2.0, 0.5); + auto three_arg_zero = model.bodyVelocityToVehicleState(2.0, 0.5, 0.0); + + CHECK(two_arg.articulation_angle_rad == Approx(three_arg_zero.articulation_angle_rad)); + CHECK(two_arg.front_axle_turning_radius_m == Approx(three_arg_zero.front_axle_turning_radius_m)); +} + } // namespace polymath::kinematics diff --git a/test/test_articulated_projector.cpp b/test/test_articulated_projector.cpp new file mode 100644 index 0000000..ee41f6c --- /dev/null +++ b/test/test_articulated_projector.cpp @@ -0,0 +1,328 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "catch2_compat.hpp" +#include "polymath_kinematics/articulated_projector.hpp" + +namespace polymath::kinematics +{ + +namespace +{ +// stueve-style parameters +constexpr double FRONT_ARM = 1.66; +constexpr double REAR_ARM = 1.44; +constexpr double FRONT_TRACK = 2.0; +constexpr double REAR_TRACK = 2.0; +constexpr double FRONT_WHEEL_RADIUS = 0.723; +constexpr double REAR_WHEEL_RADIUS = 0.723; +constexpr double MIN_ANGLE = -0.785; +constexpr double MAX_ANGLE = 0.785; + +ArticulatedModel make_model() +{ + return ArticulatedModel(FRONT_ARM, REAR_ARM, FRONT_TRACK, REAR_TRACK, FRONT_WHEEL_RADIUS, REAR_WHEEL_RADIUS); +} +} // namespace + +TEST_CASE("ArticulatedProjector construction stores model and limits") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + CHECK(projector.get_min_articulation_angle_rad() == Approx(MIN_ANGLE)); + CHECK(projector.get_max_articulation_angle_rad() == Approx(MAX_ANGLE)); + CHECK(projector.get_model().get_articulation_to_front_axle_m() == Approx(FRONT_ARM)); +} + +TEST_CASE("ArticulatedProjector step - zero rate freezes articulation angle") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto result = projector.step(0.1, pose, 0.3, 0.6, 0.0, 1.0); + CHECK(result.articulation_angle_rad == Approx(0.3)); +} + +TEST_CASE("ArticulatedProjector step - large rate snaps to target without overshoot") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto result = projector.step(0.1, pose, 0.0, 0.4, 100.0, 1.0); + CHECK(result.articulation_angle_rad == Approx(0.4)); +} + +TEST_CASE("ArticulatedProjector step - rate-limited slew advances by rate*dt") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // rate*dt = 0.02, target far away + auto result = projector.step(0.1, pose, 0.0, 0.5, 0.2, 1.0); + CHECK(result.articulation_angle_rad == Approx(0.02)); +} + +TEST_CASE("ArticulatedProjector step - target above max saturates at max") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto result = projector.step(0.1, pose, 0.0, 2.0, 100.0, 1.0); + CHECK(result.articulation_angle_rad == Approx(MAX_ANGLE)); +} + +TEST_CASE("ArticulatedProjector step - negative articulation rate is treated as magnitude") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto result = projector.step(0.1, pose, 0.0, 0.5, -0.2, 1.0); + CHECK(result.articulation_angle_rad == Approx(0.02)); +} + +TEST_CASE("ArticulatedProjector project - straight line: zero articulation, theta unchanged") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto trajectory = projector.project(1.0, 0.1, pose, 0.0, 0.0, 0.0, 1.5); + CHECK(trajectory.size() == 11); + CHECK(trajectory.back().pose.x == Approx(1.5)); + CHECK(trajectory.back().pose.y == Approx(0.0)); + CHECK(trajectory.back().pose.theta == Approx(0.0)); + CHECK(trajectory.back().articulation_angle_rad == Approx(0.0)); +} + +TEST_CASE("ArticulatedProjector project - articulation ramps then saturates at max") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // target = 0.785 (= max), rate = 0.2 rad/s, dt = 0.1 → step adds 0.02 + // Reach max in ceil(0.785 / 0.02) = 40 steps; horizon 5.0s → 50 steps total. + auto trajectory = projector.project(5.0, 0.1, pose, 0.0, 0.785, 0.2, 1.0); + CHECK(trajectory.size() == 51); + CHECK(trajectory[40].articulation_angle_rad == Approx(0.785)); + CHECK(trajectory.back().articulation_angle_rad == Approx(0.785)); +} + +TEST_CASE("ArticulatedProjector project - initial state stored as element 0") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{1.0, 2.0, 0.5}; + + auto trajectory = projector.project(0.5, 0.1, pose, 0.2, 0.6, 0.5, 1.0); + CHECK(trajectory.front().time_s == Approx(0.0)); + CHECK(trajectory.front().pose.x == Approx(1.0)); + CHECK(trajectory.front().pose.y == Approx(2.0)); + CHECK(trajectory.front().pose.theta == Approx(0.5)); + CHECK(trajectory.front().articulation_angle_rad == Approx(0.2)); +} + +TEST_CASE("ArticulatedProjector step - heading rotates with non-zero articulation at steady state") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // With current == clamped target, the realized gamma-dot is zero (no joint motion this step), + // so the rear-axle turning velocity is purely a function of v and the articulation angle. + // Positive articulation + forward v → positive (CCW) rear-axle omega and theta increase. + auto result = projector.step(0.1, pose, 0.3, 0.3, 100.0, 1.0); + CHECK(result.articulation_angle_rad == Approx(0.3)); + CHECK(result.angular_velocity_rad_s > 0.0); + CHECK(result.pose.theta > 0.0); +} + +TEST_CASE("ArticulatedProjector step - realized gamma-dot affects rear-axle omega") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // Same starting angle, same target, same v: stepping with a small rate (slow ramp) versus + // a large rate (snap) yields different rear-axle angular velocities because gamma-dot + // contributes to the kinematics. + auto slow_ramp = projector.step(0.1, pose, 0.0, 0.3, 0.2, 1.0); // step adds 0.02 (small gamma-dot) + auto snap = projector.step(0.1, pose, 0.0, 0.3, 100.0, 1.0); // snaps to 0.3 (huge gamma-dot) + CHECK(slow_ramp.angular_velocity_rad_s != Approx(snap.angular_velocity_rad_s)); +} + +// Body footprints used by the footprint tests below. Each polygon is measured from its OWN axle. +namespace +{ +// Front body: 2.2 m ahead of the front axle to the bucket, 0.4 m behind it. +constexpr double FRONT_AHEAD = 2.2; +constexpr double FRONT_BEHIND = 0.4; +constexpr double FRONT_BODY_WIDTH = 2.0; +// Rear body: 0.3 m ahead of the rear axle, 2.0 m behind it to the counterweight. +constexpr double REAR_AHEAD = 0.3; +constexpr double REAR_BEHIND = 2.0; +constexpr double REAR_BODY_WIDTH = 2.0; + +ArticulatedProjector make_footprint_projector(AxleReference reference = AxleReference::REAR) +{ + return ArticulatedProjector( + make_model(), + MIN_ANGLE, + MAX_ANGLE, + reference, + rectangleFootprint(FRONT_AHEAD, FRONT_BEHIND, FRONT_BODY_WIDTH), + rectangleFootprint(REAR_AHEAD, REAR_BEHIND, REAR_BODY_WIDTH)); +} +} // namespace + +TEST_CASE("ArticulatedProjector - no footprint set yields empty footprints (never throws)") +{ + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 0.0, 1.0); + CHECK(result.front_footprint.empty()); + CHECK(result.rear_footprint.empty()); + + // project() must also run and produce states with empty footprints. + auto states = projector.project(0.2, 0.1, pose, 0.0, 0.0, 0.0, 1.0); + REQUIRE(states.size() >= 1); + CHECK(states.front().front_footprint.empty()); + CHECK(states.front().rear_footprint.empty()); +} + +TEST_CASE("ArticulatedProjector - joint pose is reported alongside the reference-axle pose") +{ + auto projector = make_footprint_projector(); + // REAR reference: pose is the rear axle at the origin, so the joint sits REAR_ARM ahead. + auto result = projector.step(0.1, Pose2D{0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0); + CHECK(result.pose.x == Approx(0.0)); + CHECK(result.joint_pose.x == Approx(REAR_ARM)); + CHECK(result.joint_pose.y == Approx(0.0)); + CHECK(result.joint_pose.theta == Approx(0.0)); +} + +TEST_CASE("ArticulatedProjector footprint - zero articulation, each body about its own axle") +{ + auto projector = make_footprint_projector(); + // REAR reference: rear axle at the origin, rear-body heading +x. gamma=0 so both bodies align. + auto result = projector.step(0.1, Pose2D{0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0); // v=0 so pose stays + + REQUIRE(result.front_footprint.size() == 4); + REQUIRE(result.rear_footprint.size() == 4); + + // Rear body is measured from the rear axle (the origin here). + CHECK(result.rear_footprint[0].x == Approx(-REAR_BEHIND)); + CHECK(result.rear_footprint[0].y == Approx(-REAR_BODY_WIDTH / 2.0)); + CHECK(result.rear_footprint[1].x == Approx(REAR_AHEAD)); + + // Front axle is a joint arm plus a front arm ahead of the rear axle when gamma = 0. + const double front_axle_x = REAR_ARM + FRONT_ARM; + CHECK(result.front_footprint[0].x == Approx(front_axle_x - FRONT_BEHIND)); + CHECK(result.front_footprint[0].y == Approx(-FRONT_BODY_WIDTH / 2.0)); + CHECK(result.front_footprint[1].x == Approx(front_axle_x + FRONT_AHEAD)); +} + +TEST_CASE("ArticulatedProjector footprint - nonzero articulation swings the front body about the joint") +{ + auto projector = make_footprint_projector(); + const double gamma = 0.3; + auto result = projector.step(0.1, Pose2D{0.0, 0.0, 0.0}, gamma, gamma, 0.0, 0.0); // hold gamma, v=0 + + REQUIRE(result.front_footprint.size() == 4); + // Joint sits REAR_ARM ahead of the rear axle; the front axle is FRONT_ARM further along the + // front heading (theta_rear + gamma). + const double front_theta = gamma; + const double front_axle_x = REAR_ARM + FRONT_ARM * std::cos(front_theta); + const double front_axle_y = FRONT_ARM * std::sin(front_theta); + // Compare bumper midpoints, which lie on the body axis and carry no width offset. + const double bumper_mid_x = (result.front_footprint[1].x + result.front_footprint[2].x) / 2.0; + const double bumper_mid_y = (result.front_footprint[1].y + result.front_footprint[2].y) / 2.0; + CHECK(bumper_mid_x == Approx(front_axle_x + FRONT_AHEAD * std::cos(front_theta))); + CHECK(bumper_mid_y == Approx(front_axle_y + FRONT_AHEAD * std::sin(front_theta))); + // Rear body is unaffected by gamma: still measured straight back from the rear axle. + CHECK(result.rear_footprint[0].x == Approx(-REAR_BEHIND)); + CHECK(result.rear_footprint[0].y == Approx(-REAR_BODY_WIDTH / 2.0)); +} + +TEST_CASE("ArticulatedProjector - FRONT reference reports the front axle and its body heading") +{ + const double gamma = 0.3; + auto projector = make_footprint_projector(AxleReference::FRONT); + // Seed with the front-axle pose that corresponds to a rear axle at the origin heading +x. + const double front_theta = gamma; + const Pose2D front_start{ + REAR_ARM + FRONT_ARM * std::cos(front_theta), FRONT_ARM * std::sin(front_theta), front_theta}; + auto result = projector.step(0.1, front_start, gamma, gamma, 0.0, 0.0); // hold gamma, v=0 + + // Pose comes back at the front axle with the FRONT-body heading, and the joint is unchanged. + CHECK(result.pose.x == Approx(front_start.x)); + CHECK(result.pose.y == Approx(front_start.y)); + CHECK(result.pose.theta == Approx(front_theta)); + CHECK(result.joint_pose.x == Approx(REAR_ARM)); + CHECK(result.joint_pose.y == Approx(0.0)); + CHECK(result.joint_pose.theta == Approx(0.0)); +} + +TEST_CASE("ArticulatedProjector - FRONT and REAR references describe the same physical motion") +{ + const double gamma = 0.25; + auto rear_projector = make_footprint_projector(AxleReference::REAR); + auto front_projector = make_footprint_projector(AxleReference::FRONT); + + const double front_theta = gamma; + const Pose2D rear_start{0.0, 0.0, 0.0}; + const Pose2D front_start{ + REAR_ARM + FRONT_ARM * std::cos(front_theta), FRONT_ARM * std::sin(front_theta), front_theta}; + + auto rear_traj = rear_projector.project(1.0, 0.05, rear_start, gamma, gamma, 0.0, 1.0); + auto front_traj = front_projector.project(1.0, 0.05, front_start, gamma, gamma, 0.0, 1.0); + REQUIRE(rear_traj.size() == front_traj.size()); + + // Same vehicle: the joint pose and both world-frame footprints must agree sample for sample, + // regardless of which axle the caller chose to reference. + for (std::size_t i = 0; i < rear_traj.size(); ++i) { + CHECK(front_traj[i].joint_pose.x == Approx(rear_traj[i].joint_pose.x)); + CHECK(front_traj[i].joint_pose.y == Approx(rear_traj[i].joint_pose.y)); + CHECK(front_traj[i].joint_pose.theta == Approx(rear_traj[i].joint_pose.theta)); + REQUIRE(front_traj[i].front_footprint.size() == rear_traj[i].front_footprint.size()); + for (std::size_t k = 0; k < rear_traj[i].front_footprint.size(); ++k) { + CHECK(front_traj[i].front_footprint[k].x == Approx(rear_traj[i].front_footprint[k].x)); + CHECK(front_traj[i].front_footprint[k].y == Approx(rear_traj[i].front_footprint[k].y)); + CHECK(front_traj[i].rear_footprint[k].x == Approx(rear_traj[i].rear_footprint[k].x)); + CHECK(front_traj[i].rear_footprint[k].y == Approx(rear_traj[i].rear_footprint[k].y)); + } + } +} + +TEST_CASE("ArticulatedProjector footprint - arbitrary polygons are carried through vertex for vertex") +{ + // Five-vertex front body, three-vertex rear body: nothing may assume 4 corners. + const Footprint front_body{ + Point2D{-0.4, -1.0}, Point2D{1.8, -1.0}, Point2D{2.4, 0.0}, Point2D{1.8, 1.0}, Point2D{-0.4, 1.0}}; + const Footprint rear_body{Point2D{0.3, -0.9}, Point2D{-1.9, 0.0}, Point2D{0.3, 0.9}}; + ArticulatedProjector projector(make_model(), MIN_ANGLE, MAX_ANGLE, AxleReference::REAR, front_body, rear_body); + + auto result = projector.step(0.1, Pose2D{0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0); + REQUIRE(result.front_footprint.size() == front_body.size()); + REQUIRE(result.rear_footprint.size() == rear_body.size()); + // gamma = 0 and rear axle at the origin: rear body passes through unchanged, front body shifts + // by the full wheelbase with no rotation. + const double front_axle_x = REAR_ARM + FRONT_ARM; + for (std::size_t i = 0; i < rear_body.size(); ++i) { + CHECK(result.rear_footprint[i].x == Approx(rear_body[i].x)); + CHECK(result.rear_footprint[i].y == Approx(rear_body[i].y)); + } + for (std::size_t i = 0; i < front_body.size(); ++i) { + CHECK(result.front_footprint[i].x == Approx(front_axle_x + front_body[i].x)); + CHECK(result.front_footprint[i].y == Approx(front_body[i].y)); + } +} + +} // namespace polymath::kinematics diff --git a/test/test_bicycle_projector.cpp b/test/test_bicycle_projector.cpp new file mode 100644 index 0000000..2a6d201 --- /dev/null +++ b/test/test_bicycle_projector.cpp @@ -0,0 +1,256 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "catch2_compat.hpp" +#include "polymath_kinematics/bicycle_projector.hpp" + +namespace polymath::kinematics +{ + +namespace +{ +constexpr double WHEELBASE = 2.5; +constexpr double TRACK = 1.5; +constexpr double WHEEL_RADIUS = 0.3; +constexpr double MIN_ANGLE = -0.6; +constexpr double MAX_ANGLE = 0.6; +} // namespace + +TEST_CASE("BicycleProjector construction stores model and limits") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + CHECK(projector.get_min_steering_angle_rad() == Approx(MIN_ANGLE)); + CHECK(projector.get_max_steering_angle_rad() == Approx(MAX_ANGLE)); + CHECK(projector.get_model().get_wheelbase_m() == Approx(WHEELBASE)); +} + +TEST_CASE("BicycleProjector step - zero rate freezes the steering angle") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto result = projector.step(0.1, pose, 0.2, 0.5, 0.0, 1.0); + CHECK(result.steering_angle_rad == Approx(0.2)); +} + +TEST_CASE("BicycleProjector step - large rate reaches target in one step without overshoot") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // |delta| = 0.3, max_delta = rate * dt = 10.0 * 0.1 = 1.0 >> 0.3 → snaps exactly to target. + auto result = projector.step(0.1, pose, 0.0, 0.3, 10.0, 1.0); + CHECK(result.steering_angle_rad == Approx(0.3)); +} + +TEST_CASE("BicycleProjector step - rate-limited slew advances by rate*dt toward target") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // rate*dt = 0.05, target far away → expect 0.05 increment. + auto result = projector.step(0.1, pose, 0.0, 0.5, 0.5, 1.0); + CHECK(result.steering_angle_rad == Approx(0.05)); +} + +TEST_CASE("BicycleProjector step - negative steering rate is treated as magnitude") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto result = projector.step(0.1, pose, 0.0, 0.5, -0.5, 1.0); + CHECK(result.steering_angle_rad == Approx(0.05)); +} + +TEST_CASE("BicycleProjector step - target above max saturates at max") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // With large rate the angle snaps to clamped_target == max. + auto result = projector.step(0.1, pose, 0.0, 5.0, 100.0, 1.0); + CHECK(result.steering_angle_rad == Approx(MAX_ANGLE)); +} + +TEST_CASE("BicycleProjector project - straight line traces +x with theta unchanged") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto trajectory = projector.project(1.0, 0.1, pose, 0.0, 0.0, 1.0, 2.0); + // ceil(1.0 / 0.1) + 1 = 11 samples + CHECK(trajectory.size() == 11); + CHECK(trajectory.front().time_s == Approx(0.0)); + CHECK(trajectory.front().pose.x == Approx(0.0)); + CHECK(trajectory.back().time_s == Approx(1.0)); + CHECK(trajectory.back().pose.x == Approx(2.0)); // x = v * t + CHECK(trajectory.back().pose.y == Approx(0.0)); + CHECK(trajectory.back().pose.theta == Approx(0.0)); +} + +TEST_CASE("BicycleProjector project - ramp reaches target in expected number of steps") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + // target=0.5, rate=0.5, dt=0.1 → step adds 0.05; takes 10 steps to reach 0.5. + auto trajectory = projector.project(2.0, 0.1, pose, 0.0, 0.5, 0.5, 0.0); + CHECK(trajectory[10].steering_angle_rad == Approx(0.5)); + CHECK(trajectory.back().steering_angle_rad == Approx(0.5)); // pinned to target afterwards +} + +TEST_CASE("BicycleProjector project - sign symmetry: negating target mirrors trajectory across y=0") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + + auto left = projector.project(2.0, 0.05, pose, 0.0, 0.4, 1.0, 1.0); + auto right = projector.project(2.0, 0.05, pose, 0.0, -0.4, 1.0, 1.0); + REQUIRE(left.size() == right.size()); + + for (std::size_t i = 0; i < left.size(); ++i) { + CHECK(left[i].pose.x == Approx(right[i].pose.x)); + CHECK(left[i].pose.y == Approx(-right[i].pose.y)); + CHECK(left[i].pose.theta == Approx(-right[i].pose.theta)); + CHECK(left[i].steering_angle_rad == Approx(-right[i].steering_angle_rad)); + } +} + +TEST_CASE("BicycleProjector project - initial state stored as element 0") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{1.5, -2.0, 0.25}; + + auto trajectory = projector.project(0.5, 0.1, pose, 0.1, 0.3, 0.5, 1.0); + CHECK(trajectory.front().time_s == Approx(0.0)); + CHECK(trajectory.front().pose.x == Approx(1.5)); + CHECK(trajectory.front().pose.y == Approx(-2.0)); + CHECK(trajectory.front().pose.theta == Approx(0.25)); + CHECK(trajectory.front().steering_angle_rad == Approx(0.1)); +} + +TEST_CASE("BicycleProjector - no footprint set yields an empty footprint (never throws)") +{ + BicycleProjector projector(BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE); + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 0.0, 1.0); + CHECK(result.footprint.empty()); +} + +TEST_CASE("BicycleProjector footprint - rectangle corners at a known pose") +{ + constexpr double FRONT = 3.0; + constexpr double REAR = 1.0; + constexpr double WIDTH = 2.0; + BicycleProjector projector( + BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), + MIN_ANGLE, + MAX_ANGLE, + AxleReference::REAR, + rectangleFootprint(FRONT, REAR, WIDTH)); + + // v=0, zero steering: pose stays at the origin with heading +x. + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 0.0, 0.0); + REQUIRE(result.footprint.size() == 4); + // Body frame corners: rear-right, front-right, front-left, rear-left. + CHECK(result.footprint[0].x == Approx(-REAR)); + CHECK(result.footprint[0].y == Approx(-WIDTH / 2.0)); + CHECK(result.footprint[1].x == Approx(FRONT)); + CHECK(result.footprint[1].y == Approx(-WIDTH / 2.0)); + CHECK(result.footprint[2].x == Approx(FRONT)); + CHECK(result.footprint[2].y == Approx(WIDTH / 2.0)); + CHECK(result.footprint[3].x == Approx(-REAR)); + CHECK(result.footprint[3].y == Approx(WIDTH / 2.0)); +} + +TEST_CASE("BicycleProjector footprint - arbitrary polygon is carried through vertex for vertex") +{ + // A 5-vertex tapered nose, deliberately not a rectangle. + const Footprint body{ + Point2D{-1.0, -0.9}, Point2D{2.0, -0.9}, Point2D{2.8, 0.0}, Point2D{2.0, 0.9}, Point2D{-1.0, 0.9}}; + BicycleProjector projector( + BicycleModel(WHEELBASE, TRACK, WHEEL_RADIUS), MIN_ANGLE, MAX_ANGLE, AxleReference::REAR, body); + + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 0.0, 0.0); + REQUIRE(result.footprint.size() == body.size()); + for (std::size_t i = 0; i < body.size(); ++i) { + CHECK(result.footprint[i].x == Approx(body[i].x)); + CHECK(result.footprint[i].y == Approx(body[i].y)); + } +} + +TEST_CASE("BicycleProjector - FRONT reference offsets the pose by a wheelbase") +{ + const BicycleModel model(WHEELBASE, TRACK, WHEEL_RADIUS); + BicycleProjector rear(model, MIN_ANGLE, MAX_ANGLE, AxleReference::REAR); + BicycleProjector front(model, MIN_ANGLE, MAX_ANGLE, AxleReference::FRONT); + + // Straight ahead from the origin: the front-axle pose leads the rear-axle pose by the wheelbase. + auto rear_traj = rear.project(1.0, 0.1, Pose2D{0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 1.0); + auto front_traj = front.project(1.0, 0.1, Pose2D{WHEELBASE, 0.0, 0.0}, 0.0, 0.0, 0.0, 1.0); + REQUIRE(rear_traj.size() == front_traj.size()); + for (std::size_t i = 0; i < rear_traj.size(); ++i) { + CHECK(front_traj[i].pose.x == Approx(rear_traj[i].pose.x + WHEELBASE)); + CHECK(front_traj[i].pose.y == Approx(rear_traj[i].pose.y)); + CHECK(front_traj[i].pose.theta == Approx(rear_traj[i].pose.theta)); + } +} + +TEST_CASE("BicycleProjector - FRONT reference traces the same curve as REAR, offset forward") +{ + const BicycleModel model(WHEELBASE, TRACK, WHEEL_RADIUS); + const double steering = 0.3; + BicycleProjector rear(model, MIN_ANGLE, MAX_ANGLE, AxleReference::REAR); + BicycleProjector front(model, MIN_ANGLE, MAX_ANGLE, AxleReference::FRONT); + + // Same physical vehicle, same turn: seed each with its own axle's start pose. + auto rear_traj = rear.project(2.0, 0.05, Pose2D{0.0, 0.0, 0.0}, steering, steering, 0.0, 1.5); + auto front_traj = front.project(2.0, 0.05, Pose2D{WHEELBASE, 0.0, 0.0}, steering, steering, 0.0, 1.5); + REQUIRE(rear_traj.size() == front_traj.size()); + for (std::size_t i = 0; i < rear_traj.size(); ++i) { + // Headings match (one rigid chassis) and the front axle sits a wheelbase ahead along it. + const double theta = rear_traj[i].pose.theta; + CHECK(front_traj[i].pose.theta == Approx(theta)); + CHECK(front_traj[i].pose.x == Approx(rear_traj[i].pose.x + WHEELBASE * std::cos(theta))); + CHECK(front_traj[i].pose.y == Approx(rear_traj[i].pose.y + WHEELBASE * std::sin(theta))); + } +} + +TEST_CASE("BicycleProjector - footprint is anchored at whichever axle is the reference") +{ + const BicycleModel model(WHEELBASE, TRACK, WHEEL_RADIUS); + // Same physical body described from each axle: bumper 0.5 m past the front axle, 1.0 m behind + // the rear axle. From the rear axle that is (WHEELBASE + 0.5) forward; from the front axle it is + // 0.5 forward and (WHEELBASE + 1.0) back. + BicycleProjector rear( + model, MIN_ANGLE, MAX_ANGLE, AxleReference::REAR, rectangleFootprint(WHEELBASE + 0.5, 1.0, 2.0)); + BicycleProjector front( + model, MIN_ANGLE, MAX_ANGLE, AxleReference::FRONT, rectangleFootprint(0.5, WHEELBASE + 1.0, 2.0)); + + auto rear_state = rear.step(0.1, Pose2D{0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0); + auto front_state = front.step(0.1, Pose2D{WHEELBASE, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0); + REQUIRE(rear_state.footprint.size() == 4); + REQUIRE(front_state.footprint.size() == 4); + // Both descriptions must land the body in the same place in the world. + for (std::size_t i = 0; i < 4; ++i) { + CHECK(front_state.footprint[i].x == Approx(rear_state.footprint[i].x)); + CHECK(front_state.footprint[i].y == Approx(rear_state.footprint[i].y)); + } +} + +} // namespace polymath::kinematics diff --git a/test/test_differential_drive_projector.cpp b/test/test_differential_drive_projector.cpp new file mode 100644 index 0000000..138062e --- /dev/null +++ b/test/test_differential_drive_projector.cpp @@ -0,0 +1,185 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "catch2_compat.hpp" +#include "polymath_kinematics/differential_drive_projector.hpp" + +namespace polymath::kinematics +{ + +namespace +{ +constexpr double WHEEL_RADIUS = 0.1; +constexpr double TRACK_WIDTH = 0.5; +constexpr double V_MIN = -2.0; +constexpr double V_MAX = 2.0; +constexpr double OMEGA_MIN = -3.0; +constexpr double OMEGA_MAX = 3.0; + +DifferentialDriveProjector makeProjector() +{ + return DifferentialDriveProjector( + DifferentialDriveModel(WHEEL_RADIUS, TRACK_WIDTH), V_MIN, V_MAX, OMEGA_MIN, OMEGA_MAX); +} +} // namespace + +TEST_CASE("DifferentialDriveProjector construction stores model and limits") +{ + auto projector = makeProjector(); + CHECK(projector.get_min_linear_velocity_m_s() == Approx(V_MIN)); + CHECK(projector.get_max_linear_velocity_m_s() == Approx(V_MAX)); + CHECK(projector.get_min_angular_velocity_rad_s() == Approx(OMEGA_MIN)); + CHECK(projector.get_max_angular_velocity_rad_s() == Approx(OMEGA_MAX)); + CHECK(projector.get_model().get_wheel_radius_m() == Approx(WHEEL_RADIUS)); +} + +TEST_CASE("DifferentialDriveProjector step - zero acceleration freezes both velocities") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.5, 0.2, 1.5, 1.0, 0.0, 0.0); + CHECK(result.linear_velocity_m_s == Approx(0.5)); + CHECK(result.angular_velocity_rad_s == Approx(0.2)); +} + +TEST_CASE("DifferentialDriveProjector step - large acceleration snaps to target without overshoot") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + // |delta_v| = 1.0, |delta_omega| = 0.4 — much smaller than 100 * 0.1 = 10.0. + auto result = projector.step(0.1, pose, 0.0, 0.0, 1.0, 0.4, 100.0, 100.0); + CHECK(result.linear_velocity_m_s == Approx(1.0)); + CHECK(result.angular_velocity_rad_s == Approx(0.4)); +} + +TEST_CASE("DifferentialDriveProjector step - rate-limited ramps advance by accel*dt") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + // accel*dt = 0.5 * 0.1 = 0.05 for linear; 0.3 * 0.1 = 0.03 for angular. + auto result = projector.step(0.1, pose, 0.0, 0.0, 1.0, 0.5, 0.5, 0.3); + CHECK(result.linear_velocity_m_s == Approx(0.05)); + CHECK(result.angular_velocity_rad_s == Approx(0.03)); +} + +TEST_CASE("DifferentialDriveProjector step - target above linear max saturates at max") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 100.0, 100.0, 1000.0, 1000.0); + CHECK(result.linear_velocity_m_s == Approx(V_MAX)); + CHECK(result.angular_velocity_rad_s == Approx(OMEGA_MAX)); +} + +TEST_CASE("DifferentialDriveProjector step - negative acceleration is treated as magnitude") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 1.0, 0.5, -0.5, -0.3); + CHECK(result.linear_velocity_m_s == Approx(0.05)); + CHECK(result.angular_velocity_rad_s == Approx(0.03)); +} + +TEST_CASE("DifferentialDriveProjector project - straight line traces +x at constant v") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + // Constant linear=1.0, zero angular, large accels so the ramp is irrelevant. + auto trajectory = projector.project(1.0, 0.1, pose, 1.0, 0.0, 1.0, 0.0, 100.0, 100.0); + CHECK(trajectory.size() == 11); + CHECK(trajectory.front().time_s == Approx(0.0)); + CHECK(trajectory.back().time_s == Approx(1.0)); + CHECK(trajectory.back().pose.x == Approx(1.0)); + CHECK(trajectory.back().pose.y == Approx(0.0)); + CHECK(trajectory.back().pose.theta == Approx(0.0)); +} + +TEST_CASE("DifferentialDriveProjector project - initial state stored as element 0") +{ + auto projector = makeProjector(); + Pose2D pose{0.5, -1.0, 0.25}; + auto trajectory = projector.project(0.5, 0.1, pose, 0.2, 0.1, 0.5, 0.3, 0.5, 0.5); + CHECK(trajectory.front().time_s == Approx(0.0)); + CHECK(trajectory.front().pose.x == Approx(0.5)); + CHECK(trajectory.front().pose.y == Approx(-1.0)); + CHECK(trajectory.front().pose.theta == Approx(0.25)); + CHECK(trajectory.front().linear_velocity_m_s == Approx(0.2)); + CHECK(trajectory.front().angular_velocity_rad_s == Approx(0.1)); +} + +TEST_CASE("DifferentialDriveProjector step - wheel speeds derive from body command") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + // After snap, body command is (1.0 m/s, 0.0 rad/s); wheel speeds should both be 1.0 / 0.1 = 10 rad/s. + auto result = projector.step(0.1, pose, 0.0, 0.0, 1.0, 0.0, 100.0, 100.0); + CHECK(result.wheel_velocities.left_wheel_velocity_rad_s == Approx(10.0)); + CHECK(result.wheel_velocities.right_wheel_velocity_rad_s == Approx(10.0)); +} + +TEST_CASE("DifferentialDriveProjector - no footprint set yields an empty footprint (never throws)") +{ + auto projector = makeProjector(); + Pose2D pose{0.0, 0.0, 0.0}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 0.0, 0.0, 100.0, 100.0); + CHECK(result.footprint.empty()); +} + +TEST_CASE("DifferentialDriveProjector footprint - rectangle corners rotated by heading") +{ + constexpr double FRONT = 1.5; + constexpr double REAR = 0.5; + constexpr double WIDTH = 1.0; + DifferentialDriveProjector projector( + DifferentialDriveModel(WHEEL_RADIUS, TRACK_WIDTH), + V_MIN, + V_MAX, + OMEGA_MIN, + OMEGA_MAX, + rectangleFootprint(FRONT, REAR, WIDTH)); + + // Heading = +90 deg (theta = pi/2), v=0 so pose stays at the origin. + const double theta = M_PI / 2.0; + Pose2D pose{0.0, 0.0, theta}; + auto result = projector.step(0.1, pose, 0.0, 0.0, 0.0, 0.0, 100.0, 100.0); + REQUIRE(result.footprint.size() == 4); + // Body-frame front-right corner (FRONT, -WIDTH/2) rotated by +90deg: world = (+WIDTH/2, FRONT). + CHECK(result.footprint[1].x == Approx(WIDTH / 2.0)); + CHECK(result.footprint[1].y == Approx(FRONT)); + // Body-frame rear-right corner (-REAR, -WIDTH/2) rotated by +90deg: world = (+WIDTH/2, -REAR). + CHECK(result.footprint[0].x == Approx(WIDTH / 2.0)); + CHECK(result.footprint[0].y == Approx(-REAR)); +} + +TEST_CASE("DifferentialDriveProjector footprint - arbitrary polygon is rotated vertex for vertex") +{ + // A 3-vertex body, to prove nothing assumes 4 corners. No vertex sits on an axis, so the + // expected values stay clear of zero where Approx has no relative epsilon to work with. + const Footprint body{Point2D{1.0, 0.2}, Point2D{-0.5, 0.6}, Point2D{-0.5, -0.6}}; + DifferentialDriveProjector projector( + DifferentialDriveModel(WHEEL_RADIUS, TRACK_WIDTH), V_MIN, V_MAX, OMEGA_MIN, OMEGA_MAX, body); + + const double theta = M_PI / 2.0; + auto result = projector.step(0.1, Pose2D{0.0, 0.0, theta}, 0.0, 0.0, 0.0, 0.0, 100.0, 100.0); + REQUIRE(result.footprint.size() == body.size()); + for (std::size_t i = 0; i < body.size(); ++i) { + // +90 deg rotation maps (x, y) -> (-y, x). + CHECK(result.footprint[i].x == Approx(-body[i].y)); + CHECK(result.footprint[i].y == Approx(body[i].x)); + } +} + +} // namespace polymath::kinematics