Added function GetPosition: get position of the joints