Representing Robot Geometry as Distance Fields: Applications to Whole-body Manipulation
Yiming Li, Yan Zhang, Amirreza Razmjoo, Sylvain Calinon
Idiap Research Institute & EPFL
IEEE International Conference on Robotics and Automation (ICRA), 2024
Abstract
In this work, we propose a novel approach to represent robot geometry as distance fields (RDF) that extends the principle of signed distance fields (SDFs) to articulated kinematic chains. Our method employs a combination of Bernstein polynomials to encode the signed distance for each robot link with high accuracy and efficiency while ensuring the mathematical continuity and differentiability of SDFs. We further leverage the kinematics chain of the robot to produce the SDF representation in joint space, allowing robust distance queries in arbitrary joint configurations. The proposed RDF representation is differentiable and smooth in both task and joint spaces, enabling its direct integration to optimization problems. Additionally, the 0-level set of the robot corresponds to the robot surface, which can be seamlessly integrated into whole-body manipulation tasks. We conduct various experiments in both simulations and with 7-axis Franka Emika robots, comparing against baseline methods, and demonstrating its effectiveness in collision avoidance and whole-body manipulation tasks.
Video
Represent SDFs using basis functions
Illustration of iterative learning for a two-dimensional SDF from samples at different locations. The weights are initialized to resemble a circular object. Red points are sequentially sampled, allowing for subsequent weight updates. The contour of the estimated object shape is depicted by the blue curve (represented implicitly as equidistance contours of 0 from the SDF).
Numercial Performance
Comparison between basis functions and neural networks for representing robot links. Bernstein polynomials show higher data efficiency.
Smoothness in task space
Smoothness in joint space
Collision Avoidance
Collision avoidance experiment in simulation. g_1 and g_2 represent the target points. Red points are sampled with f = 0.05 on the right arm to represent the safety threshold surface.
Real-world collision avoidance experiment. Here g is the target point for the right arm. Black/red arrows show the reaching velocity with/without collision avoidance. Black dashed cycles show the potential collision area.
Dual-arm lifting
Robot experiments for whole-body dual-arm lifting. Top row: the planned joint configurations for grasping the box. Bottom row: the final states after lifting.