Path-lifting trajectory generator

Added path-lifting mechanism to trajectory generation.