Method for collision-free motion planning

A method for collision-free motion planning of a first manipulator in a first working space and a second manipulator in a second working space, the first working space and the second working space at least partially overlapping, the method comprising:

• importing a first dynamic roadmap for a first configuration space of the first manipulator, wherein the first dynamic roadmap includes a first search graph and a first mapping between the first working space and the first search graph;
• importing a second dynamic roadmap for a second configuration space of the second manipulator, wherein the second dynamic roadmap includes a second search graph and a second mapping between the second working space and the second search graph;
• and
• coordinating the motion of the first manipulator and the second manipulator based on the first dynamic roadmap and the second dynamic roadmap,
• wherein at least one of the first dynamic roadmap and the second dynamic roadmap is an extended dynamic roadmap including an extended search graph with nodes comprising additional time information, and
• wherein coordinating the motion of the first manipulator and the second manipulator is further based on the extended dynamic roadmap.

An apparatus comprising:

• a first manipulator in a first working space;
• a second manipulator in a second working space, wherein the first working space and the second working space at least partially overlap;
• and
• a control unit for dynamic motion planning, wherein the control unit is configured to perform:
• importing a first dynamic roadmap for a first configuration space of the first manipulator, wherein the first dynamic roadmap includes a first search graph and a first mapping between the first working space and the first search graph;
• importing a second dynamic roadmap for a second configuration space of the second manipulator, wherein the second dynamic roadmap includes a second search graph and a second mapping between the second working space and the second search graph;
• determining a first motion path for the first manipulator based on the first dynamic roadmap;
• determining a second motion path for the second manipulator based on the second dynamic roadmap;
• linking, to a coordination graph, at least one of (i) the first motion path and the second motion path, (ii) the first motion path and the second search graph, and (iii) the first search graph and the second search graph;
• and
• coordinating the motion of the first manipulator and the second manipulator based on the first dynamic roadmap and the second dynamic roadmap using the coordination graph.