Configuration Space Distance Fields

Configuration Space Distance Fields for Manipulation Planning

Yiming Li 1,2, Xuemin Chi 1,3, Amirreza Razmjoo 1,2 and Sylvain Calinon 1,2

1 Idiap Research Institute 2 EPFL 3 Zhejiang University

Robotics: Science and Systems (RSS), 2024

Paper

Code

Abstract

The signed distance field (SDF) 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 (or CDF for short). Similarly to the use of SDF in task space, CDF provides an efficient joint angle distance query and direct access to the derivatives (joint angle velocity). Most approaches split the overall computation with one part in task space followed by one part in configuration space (evaluating distances in task space and then computing actions with inverse kinematics). 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 (MLPs) 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.

Distance Fields in Robotics

The signed distance fields (SDFs) are conventionally employed in task space. It can also be visualized in configuration space.

A typical control task is composed of two steps:

Planning in task space
Finding corresponding joint angle configurations by solving inverse kinematics

Due to the nonlinear mapping, this problem is typically solved with a few iterations by second-order optimization (corresponding to the pseudoinverse of a Jacobian)

We propose Configuration Space Distance Field (CDF), a scalar field directly measuring the angular distance in configuration space, corresponding to the minimum joint motion required by the robot to establish contact with an object.

The distance evenly spans and the negative gradient consistently points toward the object with unit L2 norm.

CDF solves inverse kinematics problems through one-step gradient projection, while SDFs require multi-step interactions or even fails.

CDF for inverse kinematics

SDF for inverse kinematics

Geodesics on the CDF naturally wrap around the shape of the object in configuration space.

Geodesics on CDF

Geodesics on SDF

Approaches developed for SDFs can be applied to CDF to solve problems directly in configuration space.

Neural CDF

Similar to SDFs, CDF can be represented through a volumetric grid. This representation is non-parametric and inefficient in terms of computation and storage. Instead, we propose neural CDF, which is grid-free, continuous, and introduces trade-offs between accuracy, efficiency, and compression capabilities. It also allows latent space operation, enhancing flexibility.