Fix error to use longer time horizon and modify the nodes on which the shoulder-to-contact cost apply