Robots, computer program products, and methods for trajectory plan optimization are disclosed. In one embodiment, a method of controlling a robot having a first manipulator and a second manipulator includes receiving a trajectory plan including a plurality of sequential motion segments. The method further includes determining a moveable motion segment, and shifting 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 segment segments. The method may further include 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.