I want to measure the end-effector position of the robot that I attached. I'm using both gy-25 and gy-521 for calculation of the angle. As shown in the picture, joint 1 has 2 DOF, one for spatial movements, and the second one for planar rotation. The angles of the planar movement of joint 1, measures by an encoder. Each of joint 2 and 3 has 1 planar DOF, and the angles are measured by gy-521 and gy-25. I want to measure the angular rotation around the first joint's spatial axis and planar rotation of joint 2 with gy-25. The planar rotation of joint 3 measures by gy-521.
If I rotate the first joint around its spatial axis or rotate the joint 2 in the plane, the values of the roll, pitch, and yaw of both gyroscopes change which means that the rotation is a combination rotation around multiple axes because the gyroscope reference frame is global, but the rotation isn't around one of that reference frame's axes. So I want to transform the angles which are in the global frame to be measured in the local frame of the sensor, but I don't know. Can anybody help me with that?
Thanks in advance
Edit:
I attached the new figure below. Z1, Z2, Z3, Z4 are the rotation axes. Joint 1 is for spatial rotation, and on the links 2 and 3, I attached two gyroscopes (gy-25 and gy-521) to measure the theta1 in joint 1, theta 3 in joint 3 and theta 4 in joint 4. Theta 2 is being measured with an encoder.
Forward kinematic:
X = bcos(theta1) + cos(theta1)(L1*cos(theta2) +L2*cos(theta3)+L3*cos(theta4))
In my real system, you should rotate the figure 90 degree CCW and with this configuration, if you fix joint 2, joint 3, and joint 4 and just rotate around joint 1 (theta1), then all the values of both gyroscopes change. So I need to transform the gyroscope values from their frame into the global frame so that I can measure theta 3, 4, and 1. I know I should use the transformation matrix, but I know how to calculate that based on the gyroscopes values.
