change test to two joints
deleted files
added robust position_control constraint package
added velocity profiler functionality