Latest commit using Joint Trajectory Action Server. Not working very well. Find an older commit using moveit, but with joint commands? Can be improved later.