 # Untitled

a guest
Nov 30th, 2016
1,195
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. /// <summary>
2. /// Gets the roll pitch and yaw from a direction vector. 0,0,0 is up.
3. /// </summary>
4. /// <param name="d">Direction vector. Must have a length of 1</param>
5. public Vector3 ToRollPitchYaw(Vector3 d)
6. {
7.     d = Extensions.Normalize(d);
8.     double roll, pitch = 0, yaw = 0;
9.     double pi = Math.PI;
10.     roll = Math.Acos(d.Z);
11.     if (d.X == 0 && d.Y == 0)
12.     {
13.         yaw = 0;
14.     }
15.     else
16.     {
17.         Vector3 noZ = new Vector3 { X = d.X, Y = d.Y, Z = 0 };
18.         noZ = Extensions.Normalize(noZ);
19.         if (d.X <= 0)
20.             yaw = -Math.Acos(-noZ.Y);
21.         else
22.             yaw = Math.Acos(-noZ.Y);
23.     }
24.     if (pitch < -pi)
25.         pitch = 2 * pi + pitch;
26.     else if (pitch > pi)
27.         pitch = pitch - 2 * pi;
28.
29.     Vector3 rpy = new Vector3 { X = (float)roll, Y = (float)pitch, Z = (float)yaw };
30.     return rpy;
31. }
32.
33. /// <summary>
34. /// Converts a roll pitch yaw vector to a rotation vector.
35. /// </summary>
36. /// <param name="d">A vector where X=Roll, Y=Pitch, Z=Yaw</param>
37. /// <returns>A rotation vector with rx, ry and rz used to rotate the TCP of UR10</returns>
38. public Vector3 ToRotVector(Vector3 rpy)
39. {
40.     float roll = rpy.X;
41.     float pitch = rpy.Y;
42.     float yaw = rpy.Z;
43.     if (roll == 0 && pitch == 0 && yaw == 0)
44.         return new Vector3 {X=0, Y=0, Z=0};
45.     Matrix3 RollM = new Matrix3();
46.     RollM.M00 = 1; RollM.M01 = 0; RollM.M02 = 0;
47.     RollM.M10 = 0; RollM.M11 = Math.Cos(roll); RollM.M12 = -Math.Sin(roll);
48.     RollM.M20 = 0; RollM.M21 = Math.Sin(roll); RollM.M22 = Math.Cos(roll);
49.
50.     Matrix3 PitchM = new Matrix3();
51.     PitchM.M00 = Math.Cos(pitch); PitchM.M01 = 0; PitchM.M02 = Math.Sin(pitch);
52.     PitchM.M10 = 0; PitchM.M11 = 1; PitchM.M12 = 0;
53.     PitchM.M20 = -Math.Sin(pitch); PitchM.M21 = 0; PitchM.M22 = Math.Cos(pitch);
54.
55.     Matrix3 YawM = new Matrix3();
56.     YawM.M00 = Math.Cos(yaw); YawM.M01 = -Math.Sin(yaw); YawM.M02 = 0;
57.     YawM.M10 = Math.Sin(yaw); YawM.M11 = Math.Cos(yaw); YawM.M12 = 0;
58.     YawM.M20 = 0; YawM.M21 = 0; YawM.M22 = 1;
59.
60.     Matrix3 Rot = new Matrix3();
61.
62.     //rot = yaw * pitch * roll
63.     Rot = Matrix3.Dot(YawM, Matrix3.Dot(PitchM, RollM));
64.
65.     double rotSum = Rot.M00 + Rot.M11 + Rot.M22 - 1;
66.     double alpha = Math.Acos(rotSum / 2);
67.     double theta = 0;
68.     if (roll >= 0)
69.         theta = alpha;
70.     else
71.         theta = 2 * Math.PI - alpha;
72.     double my = 1d / (2 * Math.Sin(theta));
73.
74.     double rx = my * (Rot.M21 - Rot.M12) * theta;
75.     double ry = my * (Rot.M02 - Rot.M20) * theta;
76.     double rz = my * (Rot.M10 - Rot.M01) * theta;
77.
78.     Vector3 rotationVector = new Vector3();
79.     rotationVector.X = (float)rx;
80.     rotationVector.Y = (float)ry;
81.     rotationVector.Z = (float)rz;
82.     return rotationVector;
83. }
RAW Paste Data