Added assertions (using :pre conditions) to trajectory functions