This paper presents improvements in contact-before-motion planning for humanoid robots that enables to find a path in very constrained environments. Starting from our previous work, the main novelties are to use a rough trajectory to drive the search and as a criterion to find new contacts and generate the best nodes first. This way only few nodes are effectively explored, speeding up the planning process. We experience the algorithm on the real humanoid HRP-2 in a complex scenario.