Added Ach and fixed indexing
Merged with the latest
Fixed trajectory copying