Improved velocity joint trajectory controller