SHARE
TWEET

Untitled

a guest Nov 30th, 2016 208 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
Top