Made some nicer tools for smooth trajectory bending.
Adding simple set of tests for the klampt package.