The source project of this merge request has been removed.
Jacobian of the SE3 residual is expressed in the right frame
This fixed an important bug in the computation of the SE3 cost. Additionally I added data for the SE3 error which is updated in updateSE3Error. This bug was reported in #28 (closed).
I noticed that the gradient reduction is much better (a beautiful exponential for the HyQ problem), even though it's not request to rotate.
Edited by Carlos Mastalli