added eigen stuff
found violating trajectory