Configuration Space Distance Fields for Manipulation Planning

Authors: Yiming Li, Xuemin Chi, Amirreza Razmjoo, Sylvain Calinon · RSS 2024

Licences

Open Access
Paper: CC BY 4.0

Everything below draws on the full paper under its CC BY 4.0 licence; its figure is credited to the authors.

TL;DR

Defines a distance field directly over joint angles, so whole-body IK becomes a one-step gradient projection and planners avoid the local minima SDFs cause on a 7-DoF Franka.

Why it matters

Planners and controllers that already take a distance function (QP, iLQR, geometric fabrics) can use CDF in place of an SDF and work directly in joint space. On the planar robot and the Franka, the same planners had higher success rates and lower tracking error with CDF than with SDF.

Problem

Signed distance fields are normally defined in 3D task space, so manipulation control first measures distance there and then solves inverse kinematics iteratively to find joint motions. Because forward kinematics is nonlinear, the field loses its unit-gradient property when viewed in joint space, and gradient-based solvers can stall where gradients vanish or at singularities.

Proposed solution

The configuration space distance field (CDF) gives, in radians, the smallest joint motion that brings any part of the robot into contact with a point; it satisfies the eikonal equation, so the nearest contact configuration is reached by projecting along the gradient. Contact configurations are found offline for each workspace grid point with batched L-BFGS on a robot SDF model, per-point fields are fused online with a minimum operation, and a 5-layer MLP (neural CDF) gives a compact, grid-free, differentiable version.

Key insights

A distance field defined over joint angles keeps unit-norm gradients, so whole-body IK for a contact target reduces to one projection, q′ = q − f_c ∇f_c, with no Gauss-Newton iterations.
The field value depends only on the joints before the contacting link; taking a per-link minimum improves accuracy when few contact-configuration samples exist.
Per-point fields can be precomputed and combined online with a minimum (a union in configuration space), so the representation does not depend on a specific scene.
Gradients of the field follow paths that curve around obstacles in joint space, which helps reactive planners avoid the local minima seen with task-space SDFs.

Main results

Whole-body IK on the Franka from 10,000 random initial configurations (Table II): CDF with 2-step projection found 8,773 valid solutions in 10.6 ms; SDF + L-BFGS found 3,652 in 971 ms and Neural-JSDF + L-BFGS 264 in 272 ms. The text states this as over 700,000 valid solutions per second, 180 times the SDF approach.
Neural CDF accuracy: 1.39 ± 0.51 cm MAE and 91.1 ± 8.12% projection success (3 cm threshold) after 3 projection iterations; GPU inference took 0.49 ms at batch size 1 and 4.61 ms at 10^5 on an RTX 3090.
Franka motion planning over 100 runs: CDF success rates of 88–93% with IPOPT, QRQP, OSQP, qpOASES and geometric fabrics versus 38–74% with SDF; iLQR 48% versus 38%. On the 2-DoF planar robot, CDF reached 76–95% versus 40–68% for SDF.
Goalkeeper task in simulation (100 throws): the arm intercepted the ball 82% of the time with CDF versus 35% with SDF.
Dual-arm large-box contact planning: 7.65 s planning time and 1.37 rad average joint distance with CDF + filtering versus 54.80 s and 2.85 rad with SDF + Gauss-Newton optimisation.
QP controllers using the neural CDF run above 200 Hz; neural CDF training took about 2 hours on one RTX 3090.

Limitations

The field is unsigned, and with few contact-configuration samples it is inaccurate for configurations close to contact; the authors suggest adding sign information.
It is built from one robot's geometry and kinematics and must be recomputed for each robot model; scaling to long kinematic chains such as full humanoids remains open.
Offline computation is slow (1–10 s to find contact configurations), the grid version is memory-inefficient, and the neural version gives up accuracy; the authors note the network training was not fully optimised.
Distance is measured as angular (Euclidean joint) distance; geodesic distance on the configuration-space manifold is left for future work.
Real-robot planning experiments are shown qualitatively (Fig. 8) without success rates, and the goalkeeper task was run only in simulation.

Field impact

Finalist for the RSS 2024 Outstanding Paper Award, with 48 citations. The official project page links the code at github.com/yimingli1998/cdf.

Abstract

The signed distance field is a popular implicit shape representation in robotics, providing geometric information about objects and obstacles in a form that can easily be combined with control, optimization and learning techniques. Most often, SDFs are used to represent distances in task space, which corresponds to the familiar notion of distances that we perceive in our 3D world. However, SDFs can mathematically be used in other spaces, including robot configuration spaces. For a robot manipulator, this configuration space typically corresponds to the joint angles for each articulation of the robot. While it is customary in robot planning to express which portions of the configuration space are free from collision with obstacles, it is less common to think of this information as a distance field in the configuration space. In this paper, we demonstrate the potential of considering SDFs in the robot configuration space for optimization, which we call the configuration space distance field. Similarly to the use of SDF in task space, CDF provides an efficient joint angle distance query and direct access to the derivatives. Most approaches split the overall computation with one part in task space followed by one part in configuration space. Instead, CDF allows the implicit structure to be leveraged by control, optimization, and learning problems in a unified manner. In particular, we propose an efficient algorithm to compute and fuse CDFs that can be generalized to arbitrary scenes. A corresponding neural CDF representation using multilayer perceptrons is also presented to obtain a compact and continuous representation while improving computation efficiency. We demonstrate the effectiveness of CDF with planar obstacle avoidance examples and with a 7-axis Franka robot in inverse kinematics and manipulation planning tasks.

Open Source Robotics