Planning deployment of a medical robot based on concentric cannulas takes into account multiple radii of curvature. The radii of curvature are dependent on tube diameter. Tubes of smaller diameter can have tighter radii of curvature. Planning also takes into account moment of inertia and elasticity of tubes. For the purposes of planning, an A algorithm is used for cost wave propagation together with a configuration space, a cost metric, and a neighborhood. The neighborhood is adaptive. The adaptive neighborhood can be different for each node in the configuration space data structure and depends on curvature affecting properties of individual tubes used to achieve a path from a most distal point to a most proximal point within a body to be examined.
