Control API

attitude_error_vector

attitude_error_vector(R: ndarray, R_target: ndarray) -> jnp.ndarray

Compute the intrinsic attitude error used by geometric controllers.

For the current body-to-spatial attitude \(R\) and target attitude \(R_d\),

\[ \mathbf{e}_R = \frac{1}{2} \left(R_d^T R - R^T R_d\right)^\vee. \]

Parameters:

Name	Type	Description	Default
R	(Array, shape(3, 3))	Current body-to-spatial attitude.	required
R_target	(Array, shape(3, 3))	Desired body-to-spatial attitude.	required

Returns:

Type	Description
(Array, shape(3))	Attitude error expressed in the current body frame.

Notes

The error vanishes at both relative angles \(0\) and \(\pi\). The 180-degree attitudes are unstable critical points of the corresponding geometric controller, so this vector is intended for almost-global rather than global attitude stabilization.

geometric_pd_torque

geometric_pd_torque(R: ndarray, pi: ndarray, J: ndarray, R_target: ndarray, attitude_gain: float, rate_gain: float) -> jnp.ndarray

Compute body torque for fixed-target geometric PD attitude control.

Let \(\boldsymbol{\omega}=J^{-1}\boldsymbol{\pi}\). For a stationary target attitude, the control law is

\[ \boldsymbol{\tau} = -k_R\mathbf{e}_R -k_\omega\boldsymbol{\omega} +\boldsymbol{\omega}\times\boldsymbol{\pi}. \]

The final term cancels the gyroscopic term in Euler's rigid-body equation,

\[ J\dot{\boldsymbol{\omega}} +\boldsymbol{\omega}\times J\boldsymbol{\omega} = \boldsymbol{\tau}. \]

Parameters:

Name	Type	Description	Default
R	(Array, shape(3, 3))	Current body-to-spatial attitude.	required
pi	(Array, shape(3))	Current body-frame angular momentum.	required
J	(Array, shape(3, 3))	Symmetric positive-definite body inertia tensor.	required
R_target	(Array, shape(3, 3))	Desired fixed body-to-spatial attitude.	required
attitude_gain	float or Array	Positive proportional gain \(k_R\).	required
rate_gain	float or Array	Positive angular-rate gain \(k_\omega\).	required

Returns:

Type	Description
(Array, shape(3))	Commanded body-frame torque.

geometric_tracking_torque

geometric_tracking_torque(R: ndarray, pi: ndarray, J: ndarray, R_target: ndarray, target_angular_velocity: ndarray, target_angular_acceleration: ndarray, attitude_gain: float, rate_gain: float) -> jnp.ndarray

Compute body torque for geometric attitude trajectory tracking.

The desired attitude satisfies

\[ \dot R_d = R_d\widehat{\boldsymbol{\omega}}_d, \]

where both \(\boldsymbol{\omega}_d\) and \(\dot{\boldsymbol{\omega}}_d\) are expressed in the desired body frame. The desired angular velocity is transported into the current body frame before forming the rate error,

\[ \mathbf{e}_\omega = \boldsymbol{\omega} - R^T R_d\boldsymbol{\omega}_d. \]

Together with the intrinsic attitude error \(\mathbf{e}_R\), the commanded body torque is

\[ \boldsymbol{\tau} = -k_R\mathbf{e}_R - k_\omega\mathbf{e}_\omega + \boldsymbol{\omega}\times J\boldsymbol{\omega} - J\left( \widehat{\boldsymbol{\omega}}R^T R_d \boldsymbol{\omega}_d - R^T R_d\dot{\boldsymbol{\omega}}_d \right). \]

Parameters:

Name	Type	Description	Default
R	(Array, shape(3, 3))	Current body-to-spatial attitude.	required
pi	(Array, shape(3))	Current body-frame angular momentum.	required
J	(Array, shape(3, 3))	Symmetric positive-definite body inertia tensor.	required
R_target	(Array, shape(3, 3))	Desired body-to-spatial attitude at the current time.	required
target_angular_velocity	(Array, shape(3))	Desired angular velocity expressed in the desired body frame.	required
target_angular_acceleration	(Array, shape(3))	Time derivative of the desired body-frame angular velocity.	required
attitude_gain	float or Array	Positive attitude gain \(k_R\).	required
rate_gain	float or Array	Positive angular-rate gain \(k_\omega\).	required

Returns:

Type	Description
(Array, shape(3))	Commanded body-frame torque.

Notes

Setting the desired angular velocity and acceleration to zero recovers :func:geometric_pd_torque. This continuous-time control law is sampled under zero-order hold by :func:simulate_controlled_rigid_body; its continuous-time stability result does not apply to arbitrary timesteps.

References

T. Lee, M. Leok, and N. H. McClamroch, "Geometric Tracking Control of a Quadrotor UAV on SE(3)," 49th IEEE Conference on Decision and Control, 2010. doi:10.1109/CDC.2010.5717652

simulate_controlled_rigid_body

simulate_controlled_rigid_body(R0: ndarray, pi0: ndarray, J: ndarray, torque_fn: Callable, torque_params: Any, dt: float | ndarray, steps: int, newton_iters: int = 8, tolerance: float = 1e-10) -> tuple[jnp.ndarray, jnp.ndarray, jnp.ndarray, SolverInfo]

Simulate state-dependent body torque under zero-order hold.

At the beginning of interval \(k\), the simulator evaluates

\[ \boldsymbol{\tau}_k = f_\tau(t_k,R_k,\boldsymbol{\pi}_k,J,p) \]

and holds that value over the complete interval. One call to rigid_body_step therefore receives the same torque at both endpoints. The controller is evaluated again after the state reaches the next node.

Parameters:

Name	Type	Description	Default
R0	(Array, shape(3, 3))	Initial body-to-spatial attitude.	required
pi0	(Array, shape(3))	Initial body-frame angular momentum.	required
J	(Array, shape(3, 3))	Symmetric positive-definite body inertia tensor.	required
torque_fn	callable	JAX-compatible function with signature torque_fn(t, R, pi, J, torque_params), returning a body-frame vector with shape (3,). The callable is static under JIT.	required
torque_params	PyTree	Parameters passed unchanged to torque_fn.	required
dt	(float or Array, shape(steps))	Positive controller and integration timestep. A scalar applies the same interval to every transition; an array supplies one interval per transition.	required
steps	int	Number of controlled transitions. This value is static under JIT.	required
newton_iters	int	Newton iterations used for every transition. The default is 8.	8
tolerance	float or Array	Residual threshold used to form each convergence flag. The default is 1e-10.	1e-10

Returns:

Name	Type	Description
Rs	(Array, shape(steps + 1, 3, 3))	Attitude history including R0.
pis	(Array, shape(steps + 1, 3))	Body-angular-momentum history including pi0.
body_torques	(Array, shape(steps, 3))	Torque held over each transition.
solver_info	SolverInfo	Residual norms and convergence flags with shape (steps,).

Notes

Zero-order hold models a digital controller whose output changes only at sample times. It is distinct from evaluating a prescribed torque at both nodes, as done by simulate_rigid_body.