Safety-Critical Manipulation for Collision-Free Food Preparation

Andrew Singletary, William Guffey, Tamás G. Molnár, Ryan W. Sinnet, Aaron D. Ames

TL;DR

This paper introduces a novel framework using Control Barrier Functions (CBFs) to ensure robotic manipulators perform food preparation tasks safely, even in dynamic and complex environments. By dynamically modifying pre-computed trajectories in real-time, the system achieves provably collision-free motion, significantly improving robustness and reducing computation time compared to traditional re-planning methods.

What Problem Does This Paper Address?

Automating food preparation in high-throughput environments presents a significant challenge for robotic systems. These environments are often highly dynamic, with objects like food baskets or equipment frequently shifting positions. Robots need to execute precise, rapid movements, but any deviation from planned paths due to these environmental changes can lead to collisions. Traditional motion planning algorithms, while capable of generating safe trajectories, can be computationally expensive and slow to adapt to these real-time environmental changes. This paper addresses the need for a computationally efficient and robust method to ensure safety by dynamically filtering and modifying existing trajectories to avoid collisions in complex, real-world scenarios like automated cooking.

Methods

The core of the proposed solution lies in the application of Control Barrier Functions (CBFs). CBFs are mathematical tools that provide formal safety guarantees by ensuring that a system's state remains within a predefined "safe set." The authors leverage CBFs to modify desired robot trajectories.

Here's a breakdown of their approach:

  • Kinematic Safety: Initially, CBFs are applied to the robot's kinematic model (its joint positions and velocities). This involves defining a "safe set" based on the robot's configuration space and using a CBF to ensure the robot's end-effector (or tool) stays a safe distance from obstacles. This is achieved by formulating a quadratic program (QP) that minimally adjusts a desired velocity command to a safe velocity command, satisfying the CBF constraint.
  • Formal Safety Guarantees for Full Dynamics: A key theoretical contribution is proving that safety achieved at the kinematic level extends to the robot's full-order dynamics (including inertia, Coriolis forces, and gravity). This is accomplished by assuming a "good" low-level velocity tracking controller exists, which can accurately follow the safe velocity commands generated by the CBF.
  • Distance Functions for Complex Environments: To handle the intricate collision environments in food preparation, the paper utilizes signed distance functions. These functions not only measure the distance between the robot and obstacles but also indicate penetration if a collision occurs. This allows for a more nuanced definition of the "safe set."
  • Trajectory Filtering Algorithm: Instead of re-planning from scratch, the system filters pre-computed reference trajectories. It searches a cache of previously generated trajectories and their associated planning scenes. Based on the similarity of the current robot state and planning scene to those in the cache, it either selects a similar trajectory to filter with CBFs or, if no suitable match is found, resorts to traditional re-planning.

Datasets

Not discussed in the paper. The authors mention using a cache of previously computed trajectories and planning scenes, but these are generated by their system rather than being external datasets.

Key Insights and Ideas

  • Bridging Kinematics and Dynamics: The formal proof that kinematic safety guarantees translate to full-order dynamics is a crucial insight. It allows for computationally lighter kinematic planning while still ensuring safety for the complex, real-world robot dynamics.
  • Real-time Adaptation with Pre-computation: The approach cleverly combines the efficiency of pre-computed trajectories with the adaptability of real-time safety filtering. This avoids the computational burden of full re-planning in dynamic environments.
  • CBFs as a "Safety Filter": The paper frames CBFs not as a primary planner but as a highly effective "safety filter" that minimally modifies existing plans, preserving their intended behavior while guaranteeing safety.
  • Practicality in Complex Scenarios: The successful application to a real-world cooking environment, with its dense obstacles and dynamic elements, demonstrates the practical utility of the proposed CBF-based approach.

Key Results

  • Provable Safety: The framework provides formal safety guarantees, ensuring collision-free motion for both kinematic and full-order dynamic models.
  • Computational Efficiency: The CBF filtering approach achieved an average computation time of 2 ms per CBF call, leading to an average behavior computation time of 223 ms. This is a significant improvement over TrajOpt's average computation time of 5923 ms.
  • Robustness: The system successfully produced collision-free trajectories in all 100 test cases, even with an artificially limited cache of reference trajectories.
  • Minimal Modification: The CBF filter updates the trajectory every 10 ms, which is fast enough to eliminate the need for an additional local planner.
  • Hardware Validation: The framework was successfully demonstrated on a full-scale robotic manipulator in a real-world cooking environment.

Limitations and Future Work

  • Dependency on Tracking Controller: The formal safety guarantees for full-order dynamics rely on the assumption of a good low-level velocity tracking controller. The performance of the system is thus dependent on the quality of this underlying controller.
  • Cache Size Saturation: While the cache size can saturate at around 200 stored behaviors, the paper suggests that in a commercial setting with a more populated cache, the CBF would have even more prior trajectories to choose from, leading to smaller modifications.
  • Future Work: The paper implies that further exploration of more complex environments and behaviors would be beneficial. Additionally, the integration of more sophisticated cache management strategies and potentially learning-based approaches for trajectory selection could be future directions.

Summary updated

Abstract

Recent advances allow for the automation of food preparation in high-throughput environments, yet the successful deployment of these robots requires the planning and execution of quick, robust, and ultimately collision-free behaviors. In this work, we showcase a novel framework for modifying previously generated trajectories of robotic manipulators in highly detailed and dynamic collision environments using Control Barrier Functions (CBFs). This method dynamically re-plans previously validated behaviors in the presence of changing environments -- and does so in a computationally efficient manner. Moreover, the approach provides rigorous safety guarantees of the resulting trajectories, factoring in the true underlying dynamics of the manipulator. This methodology is extensively validated on a full-scale robotic manipulator in a real-world cooking environment, and has resulted in substantial improvements in computation time and robustness over re-planning.
BibTeX
@article{singletary2022safety,
  author = {Andrew Singletary and William Guffey and Tamás G. Molnár and Ryan W. Sinnet and Aaron D. Ames},
  title = {{Safety-Critical Manipulation for Collision-Free Food Preparation}},
  journal = {IEEE RA-L},
  year = {2022},
  doi = {10.1109/lra.2022.3192634},
}
Figure