Publish joint trajectories

Publish joint trajectories for the next 3 steps on /planner/trajectory