Patent ID: 8417491

Claim:
A system ( 1600 ) for determining at least one path for a non-holonomic physical tool ( 1604 ) having a directable tip ( 1605 ), comprising: at least one computer readable memory ( 1608 ) to store physical pose information of a plurality of nodes each at a different discrete location in a physical task space, said plurality of nodes including at least one goal node that corresponds to a desired physical goal pose of the directable tip of the non-holonomic physical tool when the directable tip ( 1605 ) of the non-holonomic physical tool is located at the goal node; and at least one digital data processing unit ( 1600 ) coupled to or including the at least one computer readable memory ( 1608 ), the at least one digital data processing unit ( 1600 ) including: a neighborhood generation module ( 730 ) to generate and store in the memory ( 1608 ) a neighborhood of a physical task space node comprising at least one neighbor node thereof, said physical task space node and neighbor node having at least one permissible transition direction therebetween; a transition cost calculation module ( 1603 ) to calculate and store in the memory ( 1608 ) the cost from the physical task space node to the at least one generated neighbor node along the at least one permissible transition direction therebetween; a cost wave propagation module ( 770 ) to propagate at least one calculated cost wave from the at least one goal node and assign to each physical task space node and store in the memory ( 1608 ) a cost value and a direction of transition corresponding to a physical least cost path from a physical pose of the directable tip ( 1605 ) corresponding to the physical task space node to the physical goal pose; and a path determination module ( 700 ) for determination and identification in the memory ( 1608 ) of a path ( 1606 ) comprising a sequence of physical task space nodes along the physical least cost path beginning at a start pose comprising a physical task space node corresponding to a physical natural/resting pose of the directable tip ( 1605 ) of the non-holonomic physical tool ( 1604 ) at a start location and following the physical least cost path therefrom in the permissible transition direction from the start node to successive nodes thereof to the goal node.