Dynamic Obstacles · Safe Control Density

Run

Run the velocity-obstacle density controller and filter on the dynamic-obstacle setup.

python examples/dynamic_obstacles/density_feedback/velocity_obstacle.py --scenario closing_in
python examples/dynamic_obstacles/density_filter/velocity_obstacle.py --scenario closing_in

System Dynamics

The robot uses the same unicycle dynamics as the non-holonomic examples. Obstacles are moving disks with measured center $p_j$, radius $r_j$, and velocity $v_j$.

$$ \dot p_x=v\cos\theta,\qquad \dot p_y=v\sin\theta,\qquad \dot\theta=\omega $$ $$ p_j(t+\Delta t)=p_j(t)+\Delta t\,v_j(t),\qquad R_j=r_{\mathrm{robot}}+r_j+r_{\mathrm{margin}} $$

Velocity-Aware Density

Dynamic obstacles need more than position. The reactive density sees the obstacle disk, while collision-cone and velocity-obstacle density also look at relative velocity.

Reactive moving bump

Each obstacle is treated as a moving circular density hole. This works well when position is enough, but it can react late to fast closing obstacles.

$$\Phi_j=\beta(\|p-p_j\|;R_j,R_j+r_{\mathrm{sense}})$$

Collision cone

The cone is built from relative position and relative velocity. If the relative velocity points into the cone, the density term activates before the disks touch. The density obstacle term is still a bump function: we multiply the spatial bump by a smooth bump on the collision-cone margin.

$$ \Phi_j^{\mathrm{cc}} =\beta_{\mathrm{space}}(\|p-p_j\|) \,\beta_{\mathrm{vel}}(h_{\mathrm{cc}}) $$ $$ h_{\mathrm{cc}}=p_{\mathrm{rel}}^\top v_{\mathrm{rel}}+\|p_{\mathrm{rel}}\|\|v_{\mathrm{rel}}\|\cos\phi $$

Velocity obstacle

The unsafe set is drawn in velocity space. The controller avoids commands inside the cone even before the obstacle is close. We wrap this velocity-space test in the same way: spatial bump times smooth bump on the VO margin.

$$ \Phi_j^{\mathrm{vo}} =\beta_{\mathrm{space}}(\|p-p_j\|) \,\beta_{\mathrm{vel}}(h_{\mathrm{vo}}) $$ $$ h_{\mathrm{vo}}=\max(\mathrm{cross}(\ell,v-v_j),\mathrm{cross}(v-v_j,r)) $$

Interactive Result Viewer

Choose a scenario, density construction, and controller. Dense flow also has a local-frame view that follows the robot so obstacle interactions are easier to inspect.

Scenario Controller Density Local-frame dense-flow view

Controller Gallery

Each section fixes the controller and shows the reactive bump, collision-cone, and velocity-obstacle density constructions for the closing-in and dense-flow scenarios.

Density feedback

Density feedback builds a planar motion command from the density field, then maps that command to unicycle forward speed and yaw rate. It is the fastest controller in these examples: the reactive bump uses only obstacle positions, while the collision-cone and velocity-obstacle versions bias the planar command using obstacle velocity. For dense moving scenes, the controller evaluates only the nearest active obstacles instead of every obstacle in the map. This keeps the computation small and focuses the correction on obstacles that can affect the next few seconds of motion.

Closing in

This scenario surrounds the robot with obstacles that move inward. We use it as a stress test for early velocity-aware reactions: the controller should leave the closing group before the free space collapses, not wait until the obstacles are already tight.

Closing in - reactive

Closing in - collision cone

Closing in - velocity obstacle

Dense flow

Dense flow keeps the robot moving through a cluttered stream of obstacles with different velocities. We show both the global frame and a local frame: the global view shows task progress, while the local view makes the short-range avoidance decisions easier to see.

Reactive

Global frame

Local frame

Collision cone

Global frame

Local frame

Velocity obstacle

Global frame

Local frame

Density filter

Density filter keeps a nominal unicycle command in the loop and solves a one-step constrained problem over $[v,\omega]$. The optimizer stays close to the nominal command unless the next predicted unicycle step would reduce the density condition too much. In the dynamic-obstacle scripts, the filter uses a frozen velocity snapshot for each timestep: obstacle positions and velocities are read once, the command is solved, and then the robot and obstacles are advanced together. This avoids the artificial asymmetry that can appear when velocity information changes halfway through a control update.

The filter also uses a small active-neighbor set. For ordinary dense flow, nearest neighbors are enough and much cheaper than checking every obstacle. For tightly surrounded cases, the sensing radius and velocity-aware density terms help the filter react before the obstacles physically close around the robot. The local-frame videos do not change the controller; they only move the camera with the robot so the obstacle interactions are easier to inspect.

Closing in

The filter version uses the same surrounded start, but now the safety correction is solved directly over the unicycle inputs. This highlights whether the one-step constrained solve can keep moving toward the goal while dynamic obstacles close around the robot.

Closing in - reactive

Closing in - collision cone

Closing in - velocity obstacle

Dense flow

Dense flow tests repeated local replanning. The filter only modifies the nominal command when the next predicted unicycle step would violate the density condition, so the global and local views show how much correction is needed as obstacles enter and leave the active-neighbor set.