Patent ID: 8843235

Claim:
A method of controlling a robot comprising a processor, a first manipulator and a second manipulator, the method comprising: receiving a trajectory plan comprising a plurality of sequential motion segments, wherein each motion segment corresponds to a component of the first manipulator or a component of the second manipulator, and the trajectory plan is operable to be executed by the robot to sequentially control the first manipulator and the second manipulator over a time period; determining, automatically by the processor, a moveable motion segment within the trajectory plan; shifting, automatically by the processor, the moveable motion segment and motion segments subsequent to the moveable motion segment backward in time to a shifted time such that one or more unshifted segments of the trajectory plan occur at a same time as one or more shifted segments, thereby generating an optimized trajectory plan; and controlling the robot according to the optimized trajectory plan such that one or more components of the first manipulator are moved concurrently with one or more components of the second manipulator.