Adquiriendo señal…
Adquiriendo señal…
Lección 7 de 12· Control y estimación
0 de 12 lecciones · 0 %
Hasta la lección 6 la montura es un nodo de Python (mount_sim) y el lazo otro (pid_controller o lqr_controller) que se hablan por tópicos. Funciona, y en lockstep es determinista, pero no es como la robótica ROS 2 mueve motores de verdad. La vía estándar es ros2_control: la montura se declara como un hardware con interfaces de estado y de mando, el lazo se escribe como un controlador en C++, y un controller_manager los ejecuta en un ciclo fijo. Con eso, dice docs/ROS2_CONTROL.md, «el mismo controlador mueve una montura simulada, una montura en Gazebo o una real con solo cambiar el plugin de hardware (una línea del URDF)».
Si vienes de Sistemas APT (lección 7, «Más allá del PID: LQR y robustez») ya conoces el margen de fase; si vienes de Señales y Sistemas (lección 12, «Respuesta en frecuencia y diagramas de Bode»), sabes leerlo sobre un diagrama. Esta lección usa ese margen para algo muy concreto: decidir cuánto tiempo pueden comerse el ciclo, el bus serie y el driver antes de que el lazo se vuelva inestable. Las fuentes son src/atp_control (C++), src/atp_description (el xacro) y docs/ROS2_CONTROL.md.
+---------- controller_manager -----------+
/atp/reference -----> | AtpTrackingController (PID | LQR, 20 Hz)| --> /atp/torque
| ^ position | effort |
| +------- hardware (UN plugin) -------+ |
| | AtpMountSystem backend=sim -- dinamica de planta.py en read()
| | backend=serial -- microcontrolador (protocolo ASCII)
| | GazeboSimSystem (gz_ros2_control) -- montura fisica en gz sim
| +------------------------------------+ |
| joint_state_broadcaster --> /joint_states |
+-------------------------------------------+
El controller_manager repite, a la frecuencia update_rate, siempre el mismo ciclo:
Entre el hardware y los controladores no hay tópicos: hay interfaces, variables compartidas en memoria con nombre junta/interfaz. La montura declara las mismas en todos los backends (antena.ros2_control.xacro):
Junta az, el | Interfaz | Unidad | Quién la usa |
|---|---|---|---|
| mando | effort (±8 N·m) | N·m | AtpTrackingController, effort_controllers |
| mando | position | rad | joint_trajectory_controller (servo PD interno) |
| estado | position | rad | todos |
| estado | velocity | rad/s | el broadcaster (el controlador no la usa: la estima) |
| estado | effort | N·m | el mando realmente aplicado |
La etiqueta <ros2_control> del xacro es el único lugar donde se decide qué hay debajo. Recortada:
<!-- src/atp_description/urdf/antena.ros2_control.xacro (recortado) -->
<ros2_control name="AtpMount" type="system">
<hardware>
<xacro:if value="${hw in ('sim', 'serial')}">
<plugin>atp_control/AtpMountSystem</plugin>
<param name="backend">${hw}</param>
<param name="serial_baud">$(arg serial_baud)</param>
</xacro:if>
<xacro:if value="${hw == 'gz'}">
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</xacro:if>
</hardware>
<joint name="az">
<command_interface name="effort">
<param name="min">-8</param> <param name="max">8</param>
</command_interface>
<command_interface name="position"/>
<state_interface name="position"/> <state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- junta el: identica -->
</ros2_control>
Cuatro backends para el mismo controlador: sim integra en read() un port operación por operación de planta.py (mount_plant.cpp, mismo subpaso de 2 ms); serial habla con un microcontrolador por un protocolo ASCII con XOR (o con un MCU falso en proceso, serial_port: fake); gz delega en Gazebo Harmonic; mock es mock_components/GenericSystem, solo para depurar.
AtpMountSystemUn plugin de hardware hereda de hardware_interface::SystemInterface y implementa su ciclo de vida (on_init, on_activate, on_deactivate) y las dos funciones del ciclo. En Jazzy las interfaces declaradas en el URDF se exportan solas y el plugin las lee y escribe con set_state y get_command:
// src/atp_control/src/atp_mount_system.cpp (recortado)
return_type AtpMountSystem::read(const rclcpp::Time &, const rclcpp::Duration & period)
{
std::string why;
if (!backend_->read(period.seconds(), pos_, vel_, why)) {return return_type::ERROR;}
for (size_t i = 0; i < 2; ++i) {
set_state(joint_[i] + "/position", pos_[i]);
set_state(joint_[i] + "/velocity", vel_[i]);
set_state(joint_[i] + "/effort", u_applied_[i]);
}
return return_type::OK;
}
return_type AtpMountSystem::write(const rclcpp::Time &, const rclcpp::Duration & period)
{
Vec2 u{0.0, 0.0};
for (size_t i = 0; i < 2; ++i) {
if (mode_[i] == kEffort) {
const double c = get_command(joint_[i] + "/effort");
u[i] = std::isfinite(c) ? c : 0.0;
} else if (mode_[i] == kPosition) { // servo PD interno
const double c = get_command(joint_[i] + "/position");
if (std::isfinite(c)) {u[i] = servo_kp_ * (c - pos_[i]) - servo_kd_ * vel_[i];}
}
u[i] = std::min(std::max(u[i], -u_max_), u_max_); // el tope vive en el hardware
}
u_applied_ = u;
return backend_->write(u, period.seconds()) ? return_type::OK : return_type::ERROR;
}
Dos detalles de diseño. Sin controlador activo, mode_ es kNone y el par es cero: una montura sin dueño no se mueve. Y el cambio de modo pasa por prepare_command_mode_switch, que rechaza que una misma junta quede a la vez en effort y en position.
AtpTrackingControllerUn controlador hereda de controller_interface::ControllerInterface y declara qué interfaces reclama. El de la Estación reclama el mando effort y lee solo la posición (la velocidad la estima, como el PID y el LQR de Python):
// src/atp_control/src/atp_tracking_controller.cpp (recortado)
InterfaceConfiguration AtpTrackingController::command_interface_configuration() const
{
InterfaceConfiguration c;
c.type = interface_configuration_type::INDIVIDUAL;
for (const auto & j : joints_) {c.names.push_back(j + "/effort");}
return c;
}
return_type AtpTrackingController::update(const rclcpp::Time & time, const rclcpp::Duration &)
{
const RefHistory & h = *ref_.readFromRT(); // /atp/reference sin bloquear
const int64_t now = time.nanoseconds();
if (h.n > 0 && (t_last_ns_ < 0 || now - t_last_ns_ >= dt_ns_ - tol)) {
Vec2 meas{*state_interfaces_[0].get_optional(), *state_interfaces_[1].get_optional()};
t_last_ns_ = now;
const Vec2 ref = reference_at(h, now, extrapolate_); // Lagrange si no hay lockstep
u_ = law_->compute(meas, ref); // PID o LQR (+ Kalman)
}
for (size_t i = 0; i < 2; ++i) {command_interfaces_[i].set_value(u_[i]);} // ZOH
return return_type::OK;
}
La ley corre a 20 Hz aunque el controller_manager vaya más rápido (100 Hz en tiempo real, 1 kHz en Gazebo): entre ticks de la ley se reescribe el mismo par, una retención de orden cero. Las leyes son ports de control.py; el LQR resuelve su Riccati en C++ y da , igual que Python, con (docs/ROS2_CONTROL.md §6). El YAML que los une:
# src/atp_control/config/atp_controllers.yaml (recortado)
controller_manager:
ros__parameters:
update_rate: 20 # Hz del lazo; en Gazebo se sube
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
atp_tracking_controller:
type: atp_control/AtpTrackingController
atp_tracking_controller:
ros__parameters:
joints: [az, el]
law: pid # pid | lqr
estimator: none # none | kalman
rate_hz: 20.0
kp: 25.0
ki: 2.0
kd: 8.0
Los controladores son nodos con ciclo de vida: se cargan (unconfigured), se configuran (inactive: leen parámetros, preparan memoria) y se activan (active: en ese instante reclaman sus interfaces). La regla que hace seguro a ros2_control es la reclamación exclusiva: una interfaz de mando solo puede tenerla un controlador activo. Si atp_tracking_controller ya tiene az/effort y se intenta activar effort_controller (que también la pide), el controller_manager rechaza el cambio. Para pasar de uno a otro hay que desactivar y activar en el mismo cambio:
ros2 control list_controllers
ros2 control list_hardware_interfaces # [claimed] / [unclaimed]
ros2 control switch_controllers --deactivate atp_tracking_controller \
--activate joint_trajectory_controller
lockstep_noderos2_control_node, el ejecutable estándar, mueve el ciclo con el reloj de pared y no espera a nadie. Para mantener el contrato de determinismo de la lección 3, la Estación escribió lockstep_node: el mismo controller_manager, movido por el /clock de sim_clock. En cada :
read(t_k) -> [espera la referencia con sello t_k] -> update -> write
-> [espera /joint_states(t_k)] -> /atp/step_ack(t_k)
Hizo falta rodear tres defectos medidos de ros2_control 4.48 en Jazzy: update() se salta el ciclo sin avisar si otro hilo tiene la lista de controladores (se detecta por el sello del último update y se reintenta); el joint_state_broadcaster descarta muestras si su hilo está ocupado (el nodo espera a ver pasar el estado y repite update(), que es idempotente); y consultar los controladores desde el hilo del lazo se interbloquea con load_controller. Con esas defensas, N2 dos veces con grabador da telemetry.jsonl idénticos byte a byte, 511 filas, en 6 de 6 ejecuciones.
Hay dos relojes que respetar. El primero es el periodo: read, update y write deben caber en . A 20 Hz son 50 ms; a 1 kHz, 1 ms. Si la suma se pasa, el ciclo se atrasa (overrun) y el lazo pierde la regularidad para la que se diseñó: el LQR, diseñado para ms, recibía pasos de 41/59 ms con una rejilla mal alineada y lo pagaba con picos de par de 2 N·m (docs/ROS2_CONTROL.md §1.2).
La latencia del bus serie. Con transmisión asíncrona 8N1, cada byte lleva un bit de arranque, ocho de datos y uno de parada: 10 bits. Una trama de bytes a baudios tarda
La trama de mando del protocolo (tipo C: secuencia, en µs y los dos pares con nueve decimales) ocupa unos 42 bytes; la de estado (tipo S: secuencia, tiempo y cuentas de los encoders) unos 38. A 921 600 baudios: ms de ida y ms de vuelta, 0.87 ms por ciclo. Es el 1.7 % de un ciclo de 50 ms y el 87 % de uno de 1 ms. A 115 200 baudios serían 6.9 ms: no cabe ni a 100 Hz con holgura. Por eso el firmware usa 921 600. El segundo reloj es el margen de retardo: cuánto retardo total (cómputo + bus + driver) aguanta el lazo antes de perder la estabilidad.
Paso 1. Un retardo puro multiplica el lazo abierto por . Sobre el eje imaginario, y : el retardo no cambia la magnitud, solo resta fase, y resta más cuanto mayor es la frecuencia.
Paso 2. La frecuencia de cruce (donde ) no se mueve, porque la magnitud no cambió. En ella el margen de fase pasa de a .
Paso 3. El criterio de Nyquist se viola cuando el margen llega a cero: , es decir
que coincide con los 92.5 ms de docs/CONTROL.md §2 (la diferencia es de redondeo de 52.0° y 9.8 rad/s). Ese margen es del lazo digital, que ya incluye el retardo medio de la retención de orden cero, ms. Lo que queda, 92.5 ms, es el presupuesto para todo lo demás: los 0.87 ms del bus caben de sobra; el Wi-Fi, con jitter de decenas de milisegundos, hay que medirlo.
Activas effort_controller mientras atp_tracking_controller está activo con az/effort y el/effort. ¿Qué ocurre?
En lockstep, con el backend sim, el lazo de ros2_control da las mismas cifras que el lazo de Python (docs/ROS2_CONTROL.md §8.1): N2 con PID n=1018 rms=0.0167 p95=0.0173 max=0.1382; C1 con LQR rms=0.0070 p95=0.0027 max=0.0967. No hay diferencia porque la planta del plugin es idéntica bit a bit, las leyes coinciden a N·m y el mando se retiene 50 ms igual. La única diferencia real aparece con serial: el MCU transmite cuentas enteras de 0.0055° (65 536 por vuelta) y el p95 sube de 0.0173° a 0.0185°. En Gazebo, en tiempo real, el PID da 0.0168° y el LQR 0.0070–0.0072°. La identificación por escalón de par recupera la planta: , con sim y en Gazebo, que es el diseñado (2.0 del cuerpo de elevación + 0.01 del yugo).
ros2 launch atp_control atp_ros2_control.launch.py el_max:=85 mask:=70 time_scale:=0
ros2 launch atp_control atp_ros2_control.launch.py backend:=serial
ros2 launch atp_control atp_ros2_control.launch.py mode:=realtime
ros2 launch atp_control atp_gz.launch.py
El programa calcula la latencia de las dos tramas del protocolo, el margen de retardo a partir del margen de fase, y simula 2000 ciclos con duraciones aleatorias de semilla fija a 20, 100 y 1000 Hz para contar los overruns. El modelo es pesimista a propósito: read espera la trama S completa y write envía la trama C dentro del mismo ciclo.
La salida esperada: 42 y 38 bytes, 0.868 ms por ciclo a 921 600 baudios y 6.944 ms a 115 200; margen de retardo de 92.6 ms; a 20 Hz el ciclo se usa al 2 % sin un solo overrun, a 100 Hz al 10 %, y a 1 kHz al 99.8 % de media con más de 800 overruns de 2000. La conclusión de diseño es la de la Estación: el lazo de 1 kHz no se cierra a través del PC, se cierra en el microcontrolador (lección 11), y el PC manda a 20 Hz.
El laboratorio El ciclo de ros2_control arranca en «sim a 20 Hz»: el diagrama ilumina read, update y write en orden, y la línea de tiempo muestra ciclos que ocupan una fracción mínima del periodo de 50 ms. Elige el escenario «serial a 921600 baudios» y lee la latencia de la serie (bytes × 10 / baudios); sube la frecuencia del ciclo de 20 a 1000 Hz y verás aparecer los overruns en rojo cuando la suma pase de 1 ms. Sube la carga del update y observa cómo el uso del ciclo crece linealmente. Por último, elige «Conflicto: dos controladores reclaman effort» y pulsa activar en el segundo controlador: la lectura de error de reclamación explica el rechazo; desactiva el primero y vuelve a activar para resolverlo.
PREEMPT_RT y prioridad SCHED_FIFO no hay cota del jitter; en el contenedor de la Estación «Could not enable FIFO RT scheduling», así que no hay medida de jitter del lazo.ros2_control puede modelar reductores con transmission_interface; aquí la inercia y el par ya están reflejados en el eje de carga.El controlador ya es intercambiable y el hardware también. Falta el caso que ninguno de los dos resuelve solo: un pase casi cenital en el que la montura no puede girar el acimut tan rápido como pide el satélite. En la lección 8 lo atacamos con el conocimiento del futuro: el control predictivo.
Cierre de la lección
Sigue el recorrido de la lección paso a paso.
Con una cuenta desbloqueas el cuestionario, el foro de dudas y el tutor con IA de esta lección, además del registro de progreso y el certificado verificable al terminar «ROS 2 aplicado: la Estación ATP de punta a punta».