I have a robotic arm centered at origin which I want to move from point A to point B. The robotic arm has an initial orientation matrix Rm and initial rotation matrix R. I also have a function that can compute the Quaternion to apply when going from point A to point B.
If I don't consider any rotation and orientation I can find with no problem the Quaternion to go from A to B. However, when I apply the initial rotation and orientation I'm unable to find the correct rotation. I consider to represent the points A and B in the new coordinate system according to this formula: $P_{o2} = M_2^{-1} * (o_1 - o_2 + M_1 * P_{o1})$ where $M_1, M_2$ are the matrices conformed by the axis of coordinate systems $1,2,$ $o_1, o_2$ are the origin of the coordinate systems and $P_{o1},P_{o2}$ the points in the different coordinate systems.
Assuming origins are at $0$, $M_1$ is identity matrix and $M_2$ is the initial rotation R, I transform points A,B to new coordinate system and then find the rotation Quaternion. However, the quaternion I find is not correct. I tried different matrices like $M_2 = Rm^{-1} * R * Rm$ but nothing seems to work. What I could be missing?
Some numbers: Robotic arm is already centered at origin $(0,0,0)$.
Point A = $(0.2929, 4.1605, 1.5514)$
Point B = $(0.5035, 3.8385, 2.1943)$
Initial rotation R expressed as a Quaternion$(w,x,y,z) = (0.975,0.181,-0.031,-0.122)$
And grount truth rotation to go from point A to B as Quaternion = $(0.996, 0.084,-0.014,-0.005)$
My orientation matrix is $Rm = \begin{bmatrix}0.999&&0.0089&&0.0104 \\ -0.0089 && -0.1514&& 0.9884 \\ 0.0104 && -0.9884 &&-0.1513\end{bmatrix}$