* add goPos()
* but get segmentation fault at updateVicon() now.