Assume that you need to plan cyclic motions for a redundant robotic system that must perform a repetitive task in the presence of obstacles. This open problem, which we call C-TCMP (Cyclic Task-Constrained Motion Planning), is an extension of the basic TCMP problem. For its solution, we have developed a novel control-based randomized planner which produces cyclic, collision-free paths in configuration space and guarantees continuous satisfaction of the task constraints. Our approach is illustrated in the figure above (right). In summary, a bidirectional search of the task-constrained configuration space C_task is performed by growing two Rapidly-exploring Random Trees (RRTs). In view of the constraint represented by the desired task path, tree extension in our planner is obtained via a control-based variation of the basic RRT mechanism. When the two trees become sufficiently near, they are joined by a feedback-based loop closure procedure.
The proposed C-TCMP planner has been implemented in Kite, a cross-platform motion planning software produced by Kineo CAM. We report planning results for three scenarios of increasing difficulty.
3R planar manipulator
In the first scenario, a 3R planar manipulator must move its tip along an elliptical path while avoiding three small obstacles. The following figure shows a cyclic solution which was computed by our planner. The mean and the maximum value of the task error norm over the whole path are, respectively, 0.0729 mm and 0.1354 mm.
KUKA LWR-IV robot: first scenario
In the second scenario, the KUKA LWR-IV must draw a circle on a whiteboard while avoiding collisions with the table on which it is mounted, the wall, the whiteboard itself (simple contact with the pen is obviously allowed) and an articulated lamp protruding from the wall. Self-collisions are also avoided. The solution computed by our planner is shown below. The task error is again negligible (mean value 0.076 mm and maximum value 0.1814 mm).
KUKA LWR-IV robot: second scenario
The third experiment refers to a similar scenario (draw an ellipse on the whiteboard) but the LWR-IV is now placed on the opposite side of the wall and must maneuver through a small window to reach the whiteboard. On the solution shown below, the mean and the maximum task errors are 0.0275 mm and 0.0418 mm.
Video clip
The three experiments are shown integrally in the video below. For comparison, the video also shows the path produced by simple pseudoinverse control, which is not cyclic — as expected — and also leads to collisions with the obstacles.
[1] M. Cefalo, G. Oriolo, M. Vendittelli, Planning Safe Cyclic Motions under Repetitive Task Constraints. 2013 IEEE Int. Conf. on Robotics and Automation (ICRA 2013), Karlsruhe, Germany, pp. 3807-3812, 2013 [pdf].