TP updates to fix velocity violations

After a bunch of tweaks and testing with Sam, we've finally eliminated the velocity violations from circular arcs. These fixes should also address the RT errors that Mark experienced.