refactor: Implement get position in Trajectory class
refactor: Implement get position in Trajectory class

Implement method on superclass, and refactor subclasses
bit0001/trajectory_tracking,bit0001/trajectory_tracking