Added endEfectorPosition function in Chain
Added costFun to Chain class
Added chain get num joints
Added working chain to Simulationsolver
Chain copy constructor, simulation solver finiteJacobian and wroking chain
Added updateAngles to the simulation solver, first working example