Line data    Source code

       1             : #ifndef CONTROLS_CONTROLLER_FEEDFORWARD_CONTROLLER_H
       2             : #define CONTROLS_CONTROLLER_FEEDFORWARD_CONTROLLER_H
       3             : 
       4             : #include <controller/BaseControllerInterface.h>
       5             : 
       6             : /**
       7             :  * @brief Controller that computes open-loop forces via inverse dynamics
       8             :  *
       9             :  * Uses the vehicle dynamics model to evaluate M·a + C(v)·v + D(v)·v,
      10             :  * producing a force that cancels inertial, Coriolis, and damping effects
      11             :  * for the desired trajectory state.  Returns zero near the surface when
      12             :  * surface gain scheduling is enabled.
      13             :  */
      14           1 : class FeedforwardController : public BaseControllerInterface {
      15             :     static constexpr double EXPLODE_THRESHOLD = 1e4;
      16             : 
      17             : public:
      18           0 :     using BaseControllerInterface::BaseControllerInterface;
      19             : 
      20             :     /**
      21             :      * @brief Computes the feed-forward force from the goal state's acceleration and velocity
      22             :      *
      23             :      * @param goal [in] Desired vehicle state (acceleration and twist are used)
      24             :      * @param curr [in] Current vehicle state (used only for surface-depth check)
      25             :      * @param dt   [in] Time step in seconds (unused by this controller)
      26             :      *
      27             :      * @return 6-element inverse-dynamics force vector, or zero if the result explodes
      28             :      */
      29           1 :     Vector6d getForce(const State& goal, const State& curr, double dt) override;
      30             : };
      31             : 
      32             : #endif  // CONTROLS_CONTROLLER_FEEDFORWARD_CONTROLLER_H