update cmake EOD
update headers
tmp commit
on going work, prepare for F<double>
optim finished, i hope | later : get the code better and smaller
fix bug in optim and remove useless function
add print collisions
avoid computation of collision for fixed robot base