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
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 qm is the main joint and qw 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} as σ and in the wrist frame {w} as σ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, ψ , is illustrated as a colormap, with darker colors indicating lower distances. The zero-level contour, i.e., ∂Co , 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.
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
Q95
Q99
Q99.9
max
0.001
0.005
0.033
0.061
0.141
TABLE I: Statistics of the overestimation in the ball radius. In total, 104 balls were used. A percentile is denoted with a Qp , 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 ± one standard deviation. Funnel sizes: 0.10rad (blue), 0.15rad (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±5545
11.60±4
0.10±0.00
RRT + nSCDF (ours)
23±162
10.27±3
0.36±0.08
Method steps
Projection
618±197
11.59±3
0.52±0.10
WSP
3±1
9.65±3
0.50±0.10
Smoothing
167±146
9.43±3
0.49±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 ± 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 develops methods for robots and vehicles to reach goals while avoiding collisions and respecting dynamic and uncertainty constraints.
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.
Haohua Chen, Yixuan Zhou, Yifan Zhou +1
IRMV Lab, Department of Automation, Shanghai Jiao Tong University, Shanghai 200240, China.
Collision-free motion planning requires reliable collision models from sensed environments and validation of states along a continuous trajectory. To make this tractable, most planners check for collision at discrete states along continuous trajectories against a single determinized model of the environment, introducing a trade-off between safety and computational efficiency. While continuous collision checking approaches that approximate the swept volume of the robot exist, they are computationally expensive or overly conservative. Data-driven approaches can learn the swept volume; however, these neural models are susceptible to approximation errors and are therefore often limited to serving as coarse filters for downstream collision checkers. In this work, we propose to learn a signed distance function of the swept volume as a probabilistic field, enabling quantification of epistemic uncertainty, incorporation of perception noise, and eventual integration into a chance-constrained trajectory optimization framework. We demonstrate our approach on challenging high-dimensional manipulation problems with significant sensor noise, both in simulation and on real hardware.
Qingyi Chen, Kevin Zhang, Lucas Chen +1
Department of Computer Science, Purdue University, West Lafayette, IN, USA
Optimization-based local planning and control require high-rate evaluation of collision-avoidance constraints over a prediction horizon. Accurately accounting for robot and obstacle geometry in these evaluations can be computationally expensive. The resulting bottleneck motivates collision-avoidance constraints that combine computational efficiency with geometric fidelity. The proposed polygonal signed distance function (PSDF) returns the minimum of exact signed distances between a convex polygonal robot footprint and convex obstacle components represented by their boundary edges. It is implemented as a training-free, branch-free tensorized geometric pipeline enabling batched GPU execution and automatic differentiation. The PSDF is embedded in model predictive control by locally linearizing the PSDF-based safety constraint within a sequential quadratic programming--based real-time iteration scheme, yielding the PSDF-embedded model predictive controller (PSDF-MPC). The design separates CPU/GPU computation so that the GPU evaluates batched PSDF values and gradients while the CPU solves a sparse quadratic program whose size and sparsity are determined by system dimensions and horizon length rather than obstacle and edge counts. Microbenchmarks show that PSDF scales favorably relative to geometric and learned collision-field baselines. Closed-loop comparisons in simulation, together with real-world navigation experiments, demonstrate that PSDF-MPC operates in real time and achieves collision-free navigation in dense polygonal environments.
Taekwon Ga, Jongeun Choi
School of Mechanical Engineering, Yonsei University, Seoul 03722, South Korea