Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

TOCALib: Optimal control library with interpolation for bimanual manipulation and obstacles avoidance

Yulia Danik, Dmitry Makarov, Aleksandra Arkhipova, Sergei Davidenko, Aleksandr Panov

TL;DR

TOCALib addresses the challenge of generating reliable, collision-aware motion libraries for two-arm manipulation under full kinodynamics. It fuses a nonlinear-programming approach solved with FROST/IPOPT and a differentiable collision model (DCOL) to produce optimal trajectories, stored on a grid and accessible via trilinear interpolation; Bézier-based local shaping further enables smooth interpolation. The method demonstrates high feasibility in shelf and moving-sphere scenarios, outperforming CHOMP in feasible solutions and enabling RL data generation. While computationally intensive, the interpolation framework provides a practical pathway for fast approximate planning and dataset creation for learning-based manipulation.

Abstract

The paper presents a new approach for constructing a library of optimal trajectories for two robotic manipulators, Two-Arm Optimal Control and Avoidance Library (TOCALib). The optimisation takes into account kinodynamic and other constraints within the FROST framework. The novelty of the method lies in the consideration of collisions using the DCOL method, which allows obtaining symbolic expressions for assessing the presence of collisions and using them in gradient-based optimization control methods. The proposed approach allowed the implementation of complex bimanual manipulations. In this paper we used Mobile Aloha as an example of TOCALib application. The approach can be extended to other bimanual robots, as well as to gait control of bipedal robots. It can also be used to construct training data for machine learning tasks for manipulation.

TOCALib: Optimal control library with interpolation for bimanual manipulation and obstacles avoidance

TL;DR

TOCALib addresses the challenge of generating reliable, collision-aware motion libraries for two-arm manipulation under full kinodynamics. It fuses a nonlinear-programming approach solved with FROST/IPOPT and a differentiable collision model (DCOL) to produce optimal trajectories, stored on a grid and accessible via trilinear interpolation; Bézier-based local shaping further enables smooth interpolation. The method demonstrates high feasibility in shelf and moving-sphere scenarios, outperforming CHOMP in feasible solutions and enabling RL data generation. While computationally intensive, the interpolation framework provides a practical pathway for fast approximate planning and dataset creation for learning-based manipulation.

Abstract

The paper presents a new approach for constructing a library of optimal trajectories for two robotic manipulators, Two-Arm Optimal Control and Avoidance Library (TOCALib). The optimisation takes into account kinodynamic and other constraints within the FROST framework. The novelty of the method lies in the consideration of collisions using the DCOL method, which allows obtaining symbolic expressions for assessing the presence of collisions and using them in gradient-based optimization control methods. The proposed approach allowed the implementation of complex bimanual manipulations. In this paper we used Mobile Aloha as an example of TOCALib application. The approach can be extended to other bimanual robots, as well as to gait control of bipedal robots. It can also be used to construct training data for machine learning tasks for manipulation.

Paper Structure

This paper contains 19 sections, 4 equations, 14 figures, 3 tables.

Table of Contents

  1. INTRODUCTION
  2. Related works
  3. Trajectory Planning for Manipulators
  4. Precomputed motion libraries
  5. Collision Detection
  6. PROBLEM STATEMENT
  7. MOTION LIBRARY DESIGN
  8. NLP Solver Framework
  9. Constraints
  10. Collision Detection Tool
  11. Interpolation
  12. Experiments
  13. Enhanced URDF Loading Mechanism
  14. Self-collision Constraints
  15. Collision with Static Objects
  16. ...and 4 more sections

Figures (14)

  • Figure 1: Adding a Collision Avoidance Constraint to the Aloha Manipulator Trajectory Optimization Problem
  • Figure 2: Interpolation errors for all joints of the Aloha manipulator when moving the manipulator from a given initial point to a final point located 40 cm away. The red dashed curve represents the interpolation results, while the solid blue line shows the exact solution obtained through optimization.
  • Figure 3: The comparison of real and computed joint angle trajectories
  • Figure 4: Collision-free trajectories of a manipulator
  • Figure 5: The experiment on a real Aloha (with a virtual sphere)
  • ...and 9 more figures