computeCartesianPath waypoints double-up fix

computeCartesianPath appends full trajectories between waypoints when given a vector of waypoints. As trajectories include their endpoints, this leads to the combined trajectory being generated with duplicate points at waypoints, which can lead to pauses or stuttering.

This change skips the first point in trajectories generated between waypoints.
Added empty check.
Merge pull request #160 from mszarski/patch-1

Tested this - looks good.