pdcontroller#
Actuator-agnostic quaternion PD controller.
-
namespace saltro
-
namespace controller
-
class PDController : public saltro::controller::Controller#
- #include <pdcontroller.h>
Actuator-agnostic quaternion PD controller.
Computes a desired body-frame torque via quaternion or vector PD plus angular-rate damping, then allocates the torque across available actuators by least squares.
Quaternion goal ( \(q_{\text{goal}}\) a unit quaternion):
\[ \boldsymbol{\tau}_{\text{des}} = -k_p\,\mathbf{q}_{\text{err,vec}} - k_d\,\boldsymbol{\omega} \]Vector goal ( \(q_{\text{goal}}=[\mathrm{NaN}, \hat{r}_{\text{eci}}]\), matching the Satellite stageCost sentinel):
\[ \boldsymbol{\tau}_{\text{des}} = +k_p\,\bigl(\mathbf{bs}_{\text{body}} \times R(q)^\top\hat{r}_{\text{eci}}\bigr) - k_d\,\boldsymbol{\omega} \]The cross product has magnitude \(\sin\theta_{\text{err}}\) and points in the body-frame direction that rotates \(\mathbf{bs}\) toward \(R^\top\hat{r}\).Allocation solves
\[ \min_{\mathbf{u}} \;\|J\mathbf{u} - \boldsymbol{\tau}_{\text{des}}\|^2 + \mathbf{u}^\top W \mathbf{u} \]where \(J = \partial\boldsymbol{\tau}_{\text{actuator}}/\partial\mathbf{u}\) is the numerical Jacobian of Satellite::actuatorTorque, and \(W\) weights actuators by their inverse-squared authority (column norm of \(J\)). This dispatches through the satellite’s actuator mixing and is therefore agnostic to actuator topology — any MTQ/RW combination, or future actuator types.The result is clamped to actuator limits by uniform scale-to-max: if any channel would exceed its limit, all channels scale by the same factor so the torque direction is preserved. Independent per-channel clipping distorts the direction and can send \(\boldsymbol{\tau}_{\text{actual}}\) further from \(\boldsymbol{\tau}_{\text{des}}\) than no allocation at all.
Public Functions
-
explicit PDController(const Satellite &satellite)#
Construct a PD controller with auto-tuned gains.
- Parameters:
satellite – Satellite model (inertia, actuators)
-
virtual Satellite::VecX find_u(const Satellite::VecX &x, const Eigen::Vector3d &B_eci, const Eigen::Vector4d &q_goal, const Eigen::Vector3d &boresight_body) const override#
Compute control input via quaternion PD + least-squares allocation.
- Parameters:
x – Attitude state (7+nRW): [ω(3), q(4), h_rw(nRW)]
B_eci – Magnetic field in ECI frame (T)
q_goal – Either a unit quaternion target (scalar-first) or
[NaN, r̂_eci]for a vector-pointing target.boresight_body – Boresight direction in body frame. Used by the vector-goal branch; ignored when q_goal is a quaternion.
- Returns:
Control vector (nu × 1): [m_mtq, τ_rw, …]
-
void setGains(double kp_q, double kd_w)#
Override auto-tuned gains (used by spike removal).
-
void setGoalRate(const Eigen::Vector3d &omega_des)#
Set a desired body-frame angular rate to feed forward.
When set finite, find_u damps ω toward this rate (τ_des += -kd·(ω - ω_des)) instead of toward zero, matching OldPlanner smartbdot’s goal-rate feedforward (
wkdes). Pass a non-finite/NaN vector (the default) to disable.
-
inline double kp_q() const#
-
inline double kd_w() const#
-
inline double rwScale() const#
Protected Functions
-
virtual void autoTuneGains() override#
Auto-tune to a conservative second-order response.
\(\omega_n = 0.1\,\text{rad/s}\), \(\zeta = 0.7\), based on the mean inertia tensor eigenvalue.
-
explicit PDController(const Satellite &satellite)#
-
class PDController : public saltro::controller::Controller#
-
namespace controller