Added some current position&velocity init helper functions for testing (wip)