A system (200) executes a method (130-180) to produce an optimal path for any type of path planning application. In operation, the system (200) constructs a configuration space node structure representing a discretized configuration space including a plurality of states characterized by one or more parameters, and augments the configuration space node structure with discrete parameter values explicitly quantifying each node of the configuration space node structure and/or with heuristic values serving as a search guide through a free space region of the discretized configuration space.