Second commit, code refactoring (change every call back linked to modify trajectory) + code formatting