Stabilizing Manipulator Trajectory via Collision-Aware Optimization

Abstract

In this work, we develop an optimization-based solution for a manipulation planning among obstacles. This task is particularly challenging for collaborative manipulators when the operations include movements through the singular configurations. Trajectory optimization for this case requires a collision model, which allows converging to a valid solution from an invalid initial guess. By improving the initial trajectory approximation and optimizing collisions with advanced obstacle representation, it significantly enhances trajectory planning accuracy and efficiency. In our work, we propose a method based on the combining of obstacles. We use Octomap as the baseline (obstacles are represented as a set of cubes). This creates the need to calculate each cube when calculating the trajectory. Enlarging cubes to parallelepipeds allows one to obtain a collision-free trajectory faster. We have conducted a set of experiments with Octomap and with the representation of obstacles in the form of enlarged cubes. Experiments have shown that enlarging cubes to parallelepipeds reduces planning time and increases the success rate.