Aspekty implementacyjne i systemowe

TL;DR

Moduły 1-18 omawiają algorytmy. Ten moduł — jak je wdrożyć w realnym systemie robota. Architektura, warstwy czasowe, komunikacja, profilowanie.

Architektura: planner ↔ controller ↔ executor

Standardowa hierarchia warstw, każda na innym tempie:

Każda warstwa wystawia interfejs dla wyższej: planner zwraca trajektorię (sekwencja waypointów + czasów), tracking zwraca q_des(t), joint control zwraca τ_des, motor control → current.

ROS2 stack dla manipulatora

ROS2 (Robot Operating System v2, released 2017) jest de facto standardem akademickim i przemysłowym dla manipulatorów. Stack dla Pandy:

• franka_ros2 — driver (libfranka), publikuje joint states, akceptuje commands.
• MoveIt2 — motion planning (OMPL backend + CHOMP/STOMP), kinematics (KDL/TRAC-IK), collision (FCL).
• ros2_control — joint controllers (position, velocity, effort).
• rviz2 — wizualizacja state'u + planu.
• tf2 — transformacje pomiędzy frame'ami.
• gazebo_ros / ign_ros — symulator z ROS bridge.

Pattern: action / service / topic

• Topic (publish/subscribe) — strumieniowanie danych (joint_states, tf). Brak gwarancji delivery.
• Service (request/reply) — synchroniczne zapytanie (IK request → q result). Blokujące.
• Action (request/feedback/result) — long-running tasks (motion plan + execute) z feedback'iem.

Real-time: PREEMPT_RT, 1 kHz control loops

Sterownik joints Pandy działa 1 kHz (= 1 ms cykl). Standardowy Linux ma jitter ~100µs - ms (kernel scheduling, GC, IRQs) — może łatwo zepsuć deadline.

PREEMPT_RT patch — preemptywny kernel dla Linux:

• Większość spinlocks zamieniona na mutex, preemptable.
• Threaded IRQs — można im nadać priority.
• SCHED_FIFO / SCHED_RR — true real-time scheduling z priorytetami.
• Jitter typowy: < 50µs na średnim CPU.

Praktyczne wymagania dla Pandy

• Kernel z PREEMPT_RT (latest 5.x z Real-Time linux).
• Disable hyperthreading, ASLR, frequency scaling, NUMA.
• Affinity: dedicate 1-2 cores tylko dla control loop.
• Pre-allocate memory (no mallocs in loop).
• No file I/O / logging w hot path.
• cyclictest pre-test: max latency < 100µs?

Alternatywy: real-time OS

• QNX — komercyjny mikrokernel, automotive (BMW, Mercedes).
• VxWorks — Mars rovers, lotnictwo.
• FreeRTOS / Zephyr — microcontrollers (Cortex-M).

Komunikacja: DDS, shared memory

DDS (Data Distribution Service) — middleware standardu OMG. Backbone ROS2. Cechy:

• Discovery automatic (no central master jak w ROS1).
• QoS policies: reliability, durability, deadline.
• Network-transparent (na innym hoście — wystarczy multicast).

Latencje typowe:

• Same-process: ~5-20 µs
• Loopback (TCP/UDP): ~50-200 µs
• LAN gigabit: ~200-500 µs
• Wi-Fi: 1-10 ms (NIEDOPUSZCZALNE dla 1 kHz)

Shared memory dla 1 kHz

Dla loops < 1 ms — nawet DDS może być za wolne. Pattern: shared memory ring buffer:

• Producer (control loop) pisze do kolejnego slot bez locków (lock-free).
• Consumer (logger, viz) czyta atomicznie. Slow consumers gubią ramki (akceptowalne dla viz).

Implementacje: iceoryx (Eclipse, używany w ROS2 jako alternative DDS), boost::interprocess.

Profilowanie: CPU vs GPU vs FPGA

Po co profilować? Aby wiedzieć gdzie czas idzie. Reguła kciuka dla 1 kHz loop:

• State reading + kinematics: 50-100 µs
• Collision check (mesh): 200-500 µs
• Dynamics (RNEA): 50-100 µs
• Control law (PD + FF): 10-50 µs
• Write to motor: 10 µs
• Total: 320 - 760 µs (still < 1 ms budget)

Narzędzia:

• perf (Linux) — sampling profiler, niski overhead.
• Intel VTune — szczegółowe metryki cache, branch prediction.
• Tracy — frame-based profiler dla games / robotics.
• ros2 trace — distributed tracing wzdłuż nodes.

Kiedy GPU

Klasyczna pętla sterowania 1 kHz to CPU domain — GPU ma duże opóźnienia (PCIe transfer). GPU używaj gdy:

• Batched collision: 1000+ konfiguracji równolegle (MPPI, CEM).
• Renderowanie / vision: cv processing, NeRF inference.
• Neural inference: policy network, model dynamics, diffusion.
• Trajectory optimization: cuRobo wykonuje pełen RRT-Connect + spline + collision w < 50 ms na GPU.

FPGA / dedykowany hardware

Dla bardzo niskiej latencji (10 µs):

• Motor control (FOC) — Texas Instruments C2000 z PWM.
• Sensor fusion (IMU + encoder) — dedykowany ASIC.
• Vision pre-processing — FPGA z camera interface.

Trade-off: szybkość vs flexibility. CPU dla R&D, FPGA/ASIC dla volume production.

Memory management i lock-free

W real-time loop NIGDY:

• malloc/free — może wywołać sys-call, jitter. Pre-allocate w init.
• printf / std::cout — blokujący I/O. Użyj ring-buffer logger.
• std::mutex z high priority threads — priority inversion. Użyj PI (priority inheritance) mutex.
• std::vector::push_back w sequential containers — może realloc. Pre-reserve.
• RTTI / dynamic_cast — slow path.
• exceptions — pełen unwind costly.

Lock-free patterns:

• SPSC queue (Single Producer Single Consumer) — ring buffer z atomicznymi indexami.
• RCU (Read-Copy-Update) — readers wciąż widzą starą wersję, writer atomically swap pointer.
• folly::AtomicHashMap, moodycamel::ConcurrentQueue — biblioteki popularnych lock-free structures.

Referencje

• Macenski, Foote, Gerkey, Lalancette, Woodall, „Robot Operating System 2: Design, architecture, and uses in the wild" (Science Robotics 2022).
• Coleman et al., „Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study" (JOSER 2014).
• OSRF, „ROS2 Documentation" — docs.ros.org.
• Garbenis & Mainprice, „Real-Time Robot Operating System with PREEMPT_RT" (IROS Workshop 2020).
• Eclipse iceoryx documentation: iceoryx.io.
• OMG DDS Spec: omg.org/spec/DDS.
• Carpentier et al., „Pinocchio: A Fast and Flexible Implementation of Rigid Body Dynamics Algorithms and Their Analytical Derivatives" (SII 2019) — RNEA dla 1 kHz loops.
• Franka Robotics docs: frankaemika.github.io.