Six experiments, from a point to a robot arm

I built the method one piece at a time, from the smallest example that could show each idea. Each experiment asks one question, and each result here comes from those runs, including the ones that went badly.

How I experiment

Every experiment follows the same habits, so that a good number can't come from a poor test:

HabitWhy
Always run a baseline with no filterOtherwise, "0% violations" could just mean the motion never went near anything. So I check that the baseline really crosses the unsafe area.
Check the parts before trusting the resultsGradients and Jacobians are compared with numerical differences, and the solver with a brute-force search or a known formula.
Use more than one measureA robot that never moves has 0% violations. So I always report safety and whether it reached the goal, along with time and clearance.
Check independentlyOn the arm, clearance comes from MuJoCo's distance between real meshes, not from my own spheres, so they can't mark their own homework.
Report what went wrongTrapped runs, bugs I fixed and setup changes all stay in the record.
Predict, then runBefore changing a setting, write down what you expect. Being wrong is where the learning happens.

All of this runs in simulation, in Python, with MuJoCo and OSQP, on one laptop. The filter runs at 45 Hz. The rules and object shapes are typed in or generated, not perceived.

Experiment 1Does the filter keep one point away from one object?

Setup: a point robot in 2-D, driving straight at a goal past a round object, from 200 random starts. Concept: the QP diagram.

MeasureNo filterNormalCautious
Runs entering the unsafe area100%0%0%
Closest clearance (m)−0.2340.0470.110
Time to goal (s)4.766.527.36
Paths of a point robot past a circle with no filter, normal and cautious
Paths from the 200 starts. Without any new rule about distance, the cautious robot turns away earlier.

What it showed: entries fell from 100% to 0%, but trips took 37% longer, or 55% with caution. And a robot starting exactly in line with the object and the goal got stuck, 5 mm from the circle: the safety guarantee isn't a guarantee of progress. Also, the filter only protects what it's told about: a large book it didn't know about was entered in every run.

Experiment 2What changes with many objects and flat shapes?

Setup: the same point robot, now with a real QP solver and superquadrics. Concept: the trap diagram.

Shape around a 0.30 × 0.44 m laptopArea blockedGoal straight behindGoal to one side
Circle0.222 m²100% reached, 6.23 s100% reached, 5.12 s
Box-like, e = 0.30.157 m²0% reached100% reached, 5.18 s
Paths past circular and box-like shapes, showing box-like paths stopping at the flat face
With the goal straight behind, the box-like shape stopped every run at its flat face. No run entered the true laptop.

What it showed: telling the filter about a book cut entries in the book from 100% to 0%. But a tighter shape isn't simply better. The box-like shape blocked 29% less area, yet it failed one goal completely. The solver took 0.06 ms on average, against a 22 ms budget.

Experiment 3Can the same motion be safe with a sponge and unsafe with a cup?

Setup: a point in 3-D moving over a laptop, with rules depending on what it holds, from 200 starts per row. Concept: semantic regions.

RulesGoal straight behindGoal to one sideTime, to the side
No filterPasses over: 100%, reached: 100%Passes over: 100%, reached: 100%3.18 s
Sponge, collisions onlyPasses over: 100%, reached: 100%Passes over: 100%, reached: 100%3.18 s
Cup, semanticPasses over: 0%, reached: 0%Passes over: 0%, reached: 100%4.09 s
Cup and sponge paths over a laptop
The side view can look as if paths go through the region, but the view from above shows them going around it.

What it showed: this is the core result. A collision-only filter sees no problem with passing over the laptop, because nothing is there to hit. Only the rule list changes, not the maths. The trap appeared here too: at e = 1, every run got through, but the region blocked 2.17 times the true column, against 1.35 times at e = 0.3.

Approach towards the region edge under normal and cautious settings
My version of the paper's Fig. 4. Cautious started to slow at 13.5 cm from the edge, and normal at 7.9 cm. It has the same pattern, but the numbers aren't comparable, as the shape and motion differ.

Experiment 4Does it still work on a 7-joint arm?

Setup: the Franka FR3 in MuJoCo, with rules on the tool point only, 30 runs per row, using MuJoCo's physics with position motors. Concept: inverse kinematics.

RulesTool over laptopReachedTime
No filter100%100%2.75 s
Sponge100%100%2.75 s
Cup0%100%6.08 s
The arm with its tool outside the red region and its upper links passing through
The tool stays out, but the upper links pass through the region, and the hand isn't level. Experiment 5 deals with both.

How slow can the filter run?

Filter rateEntered region
45 Hz0%
15 Hz0%
5 Hz0%
2 Hz33%

With 15 runs per rate, at about 0.3 m/s. Faster motion would need a higher rate.

What it showed: the method carries over to an arm, and the Jacobian checked out against numerical differences to 0.0000000002. The trap got worse near an edge: a goal 2.2 cm past the region's edge left the arm crawling, while one 16 cm past took 4.3 s.

Experiment 5Can it protect the whole arm and keep a cup upright?

Setup: 20 spheres on the arm, sphere pairs for self-collision, and the rotation penalty. Clearance is measured independently, between MuJoCo's real meshes. 20 runs per row. Concept: the whole arm.

The arm stopped beside a pillar
Every whole-arm run stayed clear of the pillar, and stopped there.
FilterTouched pillarLowest clearanceReached
No filter100%−8.7 cm100%
Tool point only100%−8.7 cm100%
Whole arm0%+6.5 cm0%

Keeping the cup upright

SetupReachedTimeMedian rotation speedLargest tilt
Sponge, 3-D command100%2.14 s30.8 °/s26.9°
Cup, 3-D command100%4.37 s11.7 °/s28.2°
Cup, 6-D command100%4.55 s3.9 °/s6.6°
Cup, 6-D command and penalty100%5.21 s0.75 °/s2.8°
Rotation speed distributions for each setup
My version of the paper's Fig. 5. The paper reports a 75% reduction, for a different comparison on a real robot, so the numbers aren't comparable.

What it showed: a tool-only filter let the body go 8.7 cm into a pillar. The whole-arm filter stopped that, but it never reached the goal, and my oversized spheres probably make this worse. Most of the tilt improvement came from the 6-D command, which I had to add when the arm stalled. There's no real water here: tilt is measured on the hand, so this says nothing direct about spilling. The full filter took 3.3 ms per step on average with 321 rules.

Experiment 6Does the paper's main result appear in a kitchen?

Setup: the arm on a kitchen counter, with 8 cases following the layout of the paper's Table II. There are 5 scripted motions for each case, or 40 runs per filter. Every filter is scored against the semantic filter's full rule set.

SceneHeldNo filterGeometricSemantic
BooksDry sponge0.0 (100%)0.0 (100%)0.0 (100%)
BooksCup of water47.5 (100%)40.7 (100%)0.0 (60%)
Laptop and booksNothing0.0 (100%)0.0 (100%)0.0 (100%)
Laptop and booksLit candle50.4 (100%)42.0 (100%)0.0 (60%)
Laptop and booksCup of water50.4 (100%)42.0 (100%)0.0 (60%)
Balloons and towelCup of water28.1 (100%)0.0 (80%)0.0 (80%)
Balloons and towelLit candle61.9 (100%)21.8 (80%)0.0 (20%)
Balloons and towelKnife9.6 (100%)0.0 (100%)0.0 (100%)

Mean % of scored steps breaking any rule, with % of runs reaching the goal in brackets.

One motion from each of two cases, under all three filters. Red boxes show the rules, and appear only with the semantic filter, though all three are scored against them. It's a small sample, so read it with the table, including the unfinished runs.
The paper's pattern holds, with a real cost. The semantic filter had 0% violations in all its runs, while the geometric filter still broke the rules up to 42.0% of the time. But only 72.5% of semantic runs reached their goals on average, and 20% with a candle among balloons.

Reading these carefully:

Normal and cautious approach on the arm
On the arm, after 8 s, normal closed to h = 0.11, and cautious stayed back at h = 0.45.
Rotation speed for a sponge and a cup
Median rotation while moving: sponge 31.3 °/s, cup 0.52 °/s. This compares different commands as well as the penalty, so it isn't comparable with the paper's figure.

What I learned

  1. Semantic safety does something collision checks can't. The same motion in the same scene is safe or unsafe depending on what's held, and only the rule list changes.
  2. Zero violations alone means little. A robot that never moves also scores zero, so safety has to be read with reach.
  3. A filter without a plan can get stuck. This was the most persistent problem, from the first experiment to the last. A tighter shape blocks less space but traps more often.
  4. Commands and penalties have to agree. The biggest surprise was an arm stalling because the command and the penalty fought each other.
  5. Speed wasn't the limit. About 2 ms per step on a laptop, though perception and a language model aren't included.

Not done yet

There's no language model producing the rules, no perception from a camera, no moving objects, no position error, and no real robot or person. I can't reproduce the paper's real kitchen, diffusion policies or user study here.

Your turn: predict, then run. In the demo, hold the cup and put the laptop directly between the start and goal. Before pressing the button, write down which filters you expect to enter the red area, and whether semantic will reach the goal. Then double the laptop's size and predict again.