continue rearrangement
Add screw_add: A (+) B = screw_log(screw_exp(A)*screw_exp(B))
Critical: fixed a big in screw_log to deal with theta=pi
Critical: Rewrote screw_log to be more robust
Numerical integration of the sample trajectory is tested