Forward Kinematics Kernel for Improved Proxy Collision Checking

Nikhil Das, Michael C. Yip

TL;DR

This paper introduces a novel "Forward Kinematics Kernel" (FK kernel) for robot collision checking. By leveraging forward kinematics to compare robot configurations in workspace space rather than just joint space, they achieve significantly faster and more accurate collision detection, leading to substantial improvements in robot motion planning.

What Problem Does This Paper Address?

Motion planning for robots, especially in dynamic environments, relies heavily on efficient and accurate collision checking. Traditional methods often involve computationally expensive geometric collision detectors. While proxy collision checkers offer speedups, they can sacrifice accuracy or struggle to adapt to changing environments. Existing kernel-based proxy methods, like those using Radial Basis Functions (RBF kernels), primarily compare joint configurations, failing to capture how these configurations translate to the robot's actual position and orientation in the workspace. This disconnect can lead to inaccurate collision predictions and inefficient planning.

Methods

The core contribution is the Forward Kinematics Kernel (FK kernel). Instead of directly comparing joint values (like RBF kernels), the FK kernel uses forward kinematics (FK) to compute the positions of "control points" on the robot in workspace space. The kernel then calculates a similarity score based on the distances between these corresponding control points across different robot configurations. This approach allows the kernel to implicitly understand the robot's spatial configuration.

The FK kernel is integrated into an existing proxy collision checker called Fastron. Fastron uses a kernel perceptron model to learn a C-space representation. The FK kernel replaces the previous RBF kernel within Fastron. The algorithm uses a greedy coordinate descent approach to minimize a loss function, aiming to achieve a positive margin for all training samples, indicating correct classification. This process also promotes model sparsity by identifying essential "support points."

Datasets

The paper does not explicitly mention specific named datasets. Instead, it describes experiments conducted using a 7-DOF Baxter robot arm. For training and evaluation, they generated random robot configurations and used the FCL (Fast Collision Library) as a ground truth collision detector to label these configurations. The experiments involved randomly sized and moving obstacles.

Key Insights and Ideas

  • Workspace-centric comparison: The most significant insight is that comparing robot configurations based on their workspace positions (via FK) is more effective for collision checking than solely comparing joint values. This is because it directly relates to the robot's interaction with its environment.
  • FK kernel's intuitive representation: The FK kernel provides a more intuitive representation of C-space similarity, better reflecting the perceived proximity of robot configurations in the workspace.
  • Improved model sparsity: The FK kernel, when used with Fastron, leads to significantly fewer support points needed to represent the collision-free space, reducing memory footprint and computational overhead.
  • Bridging C-space and Workspace: The FK kernel acts as a crucial bridge, explicitly linking the robot's configuration space to its workspace representation, a connection often neglected in other learning-based methods.

Key Results

  • Accuracy Boost: Fastron with the FK kernel achieved over 95% classification accuracy for a 7-DOF robot arm, a significant improvement from the 75% accuracy of the RBF kernel.
  • Speedup:
    • Proxy collision check speed was doubled compared to Fastron with RBF.
  • Collision checks were up to 9 times faster than state-of-the-art geometric collision checkers (GJK and FCL).
  • Motion planning was up to 3 times faster when using Fastron FK with various OMPL planners.
  • Memory Reduction: The FK kernel required 8 times fewer support points (model size) compared to the RBF kernel.
  • Performance in Dynamic Environments: Fastron FK maintained competitive query times even with an increasing number of obstacles, outperforming geometric methods that showed significant query time increases.
  • Reduced False Negatives: Failure cases with the FK kernel showed only slight intersections with obstacles, whereas RBF kernel failures often involved blatant penetration, indicating better robustness.

Limitations and Future Work

  • Limitations:
    • While the FK kernel is positive definite, the paper notes that summand kernel matrices can be positive semi-definite if control point locations coincide. However, they prove the overall FK kernel matrix remains positive definite under certain conditions.
  • The paper acknowledges that the Fastron algorithm's update time is still significantly influenced by the time taken for ground truth collision checks to label new data.
  • Future Work:
    • The authors suggest exploring GPU parallelization for faster kernel evaluation and proxy collision checking.
  • They also propose applying the FK kernel to other kernel-based methods.
  • Further investigation into applications of the FK kernel in other areas of robotics is implied.

Summary updated

Abstract

Kernel functions may be used in robotics for comparing different poses of a robot, such as in collision checking, inverse kinematics, and motion planning. These comparisons provide distance metrics often based on joint space measurements and are performed hundreds or thousands of times a second, continuously for changing environments. We introduce a new kernel function based on forward kinematics (FK) to compare robot manipulator configurations. We integrate our new FK kernel into our proxy collision checker, Fastron, that previously showed significant speed improvements to collision checking and motion planning. With the new FK kernel, we realize a two-fold speedup in proxy collision check speed, 8 times less memory, and a boost in classification accuracy from 74% to over 95% for a 7 degrees-of-freedom robot arm compared to the previously-used radial basis function kernel. Compared to state-of-the-art geometric collision checkers, with the FK kernel, collision checks are now 9 times faster. To show the broadness of the approach, we apply Fastron FK in OMPL across a wide variety of motion planners, showing unanimously faster robot planning.
BibTeX
@article{das2020forward,
  author = {Nikhil Das and Michael C. Yip},
  title = {{Forward Kinematics Kernel for Improved Proxy Collision Checking}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2020},
  url = {https://openreview.net/forum?id=fJ-VUjqV-3m},
}
Figure