merged, id_solver ->CartToJnt(..) now is finding a solution