mofified setup, function sergio-torso-move-joints uses joint angles now