add forward projectors for all three kinematic models

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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -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()
 

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;

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 <utility>
+#include <vector>
+
+#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<ArticulatedProjectedState> 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__