Holonomic System

A planar holonomic integrator system with directly commanded velocity. The examples show density feedback, density filtering, and CBF-style safety filtering using. This safe tajectory can also be used as a motion plan for complex robotic systems such as quadrupeds and robot arms.

Run

Run the minimal holonomic density feedback and one-step density filter examples.

python examples/single_integrator/static_single_obstacle/density_feedback.py
python examples/single_integrator/static_single_obstacle/density_filter.py

System Dynamics

The single-integrator state is the planar robot position $x=[x_1,x_2]^T$. The input $u=[u_1,u_2]^T$ directly controls the robot velocity, which makes this the simplest setting for studying density-based navigation.

$$ \dot{x}=u, \qquad x_{k+1}=x_k+\Delta t\,u_k . $$

Density Feedback Controller

The direct controller evaluates the density gradient at the current state and applies it as the input for the next discrete step. The same feedback law handles both the single-obstacle and multi-obstacle cases because obstacle information enters through $\rho$.

$$ \begin{aligned} u_k &= \nabla\rho(x_k),\\ x_{k+1} &= x_k+\Delta t\,u_k. \end{aligned} $$

Single Obstacle

Multiple Obstacles

Density Safety Filter

The density filter keeps a nominal controller in the loop and modifies it only when the one-step density condition requires intervention. In local sensing, the same filter is solved with only the currently active obstacle set.

$$ \begin{aligned} \min_{u_k,s}\quad &\frac{1}{2}\|u_k-u_{\mathrm{nom}}\|_W^2 +\frac{1}{2}w_s s^2\\ \mathrm{s.t.}\quad &x_{k+1}=x_k+\Delta t\,u_k,\\ &\rho(x_{k+1})-\rho(x_k)+s\ge 0,\\ &u_k\in\mathcal U,\qquad s\ge0. \end{aligned} $$

Single Obstacle

Multiple Obstacles

Local Sensing

Density-Filter Sensing-Radius Sweep

Changing the sensing radius changes when the obstacle begins shaping the density. A smaller radius keeps the trajectory closer to the nominal path until the robot is near the obstacle, while a larger radius makes the filter react earlier and creates a larger safety margin.

CLF-CBF Filter

The CLF-CBF filter enforces a barrier condition around the obstacle and a Lyapunov decrease condition toward the goal using the same discrete-time update.

$$ h(x)=\|x-c\|^2-R^2, \qquad V(x)=\|x-x_g\|^2. $$

$$ \begin{aligned} \min_{u_k,s_h,s_v}\quad &\frac{1}{2}\|u_k\|^2 +\frac{1}{2}w_hs_h^2 +\frac{1}{2}w_vs_v^2\\ \mathrm{s.t.}\quad &x_{k+1}=x_k+\Delta t\,u_k,\\ &h(x_{k+1})\ge(1-\gamma)h(x_k)-s_h,\\ &V(x_{k+1})\le(1-c)V(x_k)+s_v,\\ &u_k\in\mathcal U,\qquad s_h,s_v\ge0. \end{aligned} $$

Comparison with CLF-CBF filter

This comparison places the density controllers and CLF-CBF filter in the same single-obstacle scenario. With $\gamma=0.85$, the exponential CBF behavior is safe but less intuitive to tune for obtaining a desired safety margin. The density function directly encodes the desired safety margin and can be tuned more intuitively by chaning the sensing radius.