optim finished, i hope | later : get the code better and smaller
fix bug in optim and remove useless function
fix bug with fixed objects
fix bug in problem
update rbdl and collisions