Makes sure frame placements are correct for the first ik iteration

We did not actually ensure that the initial placement of the first loop was reasonable / according to q0 in the ik. This was not a major problem until I realized that pinocchio is initializing some linear joints in the area of 1e22, which led to numerical issues :)