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