Added function SetPosition: Set position of the joints