This PR adds minor improvements to the joint trajectory server:
In trajectory mode, the joint trajectory server expects trajectory goals to have a time stamp of 0.0, to keep in line with the present capabilities. Goals with timestamps in the future or in the past are aborted and a warning is logged by the driver
In position mode, added support for controlling the wrist pitch and roll joints through command groups
Added preliminary tests for smooth preemption to test goal replacement, goal queuing, and waypoint merging
This PR adds minor improvements to the joint trajectory server: