Roll Pitch Yaw in NACHI robot
-
linh_hn_vn -
December 24, 2022 at 12:02 PM -
Thread is Unresolved
-
-
First matrix: (374,0,630,30,60,90)
Second : (374,0,630,0,90,0)
(x,y,z,roll,pitch,yaw)
can you check if that is correct?
-
-
Second : (374,0,630,0,90,0)
Then the matrix would look like this (see rotation matrix)
If we are very close to pitch = 90 deg then we get this view
having pitch at 90 deg will give this result
Your conversion routine does not work for pitch = |90.0| deg
-
Then the matrix would look like this (see rotation matrix)
If we are very close to pitch = 90 deg then we get this view
having pitch at 90 deg will give this result
Your conversion routine does not work for pitch = |90.0| deg
I am sorry, I forgot to include additional check for pitch, because if pitch is +/-90 the calculation for roll and yaw cannot be applied, but this :
if (rotationMatrix[2, 0] < -0.9999999)
{
pitch = Math.PI / 2;
yaw = Math.Atan2(rotationMatrix[0, 1], rotationMatrix[0, 2]);
roll = 0;
}
// Check for singularity
else if (rotationMatrix[2, 0] > 0.9999999)
{
pitch = -Math.PI / 2;
yaw = Math.Atan2(-rotationMatrix[0, 1], -rotationMatrix[0, 2]);
roll = 0;
}
else
{
pitch = Math.Asin(-rotationMatrix[2, 0]);
yaw = Math.Atan2(rotationMatrix[2, 1] / Math.Cos(pitch), rotationMatrix[2, 2] / Math.Cos(pitch));
roll = Math.Atan2(rotationMatrix[1, 0] / Math.Cos(pitch), rotationMatrix[0, 0] / Math.Cos(pitch));
}
Pitch can be simplified like above.
I get correct results for your T02 matrix :(374,0,630,0,90,40)
-