Funnel planning and control for manipulators using neural Signed Configuration Distance Functions

Bernhard Wullt; Christos K. Verginis; Per Mattsson; Mikael Norrlöf; Thomas B. Schön

cs.ROOct 10, 2026

Funnel planning and control for manipulators using neural Signed Configuration Distance Functions

Authors: Bernhard Wullt, Christos K. Verginis, Per Mattsson, Mikael Norrlöf, Thomas B. Schön

Organizations: ABB Robotics, Sweden · the Division of Signals and Systems, Department of Electrical Engineering, Uppsala University, Uppsala, Sweden · the Division of Systems and Control, Department of Information Technology, Uppsala University, Uppsala, Sweden

Abstract

Robust motion planning and control for manipulators is the problem of guaranteeing safe execution and collision-free motion under disturbances. The Kinodynamic Motion Planning via Funnel Control (KDF) is a recently proposed planner that comes with robustness guarantees and is particularly attractive for collision-free control as it guarantees closed-loop evolution inside a funnel, i.e., a collision-free region around a nominal trajectory. Despite its attractive theoretical properties, the current approach for computing funnels is computationally demanding, which limits its expressiveness and performance. We address its current limitations by proposing a novel funnel planning and control solution. Our key tool to enable this is a neural Signed Configuration Distance Function (nSCDF) which allows us to estimate collision-free balls and gradients in a computationally efficient way. We use the nSCDF in our planner to compute a nominal trajectory, where path length and clearance are balanced. To track the trajectory with collision-avoidance guarantees, we design a funnel controller that exploits the geometric information from the nSCDF. From numerical experiments, we observe that our planner produces nominal paths $2$ times faster, while at the same time obtaining $20 \%$ shorter path length and $5$ times the clearance. Finally, we also observe improved motion performance and robustness properties when using the nSCDF for funnel control.

Figures & tables

Fig. 1: Signed distance in the configuration space (brighter colors represent larger distance); the thick purple line (obstacle boundary) is the zero isoline. Our method computes a smooth trajectory (black curve) which balances path length and clearance. To track the trajectory, we develop a funnel controller such that the robot’s closed-loop trajectory (orange curve) stays within a funnel defined by the signed distance function (black circle and transparent red region).

Fig. 2: A conceptual illustration of the representation based on slices. The high-dimensional obstacle region (purple) is discretized into slices (dark purple), by forming the intersection with the hyperplanes (black). From a given query point (red point) we compute the distance to a slice (red line), by combining the normal plane distance (green line) and the in-plane distance to the slice boundary (blue line). The smallest distance from all the slices results in our estimate of the distance to the high-dimensional boundary.

Fig. 3: All: A conceptual illustration of how the wrist configuration distance is computed for a 2 DOF arm, where

q_{m}

is the main joint and

q_{w}

is the wrist joint. The left plot is illustrated in the world space, the rest is illustrated in the configuration space. Left: The manipulator has two links (colored black). The main link is made transparent to emphasize that its collision with the obstacle is not considered. The obstacle is a disk (colored red). Its parameter vector is expressed in the base frame

\{0\}

\bm{\sigma}

and in the wrist frame

\{w\}

\bm{\sigma}_{\text{lw}}

. Computing the distance from the local wrist function results in a wrist movement (dashed blue arc) that moves the wrist to the obstacle boundary (transparent green). The distance or the movement does not include the main joint. Middle: Corresponding configuration space representation of the left plot. The configuration (red point); distance computed by the local wrist nSCDF (dashed blue line); the ground truth distance including the movement of the main joint (transparent red disk and green line). Right: To compute an estimate of the distance where the movement of the main joint is included, we discretize the space of the main joints (black points along the horizontal axis). For each grid point, we compute a distance, by forming the hypotenuse (red line) with the distance of the main joint (green line) and the wrist distance (blue line). This forms an estimated safe set (orange circle). From all estimates we take the smallest distance (blue circle).

Fig. 4: Illustrations in the configuration space. The SCDF, $\psi$ , is illustrated as a colormap, with darker colors indicating lower distances. The zero-level contour, i.e., $\partial\mathcal{C}_{\text{o}}$ , is represented by the solid purple curve. Left: Illustration of our medial-axis projection step. A discretized path (red points) is projected to points approximately on the medial axis (purple points), resulting in increased clearance. Middle: The projection steps result in a nonsmooth and distorted path. We solve a weighted shortest path problem by constructing a graph by using the points on the path as vertices. The edges are formed over intersecting balls (black lines). The weight function balances path length and clearance. Solving the problem results in an optimized path in the graph (red curve), which is continuously collision-free due to the intersection requirement of the resulting balls (red circles). Right: We smoothen the piece-wise linear path by solving a convex optimization problem. The key idea is to find a smooth path that is close to the original one (black curve). We do this by constructing collision-free capsules (transparent red) along the connecting line segments of the path. Each capsule is assigned a Bézier curve (colored curve within capsule). Continuity constraints are enforced with its neighboring curve. The closeness of the original path is controlled by the width of the capsules. Solving the optimization problem results in a smooth collision-free path.

Panel of Figure 5 — Fig. 5: Left: Illustration in world space, showing an example problem instance used in the experiments. The manipulator is 6 DOF colored black at a configuration along a collision-free path. The end-effector position is traced out along the path (blue curve). The start and goal configurations are colored transparent red and green, respectively. The obstacles are spheres (colored gray). Right: The learned configuration space representation of the world instance to the left, illustrated in the sub-space of the first three joints, i.e., main joints. The red, green and blue colored regions are the obstacle spaces for the main joints, wrist joints and self-collisions. The resulting path from our method is the black curve.

mean	$Q_{95}$	$Q_{99}$	$Q_{99.9}$	max
0.001	0.005	0.033	0.061	0.141

TABLE I: Statistics of the overestimation in the ball radius. In total,

10^{4}

balls were used. A percentile is denoted with a

Q_{p}

, where

p

is the used percentile. All values are in radians.

Fig. 6: All: The vertical axis presents the integrated norm of the resulting jerk. Each data point is the average from 10 simulations in different funnels. If a run failed, then the average was not formed and no data point was added to the graph. The shaded region corresponds to

\pm

one standard deviation. Funnel sizes:

0.10\>rad

(blue),

0.15\>rad

(orange) and with the nSCDF (green). Left: The horizontal axis represents different levels of model uncertainties. Right: The horizontal axis represents different amplitudes on the disturbance signal.

Path planning	R.time $(ms)$	P.len $(rad)$	Clear. $(rad)$
RRT + EFSS [ 8 ]	$2006\pm 5545$	$11.60\pm 4$	$0.10\pm 0.00$
RRT + nSCDF (ours)	$23\pm 162$	$10.27\pm 3$	$0.36\pm 0.08$
Method steps
Projection	$618\pm 197$	$11.59\pm 3$	$0.52\pm 0.10$
WSP	$3\pm 1$	$9.65\pm 3$	$0.50\pm 0.10$
Smoothing	$167\pm 146$	$9.43\pm 3$	$0.49\pm 0.10$

TABLE II: Statistics from 100 problem instances. Performance is measured based on runtime (R.time), path length (P.len) and clearance (Clear.). The format of the data is average

\pm

standard deviation. The EFSS method is not able to give any clearance beyond the radius used to validate the region. All paths were collision-free.

Safe Motion Planning

Safe motion planning develops methods for robots and vehicles to reach goals while avoiding collisions and respecting dynamic and uncertainty constraints.

See all 211 papers

CSSDF-Net: Safe Motion Planning Based on Neural Implicit Representations of Configuration Space Distance Field
Mar 19, 2026Haohua Chen, Yixuan Zhou, Yifan Zhou +1Safe Motion Planning Robot Collision Avoidance
IRMV Lab, Department of Automation, Shanghai Jiao Tong University, Shanghai 200240, China.
Stochastic Neural Signed Swept Volume for Real-time Chance-Constrained Trajectory Optimization
Sep 18, 2026Qingyi Chen, Kevin Zhang, Lucas Chen +1Robot Motion Planning Robot Collision Avoidance
Department of Computer Science, Purdue University, West Lafayette, IN, USA
GPU-Accelerated Polygonal Signed Distance Functions for Real-Time Collision Avoidance
Jul 5, 2026Taekwon Ga, Jongeun ChoiSigned Distance Functions GPU Acceleration
School of Mechanical Engineering, Yonsei University, Seoul 03722, South Korea

Mar 19, 2026J/K move · Enter open · S save

CSSDF-Net: Safe Motion Planning Based on Neural Implicit Representations of Configuration Space Distance Field

Haohua Chen, Yixuan Zhou, Yifan Zhou, Hesheng Wang

IRMV Lab, Department of Automation, Shanghai Jiao Tong University, Shanghai 200240, China.

Safe Motion Planning Robot Collision Avoidance Constrained Motion Planning

Open paper AskPDF arXiv

High-dimensional manipulator operation in unstructured environments requires a differentiable, scene-agnostic distance query mechanism to guide safe motion generation. Existing geometric collision checkers are typically non-differentiable, while workspace-based implicit distance models are hindered by the highly nonlinear workspace--configuration mapping and often suffer from poor convergence; moreover, self-collision and environment collision are commonly handled as separate constraints. We propose Configuration-Space Signed Distance Field-Net (CSSDF-Net), which learns a continuous signed distance field directly in configuration space to provide joint-space distance and gradient queries under a unified geometric notion of safety. To enable zero-shot generalization without environment-specific retraining, we introduce a spatial-hashing-based data generation pipeline that encodes robot-centric geometric priors and supports efficient retrieval of risk configurations for arbitrary obstacle point sets. The learned distance field is integrated into safety-constrained trajectory optimization and receding-horizon MPC, enabling both offline planning and online reactive avoidance. Experiments on a planar arm and a 7-DoF manipulator demonstrate stable gradients, effective collision avoidance in static and dynamic scenes, and practical inference latency for large-scale point-cloud queries, supporting deployment in previously unseen environments.

Abstract

Safe Motion Planning

Explore similar work

CSSDF-Net: Safe Motion Planning Based on Neural Implicit Representations of Configuration Space Distance Field

Stochastic Neural Signed Swept Volume for Real-time Chance-Constrained Trajectory Optimization

GPU-Accelerated Polygonal Signed Distance Functions for Real-Time Collision Avoidance