Update costOptimization.hpp
Update costOptimization.cpp
Update smoothing.cpp
Update smoothing.hpp
Merge branch 'master' of github.com:jmainpri/libmove3d-planners into develop_jim