The optimization of cost function representing a movement of the mass by the multi-motor control system from an initial point to a final point is subject to non-convex constraints due to avoidance of a region of the obstacle located between the initial point and the final point. Hence, the method determines a union of convex regions connecting the initial point with the final point, such that each convex region does not intersect the region of the obstacle, a convex constraint for each convex region and a set of switching times when the trajectory crosses boundaries of the convex regions. The method optimizes the cost function by jointly updating the positions and the set of switching times to produce an optimal trajectory of the movement. The optimizing is subject to the set of convex constraints applied independently for each corresponding convex region.