public void GetPose()
{
VREPWrapper.simxGetObjectPosition(ClientId, HandleSick, HandleRelative, _pos, simx_opmode.oneshot_wait);
VREPWrapper.simxGetObjectOrientation(ClientId, HandleSick, HandleRelative, _ori, simx_opmode.oneshot_wait);
//mysterious formula
if (_ori[0] < 0)
_ori[1] = _ori[1] * -1 + (float)Math.PI / 2;
else _ori[1] = _ori[1] + (float)Math.PI + (float)Math.PI / 2;
Pose = new Pose((int)(_pos[0] * RobotControl.MapZoom), (int)(_pos[1] * RobotControl.MapZoom), 180.0 * _ori[0] / Math.PI);
}
