I have two quaternions that indicate the initial orientation of a four wheel robot, each one in relative to one reference systems.
The robot's orientation given by a quaternion q is not the same in the two reference systems: For one reference system the quaternion q1 might refer to the robot looking at positive x while the same quaternion components q1 in the second reference system might refer to the robot looking at the negative x.
I have two matrices which indicate the position of the robot in time in its correspondent reference system.
I want to find the correspondent points of the first matrix in to the second reference system. Each matrix is built with a different sensor, so the results will be similar but not exactly the same.
I think I should find the transformation from the first reference system to the second and then apply it for each point of the first matrix. How can I find this transformation? The translation part I think is clear, but the rotation not at all.
The solution proposed does not solve the problem, I think that the reason is because each system represents the robot in a different way.
In the initial one (A) the robot moving forward with no rotation would increase in the X axis. In the goal referential system (B) the robot moving forward with no rotation would increase in the Z axis. (See figure for more details)
First system (A) ______ |\ T \ | \_____\ z |B | | : y ^ \ | R | \| \|____| +--> x Second system (B) ______ |\ T \ | \_____\ x |B | | : ^ \ | R | | \|____| +--> z \ y R: Right side B: Back side T: Top
I think I have to apply an extra rotation to change the initial quaternion that belongs to the first system to be in accordance with the second system before applying the transformation of your post.
One rotation of 180 degrees around x followed by one of 90 around y. Would rotate from A to B
This is how I tried to solve it:
# Quaternion to adjust reference system first_quat = make_quaternion(unitary_x, pi) # Generates the quaternion that rotates pi around X second_quat = make_quaternion(unitary_y, pi/2.0) # Generates the quaternion that rotates pi/2 around Y composed_fs_q = first_quat*second_quat # Quaternion to rotate from one reference system to the other quaternion_ini_A = quaternion_ini_A*composed_fs_q A2B_quaternion = quaternion_ini_B*(quaternion_ini_A.inverse())
A2B_quaternion is the quaternion that I use for the rotation but still doesn't perform the right rotation. Any idea?