Method for determining trajectories manipulators to avoid obstacles

A method for optimizing a trajectory for a motion of a manipulator avoiding at least one obstacle, comprising:

• determining 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;
• selecting a set of locations on the trajectory representing positions of the manipulator at different times;
• determining, 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 to produce a set of linear convex constraints;
• and
• optimizing the cost function subject to the set of linear convex constraints to optimize the trajectory, wherein steps of the method are performed by a processor.