Merge coupled_joints stuff in new_wbi_ID

@DanielePucci @francesco-romano  can we merge this in new_wbi_ID so we can reduce the branch madness?