Fixed joint angle unwrapping problem.
Added cost finding function.
Increased max_iter for fitting with regularized rotation.