Tmp/reference filter quat (#688)

* update waypoint utils to quat

* refernce filter quat implementation

* RPY reference testing publisher

* sim test for quat ref filter

* update pkg maintainer :)

* add reference quat sim test to workflow

* helpers for step function

* fix readme

* ref_quat remove waypoint_utils and ref feedback

* remove RPY debug publisher

* pre-commit

Author: jorgenfj

.github/workflows/simulator-test.yml

@@ -19,4 +19,5 @@ jobs:
         "tests/simulator_tests/waypoint_navigation/simulator_test.sh",
         "tests/simulator_tests/los_test/simulator_test.sh",
         "tests/simulator_tests/waypoint_manager_test/simulator_test.sh",
+        "tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh",
         ]'

guidance/reference_filter_dp_quat/CMakeLists.txt

@@ -28,7 +28,6 @@ set(CORE_LIB_NAME "${PROJECT_NAME}")
 
 add_library(${CORE_LIB_NAME} SHARED
   src/lib/reference_filter.cpp
-  src/lib/waypoint_utils.cpp
   src/lib/waypoint_follower.cpp
 )
 

guidance/reference_filter_dp_quat/README.md

@@ -0,0 +1,106 @@
+## Reference filter (quaternion)
+
+### Third-order reference filter
+
+The underlying filter is the third-order model (Fossen, 2021):
+```math
+\dot{x}_d = A_d x_d + B_d r
+```
+where
+```math
+A_d = \begin{bmatrix}
+0_{n\times n} & I_n & 0_{n\times n} \\
+0_{n\times n} & 0_{n \times n} & I_n \\
+-\Omega^3 & -(2 \Delta + I_n) \Omega^2 & -(2 \Delta + I_n) \Omega
+\end{bmatrix}
+```
+and
+```math
+B_d = \begin{bmatrix}
+0_{n \times n} \\
+0_{n \times n} \\
+\Omega^3
+\end{bmatrix}.
+```
+
+The state is integrated using forward Euler: $x_{i+1} = x_i + \dot{x}_i \cdot dt$.
+
+### Error-state formulation
+
+This package avoids the singularity issues of Euler angles by using a quaternion-based error state.
+
+**Nominal pose** — a `Pose` (position + quaternion) representing the current best estimate of the reference trajectory. This is the actual output.
+
+**Error state** — the 18D filter state, where the first 6 elements are small errors relative to the nominal:
+```math
+x = \begin{bmatrix} \delta p \\ \delta \phi \\ \dot{\eta} \\ \ddot{\eta} \end{bmatrix} \in \mathbb{R}^{18}
+```
+
+| Indices | Meaning |
+|---|---|
+| `x[0:3]` | Position error $\delta p$ from nominal (meters) |
+| `x[3:6]` | Orientation error $\delta \phi$ from nominal (rotation vector, radians) |
+| `x[6:9]` | World-frame linear velocity (m/s) |
+| `x[9:12]` | World-frame angular velocity (rad/s) |
+| `x[12:18]` | Accelerations (linear + angular) |
+
+### Step cycle
+
+Each time step performs three operations:
+
+1. **Compute reference error** — The 6D reference $r$ is the error between the waypoint goal and the nominal pose:
+   - $r_{0:3} = p_{goal} - p_{nominal}$
+   - $r_{3:6} = q_{\mathrm{err}}(q_{nominal}, q_{goal})$
+
+   The `quaternion_error` function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal.
+
+2. **Integrate** — The standard filter step:
+   ```math
+   \dot{x} = A_d x + B_d r, \quad x \leftarrow x + \dot{x} \cdot dt
+   ```
+
+3. **Reset** — Absorb position and orientation errors into the nominal pose, then zero them:
+   - $p_{nominal} \leftarrow p_{nominal} + \delta p, \quad \delta p \leftarrow 0$
+   - $q_{nominal} \leftarrow q_{nominal} \otimes \exp(\delta \phi), \quad \delta \phi \leftarrow 0$
+
+   where $\exp(\delta \phi)$ converts the rotation vector to a quaternion via `AngleAxis`.
+
+The reset step keeps $\delta p$ and $\delta \phi$ near zero at all times, ensuring the linearized quaternion error remains accurate. The velocity and acceleration states ($x_{6:18}$) are **not** reset — they carry over to provide smooth, continuous motion.
+
+### Output message (`ReferenceFilterQuat`)
+
+| Field | Source | Meaning |
+|---|---|---|
+| `x`, `y`, `z` | Nominal pose position | Desired position in world frame (m) |
+| `qw`, `qx`, `qy`, `qz` | Nominal pose quaternion | Desired orientation (unit quaternion) |
+| `x_dot`, `y_dot`, `z_dot` | `x[6:9]` | World-frame linear velocity (m/s) |
+| `roll_dot`, `pitch_dot`, `yaw_dot` | `x[9:12]` | World-frame angular velocity (rad/s) |
+
+
+
+
+### Waypoint modes
+
+Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured.
+
+| Mode | Controlled DOFs | Convergence metric |
+|---|---|---|
+| `FULL_POSE` | All 6 DOF (position + orientation) | Position error norm + quaternion error norm |
+| `ONLY_POSITION` | x, y, z (orientation holds current value) | Position error norm |
+| `FORWARD_HEADING` | x, y, z + yaw toward target | Position error norm + yaw component of quaternion error |
+| `ONLY_ORIENTATION` | Orientation only (position holds current value) | Quaternion error norm |
+
+For all modes, convergence is reached when the error metric drops below the `convergence_threshold` specified in the action goal.
+
+### Action Server
+
+The action server handles goal requests and publishes guidance commands. The server always prioritizes new goal requests, aborting ongoing requests when a new one arrives.
+
+- Action name: `/reference_filter`
+- Goal type: Waypoint (pose + mode + convergence threshold)
+- Result type: bool
+- Guidance topic: `/dp/reference`
+
+### Overwriting the reference during an action
+
+While an action is executing, the reference goal can be updated by publishing a `geometry_msgs/msg/PoseStamped` to the reference pose topic. The convergence check will use the updated reference.

guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp

@@ -3,18 +3,25 @@
 
 #include <mutex>
 #include <vortex/utils/types.hpp>
+#include <vortex/utils/waypoint_utils.hpp>
 #include "reference_filter_dp_quat/lib/eigen_typedefs.hpp"
 #include "reference_filter_dp_quat/lib/reference_filter.hpp"
-#include "reference_filter_dp_quat/lib/waypoint_types.hpp"
 
 namespace vortex::guidance {
 
-using vortex::utils::types::PoseEuler;
+using vortex::utils::types::Pose;
 using vortex::utils::types::Twist;
+using vortex::utils::types::Waypoint;
+using vortex::utils::types::WaypointMode;
 
 /**
  * @brief Manages reference filter state and waypoint following logic.
  *
+ * Uses an error-state formulation: the 18D state vector holds
+ * [δp(3), δφ(3), velocity(6), acceleration(6)], while a separate
+ * nominal Pose tracks the full quaternion orientation. Each step,
+ * position/orientation errors are absorbed into the nominal and reset.
+ *
  * Thread-safe: all public methods acquire a mutex.
  */
 class WaypointFollower {
@@ -28,52 +35,74 @@ class WaypointFollower {
      * @param waypoint Target waypoint with mode.
      * @param convergence_threshold Max error norm to consider target reached.
      */
-    void start(const PoseEuler& pose,
+    void start(const Pose& pose,
                const Twist& twist,
                const Waypoint& waypoint,
                double convergence_threshold);
 
     /**
      * @brief Advance the filter by one time step.
-     * @return  The updated filter state.
+     *
+     * Computes the error-state reference, integrates, then absorbs
+     * position/orientation errors into the nominal pose.
      */
-    Eigen::Vector18d step();
+    void step();
 
     /**
-     * @brief Check if the measured pose has converged to the reference goal.
+     * @brief Check if the measured pose has converged to the waypoint goal.
      * @param measured_pose Current measured pose.
      * @return True if the error norm is within the convergence threshold.
      */
-    bool within_convergance(const Eigen::Vector6d& measured_pose) const;
+    bool within_convergance(const Pose& measured_pose) const;
 
     /**
      * @brief Update the reference goal pose mid-sequence.
      * @param reference_goal_pose The new reference pose.
      */
-    void set_reference(const PoseEuler& reference_goal_pose);
+    void set_reference(const Pose& reference_goal_pose);
 
     /**
-     * @brief Snap the position component of the filter state to the reference.
+     * @brief Snap the nominal pose to the waypoint goal and zero
+     * errors/velocity.
      *
-     * Useful after convergence to eliminate any remaining steady-state offset
+     * Useful after convergence to eliminate any remaining steady-state offset.
      */
     void snap_state_to_reference();
 
-    /// @brief Get the current 18D filter state.
-    Eigen::Vector18d state() const;
+    /**
+     * @brief Get the current nominal pose (position + quaternion).
+     */
+    Pose pose() const;
+
+    /**
+     * @brief Get the current world-frame velocity (linear + angular).
+     */
+    Eigen::Vector6d velocity() const;
 
-    /// @brief Get the current 6D reference goal pose.
-    Eigen::Vector6d reference() const;
+    /**
+     * @brief Get the current waypoint goal.
+     */
+    Pose waypoint_goal() const;
 
    private:
-    Eigen::Vector18d compute_initial_state(const PoseEuler& pose,
-                                           const Twist& twist);
+    /**
+     * @brief Compute error-state reference based on the current nominal pose
+     * and waypoint goal.
+     */
+    Eigen::Vector6d update_reference() const;
+
+    /**
+     * @brief Absorb position/orientation errors into the nominal pose and
+     * reset the error states.
+     */
+    void inject_and_reset();
 
     mutable std::mutex mutex_;
     ReferenceFilter filter_;
     double dt_seconds_{0.01};
+    Pose nominal_pose_;
     Eigen::Vector18d state_ = Eigen::Vector18d::Zero();
-    Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero();
+    Pose waypoint_goal_;
     WaypointMode waypoint_mode_{WaypointMode::FULL_POSE};
     double convergence_threshold_{0.1};
 };

guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp

@@ -9,8 +9,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <vortex/utils/types.hpp>
-#include <vortex_msgs/action/reference_filter_waypoint.hpp>
-#include <vortex_msgs/msg/reference_filter.hpp>
+#include <vortex_msgs/action/reference_filter_quat_waypoint.hpp>
+#include <vortex_msgs/msg/reference_filter_quat.hpp>
 #include <vortex_msgs/msg/waypoint.hpp>
 #include "reference_filter_dp_quat/lib/waypoint_follower.hpp"
 
@@ -37,17 +37,17 @@ class ReferenceFilterNode : public rclcpp::Node {
     rclcpp_action::GoalResponse handle_goal(
         const rclcpp_action::GoalUUID& uuid,
         std::shared_ptr<
-            const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal);
+            const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> goal);
 
     /// @brief Accept all cancel requests.
     rclcpp_action::CancelResponse handle_cancel(
         const std::shared_ptr<rclcpp_action::ServerGoalHandle<
-            vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle);
+            vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle);
 
     /// @brief Join the old execution thread and spawn a new one for the goal.
     void handle_accepted(
         const std::shared_ptr<rclcpp_action::ServerGoalHandle<
-            vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle);
+            vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle);
 
     /**
      * @brief Execute the action goal in a loop until convergence or
@@ -56,16 +56,16 @@ class ReferenceFilterNode : public rclcpp::Node {
      */
     void execute(
         const std::shared_ptr<rclcpp_action::ServerGoalHandle<
-            vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle);
+            vortex_msgs::action::ReferenceFilterQuatWaypoint>> goal_handle);
 
-    rclcpp_action::Server<
-        vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_;
+    rclcpp_action::Server<vortex_msgs::action::ReferenceFilterQuatWaypoint>::
+        SharedPtr action_server_;
 
     ReferenceFilterParams filter_params_;
 
     std::unique_ptr<WaypointFollower> follower_;
 
-    rclcpp::Publisher<vortex_msgs::msg::ReferenceFilter>::SharedPtr
+    rclcpp::Publisher<vortex_msgs::msg::ReferenceFilterQuat>::SharedPtr
         reference_pub_;
 
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
@@ -77,11 +77,9 @@ class ReferenceFilterNode : public rclcpp::Node {
     rclcpp::Subscription<
         geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
 
-    rclcpp::TimerBase::SharedPtr reference_pub_timer_;
-
     std::chrono::milliseconds time_step_{};
 
-    vortex::utils::types::PoseEuler current_pose_;
+    vortex::utils::types::Pose current_pose_;
 
     vortex::utils::types::Twist current_twist_;