Split Trajectory class into TrajectoryBase, CartesianTrajectory, and JointTrajectory

[bpapaspyros]

The existing Trajectory class has quite a few nice elements in my opinion and it would be worth reusing quite a few of them.

Notably, the "issues" that it has is that it defines and manages the following data structures:

  std::deque<StateT> points_;
  std::deque<std::chrono::nanoseconds> times_;
  std::string reference_frame_; ///< name of the reference frame
  std::vector<std::string> joint_names_; ///< 

What could change:

• points_ is a State type which means we are encapsulating a bit more than we perhaps need.

We could have a flatter representation of the Cartesian/Joint points in the derived classes.

• times_ I think this is fine (at the point of writing at least)
• reference_frame_ not really applicable for Joint types, should be pushed down to CartesianTrajectory
• joint_names_ this time, not applicable to Cartesian types, should be pushed down to JointTrajectory

[bpapaspyros]

fyi @domire8

[domire8]

Agree with the points, I think instead of the State we should save Eigen vectors of correct size (25 for Cartesian and 4*nb_joints for joint)