Method for Determining Trajectories Manipulators to Avoid Obstacles

A method optimizes a trajectory for a motion of a manipulator avoiding at least one obstacle. The method determines a cost function representing the motion of the manipulator from an initial position to a final position, wherein an optimization of the cost function is subject to spatial non-convex constraints due to avoidance of a region of the obstacle located between the initial position and the final position; and selects a set of locations on the trajectory representing positions of the manipulator at different times. Next, for each location in the set of locations, at least one linear convex constraint defining an interval of values for a projection of a position of the location on an axis of a direction of the motion of the manipulator is determined to produce a set of linear convex constraints and the cost function is optimized subject to the set of linear convex constraints.