diff --git a/CMakeLists.txt b/CMakeLists.txt index ed5b7d6..e654c43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -130,7 +130,9 @@ if(BUILD_TESTING) if(ament_cmake_FOUND) find_package(ament_cmake_pytest REQUIRED) - ament_add_pytest_test(test_python_bindings test) + ament_add_pytest_test(test_python_bindings test/test_python_bindings.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) endif() endif() diff --git a/polymath_kinematics/__init__.py b/polymath_kinematics/__init__.py index fdf2e74..54c7a19 100644 --- a/polymath_kinematics/__init__.py +++ b/polymath_kinematics/__init__.py @@ -12,64 +12,53 @@ # See the License for the specific language governing permissions and # limitations under the License. -import math - from polymath_kinematics_cpp import ( ArticulatedAxleVelocities, ArticulatedModel, + ArticulatedProjectedState, + ArticulatedProjector, ArticulatedVehicleState, + AxleReference, BicycleBodyVelocity, BicycleModel, + BicycleProjectedState, + BicycleProjector, BicycleSteeringState, DifferentialDriveBodyVelocity, DifferentialDriveModel, + DifferentialDriveProjectedState, + DifferentialDriveProjector, DifferentialDriveWheelVelocities, + Point2D, + Pose2D, + rectangle_footprint, + transform_footprint, ) +# The C++ Footprint is std::vector, which pybind11 exposes as a plain list of Point2D. +# Alias it so annotations can name the concept. +Footprint = list[Point2D] + __all__ = [ 'ArticulatedAxleVelocities', 'ArticulatedModel', + 'ArticulatedProjectedState', + 'ArticulatedProjector', 'ArticulatedVehicleState', + 'AxleReference', 'BicycleBodyVelocity', 'BicycleModel', + 'BicycleProjectedState', + 'BicycleProjector', 'BicycleSteeringState', 'DifferentialDriveBodyVelocity', 'DifferentialDriveModel', + 'DifferentialDriveProjectedState', + 'DifferentialDriveProjector', 'DifferentialDriveWheelVelocities', + 'Footprint', + 'Point2D', 'Pose2D', - 'Twist2D', - 'normalize_angle', - 'transform_pose', + 'rectangle_footprint', + 'transform_footprint', ] - - -class Pose2D: - def __init__(self, x=0.0, y=0.0, theta=0.0): - self.x = x - self.y = y - self.theta = theta - - -class Twist2D: - def __init__(self, linear_x=0.0, linear_y=0.0, angular_z=0.0): - self.linear_x = linear_x - self.linear_y = linear_y - self.angular_z = angular_z - - -def normalize_angle(angle): - """Normalize angle to [-pi, pi]""" - while angle > math.pi: - angle -= 2.0 * math.pi - while angle < -math.pi: - angle += 2.0 * math.pi - return angle - - -def transform_pose(pose, transform): - """Transform a pose by a relative transform""" - result = Pose2D() - result.x = pose.x + transform.x * math.cos(pose.theta) - transform.y * math.sin(pose.theta) - result.y = pose.y + transform.x * math.sin(pose.theta) + transform.y * math.cos(pose.theta) - result.theta = normalize_angle(pose.theta + transform.theta) - return result diff --git a/src/kinematics_pybind.cpp b/src/kinematics_pybind.cpp index b2ecf9d..6d44e26 100644 --- a/src/kinematics_pybind.cpp +++ b/src/kinematics_pybind.cpp @@ -13,9 +13,14 @@ // limitations under the License. #include "polymath_kinematics/articulated_model.hpp" +#include "polymath_kinematics/articulated_projector.hpp" #include "polymath_kinematics/bicycle_model.hpp" +#include "polymath_kinematics/bicycle_projector.hpp" #include "polymath_kinematics/differential_drive_model.hpp" +#include "polymath_kinematics/differential_drive_projector.hpp" +#include "polymath_kinematics/pose2d.hpp" #include "pybind11/pybind11.h" +#include "pybind11/stl.h" namespace py = pybind11; @@ -132,6 +137,186 @@ PYBIND11_MODULE(polymath_kinematics_cpp, m) .def_property_readonly("wheelbase", &BicycleModel::get_wheelbase_m) .def_property_readonly("track_width", &BicycleModel::get_track_width_m) .def_property_readonly("wheel_radius", &BicycleModel::get_wheel_radius_m); + + // Shared Pose2D + py::class_(m, "Pose2D") + .def(py::init<>()) + .def( + py::init([](double x, double y, double theta) { return Pose2D{x, y, theta}; }), + py::arg("x") = 0.0, + py::arg("y") = 0.0, + py::arg("theta") = 0.0) + .def_readwrite("x", &Pose2D::x) + .def_readwrite("y", &Pose2D::y) + .def_readwrite("theta", &Pose2D::theta); + + // Shared Point2D (footprint polygon vertex). Footprint = std::vector binds to a + // Python list of Point2D via pybind11/stl.h. + py::class_(m, "Point2D") + .def(py::init<>()) + .def(py::init([](double x, double y) { return Point2D{x, y}; }), py::arg("x") = 0.0, py::arg("y") = 0.0) + .def_readwrite("x", &Point2D::x) + .def_readwrite("y", &Point2D::y); + + py::enum_(m, "AxleReference", "Which axle a projector's poses and footprint are measured from.") + .value("FRONT", AxleReference::FRONT) + .value("REAR", AxleReference::REAR); + + m.def( + "rectangle_footprint", + &rectangleFootprint, + py::arg("front_m"), + py::arg("rear_m"), + py::arg("width_m"), + "Build a rectangular body-frame footprint (CCW, open): rear-right, front-right, front-left, rear-left."); + + m.def( + "transform_footprint", + &transformFootprint, + py::arg("body_frame"), + py::arg("pose"), + "Transform a body-frame footprint into the world frame by `pose`."); + + // Bicycle projector bindings + py::class_(m, "BicycleProjectedState") + .def(py::init<>()) + .def_readwrite("time_s", &BicycleProjectedState::time_s) + .def_readwrite("pose", &BicycleProjectedState::pose) + .def_readwrite("steering_angle_rad", &BicycleProjectedState::steering_angle_rad) + .def_readwrite("linear_velocity_m_s", &BicycleProjectedState::linear_velocity_m_s) + .def_readwrite("angular_velocity_rad_s", &BicycleProjectedState::angular_velocity_rad_s) + .def_readwrite("steering_state", &BicycleProjectedState::steering_state) + .def_readwrite("footprint", &BicycleProjectedState::footprint); + + py::class_(m, "BicycleProjector") + .def( + py::init(), + py::arg("model"), + py::arg("min_steering_angle_rad"), + py::arg("max_steering_angle_rad"), + py::arg("axle_reference") = AxleReference::REAR, + py::arg("footprint") = Footprint{}) + .def( + "step", + &BicycleProjector::step, + py::arg("dt_s"), + py::arg("current_pose"), + py::arg("current_steering_angle_rad"), + py::arg("target_steering_angle_rad"), + py::arg("steering_rate_rad_s"), + py::arg("linear_velocity_m_s")) + .def( + "project", + &BicycleProjector::project, + py::arg("horizon_s"), + py::arg("dt_s"), + py::arg("initial_pose"), + py::arg("initial_steering_angle_rad"), + py::arg("target_steering_angle_rad"), + py::arg("steering_rate_rad_s"), + py::arg("linear_velocity_m_s")) + .def_property_readonly("model", &BicycleProjector::get_model, py::return_value_policy::reference_internal) + .def_property_readonly("min_steering_angle_rad", &BicycleProjector::get_min_steering_angle_rad) + .def_property_readonly("max_steering_angle_rad", &BicycleProjector::get_max_steering_angle_rad) + .def_property_readonly("axle_reference", &BicycleProjector::get_axle_reference) + .def_property_readonly("footprint", &BicycleProjector::get_footprint); + + // Articulated projector bindings + py::class_(m, "ArticulatedProjectedState") + .def(py::init<>()) + .def_readwrite("time_s", &ArticulatedProjectedState::time_s) + .def_readwrite("pose", &ArticulatedProjectedState::pose) + .def_readwrite("articulation_angle_rad", &ArticulatedProjectedState::articulation_angle_rad) + .def_readwrite("linear_velocity_m_s", &ArticulatedProjectedState::linear_velocity_m_s) + .def_readwrite("angular_velocity_rad_s", &ArticulatedProjectedState::angular_velocity_rad_s) + .def_readwrite("vehicle_state", &ArticulatedProjectedState::vehicle_state) + .def_readwrite("joint_pose", &ArticulatedProjectedState::joint_pose) + .def_readwrite("front_footprint", &ArticulatedProjectedState::front_footprint) + .def_readwrite("rear_footprint", &ArticulatedProjectedState::rear_footprint); + + py::class_(m, "ArticulatedProjector") + .def( + py::init(), + py::arg("model"), + py::arg("min_articulation_angle_rad"), + py::arg("max_articulation_angle_rad"), + py::arg("axle_reference") = AxleReference::REAR, + py::arg("front_footprint") = Footprint{}, + py::arg("rear_footprint") = Footprint{}) + .def( + "step", + &ArticulatedProjector::step, + py::arg("dt_s"), + py::arg("current_pose"), + py::arg("current_articulation_angle_rad"), + py::arg("target_articulation_angle_rad"), + py::arg("articulation_rate_rad_s"), + py::arg("linear_velocity_m_s")) + .def( + "project", + &ArticulatedProjector::project, + py::arg("horizon_s"), + py::arg("dt_s"), + py::arg("initial_pose"), + py::arg("initial_articulation_angle_rad"), + py::arg("target_articulation_angle_rad"), + py::arg("articulation_rate_rad_s"), + py::arg("linear_velocity_m_s")) + .def_property_readonly("model", &ArticulatedProjector::get_model, py::return_value_policy::reference_internal) + .def_property_readonly("min_articulation_angle_rad", &ArticulatedProjector::get_min_articulation_angle_rad) + .def_property_readonly("max_articulation_angle_rad", &ArticulatedProjector::get_max_articulation_angle_rad) + .def_property_readonly("axle_reference", &ArticulatedProjector::get_axle_reference) + .def_property_readonly("front_footprint", &ArticulatedProjector::get_front_footprint) + .def_property_readonly("rear_footprint", &ArticulatedProjector::get_rear_footprint); + + // Differential drive projector bindings + py::class_(m, "DifferentialDriveProjectedState") + .def(py::init<>()) + .def_readwrite("time_s", &DifferentialDriveProjectedState::time_s) + .def_readwrite("pose", &DifferentialDriveProjectedState::pose) + .def_readwrite("linear_velocity_m_s", &DifferentialDriveProjectedState::linear_velocity_m_s) + .def_readwrite("angular_velocity_rad_s", &DifferentialDriveProjectedState::angular_velocity_rad_s) + .def_readwrite("wheel_velocities", &DifferentialDriveProjectedState::wheel_velocities) + .def_readwrite("footprint", &DifferentialDriveProjectedState::footprint); + + py::class_(m, "DifferentialDriveProjector") + .def( + py::init(), + py::arg("model"), + py::arg("min_linear_velocity_m_s"), + py::arg("max_linear_velocity_m_s"), + py::arg("min_angular_velocity_rad_s"), + py::arg("max_angular_velocity_rad_s"), + py::arg("footprint") = Footprint{}) + .def( + "step", + &DifferentialDriveProjector::step, + py::arg("dt_s"), + py::arg("current_pose"), + py::arg("current_linear_velocity_m_s"), + py::arg("current_angular_velocity_rad_s"), + py::arg("target_linear_velocity_m_s"), + py::arg("target_angular_velocity_rad_s"), + py::arg("linear_acceleration_m_s2"), + py::arg("angular_acceleration_rad_s2")) + .def( + "project", + &DifferentialDriveProjector::project, + py::arg("horizon_s"), + py::arg("dt_s"), + py::arg("initial_pose"), + py::arg("initial_linear_velocity_m_s"), + py::arg("initial_angular_velocity_rad_s"), + py::arg("target_linear_velocity_m_s"), + py::arg("target_angular_velocity_rad_s"), + py::arg("linear_acceleration_m_s2"), + py::arg("angular_acceleration_rad_s2")) + .def_property_readonly("model", &DifferentialDriveProjector::get_model, py::return_value_policy::reference_internal) + .def_property_readonly("min_linear_velocity_m_s", &DifferentialDriveProjector::get_min_linear_velocity_m_s) + .def_property_readonly("max_linear_velocity_m_s", &DifferentialDriveProjector::get_max_linear_velocity_m_s) + .def_property_readonly("min_angular_velocity_rad_s", &DifferentialDriveProjector::get_min_angular_velocity_rad_s) + .def_property_readonly("max_angular_velocity_rad_s", &DifferentialDriveProjector::get_max_angular_velocity_rad_s) + .def_property_readonly("footprint", &DifferentialDriveProjector::get_footprint); } } // namespace polymath::kinematics diff --git a/test/test_python_bindings.py b/test/test_python_bindings.py index 1cf1880..3514c38 100644 --- a/test/test_python_bindings.py +++ b/test/test_python_bindings.py @@ -18,8 +18,16 @@ from polymath_kinematics import ( ArticulatedModel, + ArticulatedProjector, + AxleReference, BicycleModel, + BicycleProjector, DifferentialDriveModel, + DifferentialDriveProjector, + Point2D, + Pose2D, + rectangle_footprint, + transform_footprint, ) @@ -141,3 +149,309 @@ def test_reverse_roundtrip(self): axle_vel = model.articulation_to_axle_velocities(linear_velocity, vehicle_state.articulation_angle_rad) assert axle_vel.front_axle_turning_velocity_rad_s == pytest.approx(angular_velocity, abs=1e-6) + + def test_axle_velocities_rate_defaults_to_zero(self): + model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5) + no_arg = model.articulation_to_axle_velocities(2.0, 0.3) + explicit_zero = model.articulation_to_axle_velocities(2.0, 0.3, 0.0) + + assert no_arg.front_axle_turning_velocity_rad_s == pytest.approx( + explicit_zero.front_axle_turning_velocity_rad_s + ) + assert no_arg.rear_axle_turning_velocity_rad_s == pytest.approx(explicit_zero.rear_axle_turning_velocity_rad_s) + + def test_axle_velocities_nonzero_rate_changes_rear(self): + model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5) + gamma_dot = 0.25 + with_rate = model.articulation_to_axle_velocities(2.0, 0.3, gamma_dot) + # rear-axle turning velocity = front-axle turning velocity - gamma_dot, exactly. + assert ( + with_rate.front_axle_turning_velocity_rad_s - with_rate.rear_axle_turning_velocity_rad_s + == pytest.approx(gamma_dot) + ) + + +class TestBicycleProjector: + def _make(self): + return BicycleProjector( + model=BicycleModel(2.5, 1.5, 0.3), + min_steering_angle_rad=-0.6, + max_steering_angle_rad=0.6, + ) + + def test_construction(self): + projector = self._make() + assert projector.min_steering_angle_rad == pytest.approx(-0.6) + assert projector.max_steering_angle_rad == pytest.approx(0.6) + assert projector.model.wheelbase == pytest.approx(2.5) + + def test_zero_rate_freezes_angle(self): + projector = self._make() + result = projector.step( + dt_s=0.1, + current_pose=Pose2D(), + current_steering_angle_rad=0.2, + target_steering_angle_rad=0.5, + steering_rate_rad_s=0.0, + linear_velocity_m_s=1.0, + ) + assert result.steering_angle_rad == pytest.approx(0.2) + + def test_clamping_saturates_at_max(self): + projector = self._make() + result = projector.step(0.1, Pose2D(), 0.0, 5.0, 100.0, 1.0) + assert result.steering_angle_rad == pytest.approx(0.6) + + def test_rate_limited_ramp(self): + projector = self._make() + result = projector.step(0.1, Pose2D(), 0.0, 0.5, 0.5, 1.0) + assert result.steering_angle_rad == pytest.approx(0.05) + + def test_project_straight_line(self): + projector = self._make() + traj = projector.project(1.0, 0.1, Pose2D(), 0.0, 0.0, 1.0, 2.0) + assert len(traj) == 11 + assert traj[0].time_s == pytest.approx(0.0) + assert traj[-1].time_s == pytest.approx(1.0) + assert traj[-1].pose.x == pytest.approx(2.0) + assert traj[-1].pose.y == pytest.approx(0.0) + assert traj[-1].pose.theta == pytest.approx(0.0) + + def test_project_ramps_to_target(self): + projector = self._make() + # target=0.5, rate=0.5, dt=0.1 → step adds 0.05; reaches 0.5 at step 10. + traj = projector.project(2.0, 0.1, Pose2D(), 0.0, 0.5, 0.5, 0.0) + assert traj[10].steering_angle_rad == pytest.approx(0.5) + assert traj[-1].steering_angle_rad == pytest.approx(0.5) + + +class TestArticulatedProjector: + def _make(self): + return ArticulatedProjector( + model=ArticulatedModel(1.66, 1.44, 2.0, 2.0, 0.723, 0.723), + min_articulation_angle_rad=-0.785, + max_articulation_angle_rad=0.785, + ) + + def test_construction(self): + projector = self._make() + assert projector.min_articulation_angle_rad == pytest.approx(-0.785) + assert projector.max_articulation_angle_rad == pytest.approx(0.785) + assert projector.model.articulation_to_front_axle == pytest.approx(1.66) + + def test_zero_rate_freezes_angle(self): + projector = self._make() + result = projector.step(0.1, Pose2D(), 0.3, 0.6, 0.0, 1.0) + assert result.articulation_angle_rad == pytest.approx(0.3) + + def test_clamping_saturates_at_max(self): + projector = self._make() + result = projector.step(0.1, Pose2D(), 0.0, 2.0, 100.0, 1.0) + assert result.articulation_angle_rad == pytest.approx(0.785) + + def test_rate_limited_ramp(self): + projector = self._make() + result = projector.step(0.1, Pose2D(), 0.0, 0.5, 0.2, 1.0) + assert result.articulation_angle_rad == pytest.approx(0.02) + + def test_project_stueve_max_articulation(self): + projector = self._make() + # target=0.785, rate=0.2 rad/s, dt=0.1 → 0.02 per step; reaches max at step 40 (~3.925s ceil to 4.0s). + traj = projector.project(5.0, 0.1, Pose2D(), 0.0, 0.785, 0.2, 1.0) + assert len(traj) == 51 + assert traj[40].articulation_angle_rad == pytest.approx(0.785) + assert traj[-1].articulation_angle_rad == pytest.approx(0.785) + + def test_project_initial_state_anchored(self): + projector = self._make() + initial = Pose2D(x=1.0, y=2.0, theta=0.5) + traj = projector.project(0.5, 0.1, initial, 0.2, 0.6, 0.5, 1.0) + assert traj[0].time_s == pytest.approx(0.0) + assert traj[0].pose.x == pytest.approx(1.0) + assert traj[0].pose.y == pytest.approx(2.0) + assert traj[0].pose.theta == pytest.approx(0.5) + assert traj[0].articulation_angle_rad == pytest.approx(0.2) + + +class TestDifferentialDriveProjector: + def _make(self): + return DifferentialDriveProjector( + model=DifferentialDriveModel(0.1, 0.5), + min_linear_velocity_m_s=-2.0, + max_linear_velocity_m_s=2.0, + min_angular_velocity_rad_s=-3.0, + max_angular_velocity_rad_s=3.0, + ) + + def test_construction(self): + projector = self._make() + assert projector.min_linear_velocity_m_s == pytest.approx(-2.0) + assert projector.max_linear_velocity_m_s == pytest.approx(2.0) + assert projector.min_angular_velocity_rad_s == pytest.approx(-3.0) + assert projector.max_angular_velocity_rad_s == pytest.approx(3.0) + assert projector.model.wheel_radius == pytest.approx(0.1) + + def test_zero_accel_freezes_velocities(self): + projector = self._make() + result = projector.step( + dt_s=0.1, + current_pose=Pose2D(), + current_linear_velocity_m_s=0.5, + current_angular_velocity_rad_s=0.2, + target_linear_velocity_m_s=1.5, + target_angular_velocity_rad_s=1.0, + linear_acceleration_m_s2=0.0, + angular_acceleration_rad_s2=0.0, + ) + assert result.linear_velocity_m_s == pytest.approx(0.5) + assert result.angular_velocity_rad_s == pytest.approx(0.2) + + def test_clamping_saturates_at_max(self): + projector = self._make() + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 100.0, 100.0, 1000.0, 1000.0) + assert result.linear_velocity_m_s == pytest.approx(2.0) + assert result.angular_velocity_rad_s == pytest.approx(3.0) + + def test_rate_limited_ramp(self): + projector = self._make() + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 1.0, 0.5, 0.5, 0.3) + assert result.linear_velocity_m_s == pytest.approx(0.05) + assert result.angular_velocity_rad_s == pytest.approx(0.03) + + def test_project_straight_line(self): + projector = self._make() + # Constant linear=1.0, zero angular, large accels. + traj = projector.project(1.0, 0.1, Pose2D(), 1.0, 0.0, 1.0, 0.0, 100.0, 100.0) + assert len(traj) == 11 + assert traj[-1].pose.x == pytest.approx(1.0) + assert traj[-1].pose.y == pytest.approx(0.0) + assert traj[-1].pose.theta == pytest.approx(0.0) + + +class TestPoint2D: + def test_construction(self): + from polymath_kinematics import Point2D + + point = Point2D(1.5, -2.0) + assert point.x == pytest.approx(1.5) + assert point.y == pytest.approx(-2.0) + + +class TestProjectorFootprints: + def test_bicycle_no_footprint_is_empty(self): + projector = BicycleProjector(BicycleModel(2.5, 1.5, 0.3), -0.6, 0.6) + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 1.0) + assert list(result.footprint) == [] + + def test_bicycle_footprint_exposed(self): + # Binding-surface check only: exact geometry is covered by the C++ projector tests. + projector = BicycleProjector( + BicycleModel(2.5, 1.5, 0.3), -0.6, 0.6, footprint=rectangle_footprint(3.0, 1.0, 2.0) + ) + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 0.0) + assert len(result.footprint) == 4 + assert hasattr(result.footprint[0], 'x') and hasattr(result.footprint[0], 'y') + + def test_bicycle_arbitrary_polygon_vertex_count_is_preserved(self): + body = [Point2D(-1.0, -0.9), Point2D(2.0, -0.9), Point2D(2.8, 0.1), Point2D(2.0, 0.9), Point2D(-1.0, 0.9)] + projector = BicycleProjector(BicycleModel(2.5, 1.5, 0.3), -0.6, 0.6, footprint=body) + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 0.0) + assert len(result.footprint) == 5 + assert result.footprint[2].x == pytest.approx(2.8) + assert result.footprint[2].y == pytest.approx(0.1) + + def test_differential_footprint_exposed(self): + projector = DifferentialDriveProjector( + DifferentialDriveModel(0.1, 0.5), -2.0, 2.0, -3.0, 3.0, footprint=rectangle_footprint(1.5, 0.5, 1.0) + ) + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 0.0, 100.0, 100.0) + assert len(result.footprint) == 4 + + def test_articulated_no_footprint_is_empty(self): + projector = ArticulatedProjector(ArticulatedModel(1.66, 1.44, 2.0, 2.0, 0.723, 0.723), -0.785, 0.785) + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 1.0) + assert list(result.front_footprint) == [] + assert list(result.rear_footprint) == [] + + def test_articulated_footprint_exposed(self): + # Binding-surface check only: exact geometry is covered by the C++ projector tests. + projector = ArticulatedProjector( + ArticulatedModel(1.66, 1.44, 2.0, 2.0, 0.723, 0.723), + -0.785, + 0.785, + front_footprint=rectangle_footprint(2.2, 0.4, 2.0), + rear_footprint=rectangle_footprint(0.3, 2.0, 2.0), + ) + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 0.0) + assert len(result.front_footprint) == 4 + assert len(result.rear_footprint) == 4 + assert hasattr(result.front_footprint[0], 'x') and hasattr(result.front_footprint[0], 'y') + + def test_articulated_reports_joint_pose(self): + articulation_to_rear = 1.44 + projector = ArticulatedProjector( + ArticulatedModel(1.66, articulation_to_rear, 2.0, 2.0, 0.723, 0.723), -0.785, 0.785 + ) + # REAR reference (the default): the rear axle is the pose, so the joint leads it by L_r. + result = projector.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 0.0) + assert result.joint_pose.x == pytest.approx(articulation_to_rear) + assert result.joint_pose.y == pytest.approx(0.0) + + +class TestAxleReference: + def test_enum_values_exposed(self): + assert AxleReference.FRONT != AxleReference.REAR + + def test_bicycle_default_is_rear(self): + projector = BicycleProjector(BicycleModel(2.5, 1.5, 0.3), -0.6, 0.6) + assert projector.axle_reference == AxleReference.REAR + + def test_bicycle_front_reference_leads_by_a_wheelbase(self): + wheelbase = 2.5 + model = BicycleModel(wheelbase, 1.5, 0.3) + rear = BicycleProjector(model, -0.6, 0.6, axle_reference=AxleReference.REAR) + front = BicycleProjector(model, -0.6, 0.6, axle_reference=AxleReference.FRONT) + # Straight ahead: the front-axle pose stays a wheelbase ahead of the rear-axle pose. + rear_state = rear.step(0.1, Pose2D(), 0.0, 0.0, 0.0, 1.0) + front_state = front.step(0.1, Pose2D(x=wheelbase), 0.0, 0.0, 0.0, 1.0) + assert front_state.pose.x == pytest.approx(rear_state.pose.x + wheelbase) + assert front_state.pose.y == pytest.approx(rear_state.pose.y) + + def test_articulated_front_reference_uses_the_front_body_heading(self): + articulation_to_front, articulation_to_rear = 1.66, 1.44 + gamma = 0.3 + projector = ArticulatedProjector( + ArticulatedModel(articulation_to_front, articulation_to_rear, 2.0, 2.0, 0.723, 0.723), + -0.785, + 0.785, + axle_reference=AxleReference.FRONT, + ) + # Front-axle pose corresponding to a rear axle at the origin heading +x, held at gamma. + start = Pose2D( + x=articulation_to_rear + articulation_to_front * math.cos(gamma), + y=articulation_to_front * math.sin(gamma), + theta=gamma, + ) + result = projector.step(0.1, start, gamma, gamma, 0.0, 0.0) + assert result.pose.theta == pytest.approx(gamma) + assert result.joint_pose.x == pytest.approx(articulation_to_rear) + assert result.joint_pose.theta == pytest.approx(0.0) + + +class TestRectangleFootprint: + def test_corner_order_and_extents(self): + corners = rectangle_footprint(3.0, 1.0, 2.0) + assert len(corners) == 4 + # CCW, open: rear-right, front-right, front-left, rear-left. + assert (corners[0].x, corners[0].y) == pytest.approx((-1.0, -1.0)) + assert (corners[1].x, corners[1].y) == pytest.approx((3.0, -1.0)) + assert (corners[2].x, corners[2].y) == pytest.approx((3.0, 1.0)) + assert (corners[3].x, corners[3].y) == pytest.approx((-1.0, 1.0)) + + def test_non_positive_width_is_empty(self): + assert list(rectangle_footprint(3.0, 1.0, 0.0)) == [] + + def test_transform_footprint_rotates_and_translates(self): + corners = transform_footprint([Point2D(1.0, 0.0)], Pose2D(x=2.0, y=3.0, theta=math.pi / 2)) + assert corners[0].x == pytest.approx(2.0) + assert corners[0].y == pytest.approx(4.0)