fix DOF limit issue
fixing stuff with trajopt. seems to work well now
enable sending to ROS