A method of planning a path for an articulated arm of robot includes generating a directed graph corresponding to a joint space of the articulated arm. The directed graph includes a plurality of nodes each corresponding to a joint pose of the articulated arm. The method also includes generating a planned path from a start node associated with a start pose of the articulated arm to an end node associated with a target pose of the articulated arm. The planned path includes a series of movements along the nodes between the start node and the end node. The method also includes determining when the articulated arm can travel to a subsequent node or the target pose, terminating a movement of the articulated arm towards a target node, and initiating a subsequent movement of the articulated arm to move directly to the target pose or the subsequent node.