|
| CTBRParams | make_ctbr_params (X2Params x2, CTBRConfig cfg=CTBRConfig()) |
| |
| tuple[jnp.ndarray, jnp.ndarray] | normalized_to_command (jnp.ndarray action, CTBRParams p) |
| |
| jnp.ndarray | command_to_normalized (jnp.ndarray thrust, jnp.ndarray rates, CTBRParams p) |
| |
| jnp.ndarray | rate_control_torque (jnp.ndarray rate_des, jnp.ndarray rate_meas, CTBRParams p) |
| |
| tuple[jnp.ndarray, MixInfo] | mix (jnp.ndarray thrust, jnp.ndarray torque, CTBRParams p) |
| |
| tuple[jnp.ndarray, MixInfo] | ctbr_motor_thrusts (jnp.ndarray action, jnp.ndarray rate_meas, CTBRParams p) |
| |
Normalized collective-thrust-and-body-rate (CTBR) action interface for the X2.
Pipeline (pure ``jax.numpy``; jit/vmap-able), applied at every physics substep:
1. **Normalized command** ``a = [a_T, a_p, a_q, a_r] in [-1, 1]^4`` (clipped).
Collective thrust moves above/below hover using the available margins::
T_cmd = T_hover + a_T * (T_max - T_hover) if a_T >= 0
T_cmd = T_hover + a_T * (T_hover - T_min) if a_T < 0
so ``a_T = -1, 0, +1`` map to ``T_min, T_hover, T_max``. Body-rate command
``w_des = [a_p, a_q, a_r] * rate_max`` (body frame, rad/s).
2. **Rate loop** (P + gyroscopic feedforward, always on)::
tau = J @ (K * (w_des - w_meas)) + w_meas x (J @ w_meas)
With exact inertia this gives decoupled first-order rate dynamics
``dw/dt = K (w_des - w)`` (time constant ``1/K``), i.e. ``K`` is the rate-loop
bandwidth in rad/s. ``J`` is the full inertia about the COM in body axes from
the compiled MJCF (it has a non-zero ``I_xz``).
3. **Allocation** ``f = A^{-1} [T, tau_x, tau_y, tau_z]`` with the X-configuration
matrix ``A`` built from the MJCF sites and gears (:mod:`rl_stl.envs.skydio_x2.model`).
4. **Prioritized desaturation** into ``[motor_min, motor_max]``, priority
roll/pitch > collective thrust > yaw:
a. roll/pitch torques are scaled down only if their motor spread alone exceeds
the motor range (physically unavoidable);
b. collective thrust is shifted (per-motor base clipped) so that the roll/pitch
differential fits;
c. yaw torque is scaled into whatever headroom remains, without moving thrust;
d. a final clip guards against round-off.
The realized wrench ``A @ f`` and the scale/shift factors are returned for logging.