Generation of synchronized configuration space trajectories with workspace path constraints for an ensemble of robots

We present an approach to generate path-constrained synchronous motion for the coupled ensemble of robots. In this article, we refer to serial-link manipulators and mobile bases as robots. We assume that the relative motion constraints among the objects in the environment are given. We represent the motion constraints as path constraints and pose the problem of path-constrained synchronous trajectory generation as a non-linear optimization problem. Our approach generates configuration space trajectories for the robots to manipulate the objects such that the given motion constraints among the objects are satisfied. We present a method that formulates the problem as a discrete parameter optimization problem and solves it using successive constraint refinement techniques. The method adaptively selects the parametric representation of the configuration variables for a given scenario. It also generates an approximate solution as the starting point for the successive constraint refinement stages to reduce the computation time. We discuss in detail why successive constraint refinement strategies are useful for solving this class of problems. We demonstrate the effectiveness of the proposed method on challenging test cases in simulation and physical environments with high-degree-of-freedom robotic systems.