Update Cmake to detect Eigen properly
Improve communication reliability
Update robot parameters