{"id":1874,"date":"2016-12-13T15:51:12","date_gmt":"2016-12-13T07:51:12","guid":{"rendered":"http:\/\/www.zacobria.com\/universal-robots-knowledge-base-tech-support-forum-hints-tips\/?page_id=1874"},"modified":"2023-01-21T05:19:15","modified_gmt":"2023-01-21T05:19:15","slug":"c-code-example-of-converting-rpyeuler-angles-to-rotation-vectorangle-axis-for-universal-robots","status":"publish","type":"page","link":"https:\/\/www.zacobria.com\/universal-robots-knowledge-base-tech-support-forum-hints-tips-cb2-cb3\/index.php\/c-code-example-of-converting-rpyeuler-angles-to-rotation-vectorangle-axis-for-universal-robots\/","title":{"rendered":"C# code Example of converting RPY\/Euler angles to Rotation Vector\/Angle Axis for Universal-Robots."},"content":{"rendered":"

Visit Zacobria Webshop<\/a><\/p>\n

C# code Example of converting RPY\/Euler angles to Rotation Vector\/Angle Axis for Universal-Robots.<\/strong><\/p>\n

C# code Example of converting RPY\/Euler angles to Rotation Vector\/Angle Axis for Universal-Robots.<\/p>\n

Here\u2019s my C# implementation based on Erwin\u2019s math. I noticed you have to change the theta value depending on the sign of your roll. At least this works for me\u2026 please point out any flaws. Thanks.
\nGiven a direction vector, convert that to a roll and a yaw and convert that to a rotation vector:
\nLink to code.<\/a><\/p>\n

\/\/\/ <summary>\n\/\/\/ Gets the roll pitch and yaw from a direction vector. 0,0,0 is up.\n\/\/\/ <\/summary>\n\/\/\/ <param name=\"d\">Direction vector. Must have a length of 1<\/param>\npublic Vector3 ToRollPitchYaw(Vector3 d)\n{\n d = Extensions.Normalize(d);\n double roll, pitch = 0, yaw = 0;\n double pi = Math.PI;\n roll = Math.Acos(d.Z);\n if (d.X == 0 && d.Y == 0)\n {\n yaw = 0;\n }\n else\n {\n Vector3 noZ = new Vector3 { X = d.X, Y = d.Y, Z = 0 };\n noZ = Extensions.Normalize(noZ);\n if (d.X <= 0)\n yaw = -Math.Acos(-noZ.Y);\n else\n yaw = Math.Acos(-noZ.Y);\n }\n if (pitch < -pi)\n pitch = 2 * pi + pitch;\n else if (pitch > pi)\n pitch = pitch - 2 * pi;\n\n Vector3 rpy = new Vector3 { X = (float)roll, Y = (float)pitch, Z = (float)yaw };\n return rpy;\n}\n\n\/\/\/ <summary>\n\/\/\/ Converts a roll pitch yaw vector to a rotation vector.\n\/\/\/ <\/summary>\n\/\/\/ <param name=\"d\">A vector where X=Roll, Y=Pitch, Z=Yaw<\/param>\n\/\/\/ <returns>A rotation vector with rx, ry and rz used to rotate the TCP of UR10<\/returns>\npublic Vector3 ToRotVector(Vector3 rpy)\n{\n float roll = rpy.X;\n float pitch = rpy.Y;\n float yaw = rpy.Z;\n if (roll == 0 && pitch == 0 && yaw == 0)\n return new Vector3 {X=0, Y=0, Z=0};\n Matrix3 RollM = new Matrix3();\n RollM.M00 = 1; RollM.M01 = 0; RollM.M02 = 0;\n RollM.M10 = 0; RollM.M11 = Math.Cos(roll); RollM.M12 = -Math.Sin(roll);\n RollM.M20 = 0; RollM.M21 = Math.Sin(roll); RollM.M22 = Math.Cos(roll);\n\n Matrix3 PitchM = new Matrix3();\n PitchM.M00 = Math.Cos(pitch); PitchM.M01 = 0; PitchM.M02 = Math.Sin(pitch);\n PitchM.M10 = 0; PitchM.M11 = 1; PitchM.M12 = 0;\n PitchM.M20 = -Math.Sin(pitch); PitchM.M21 = 0; PitchM.M22 = Math.Cos(pitch);\n\n Matrix3 YawM = new Matrix3();\n YawM.M00 = Math.Cos(yaw); YawM.M01 = -Math.Sin(yaw); YawM.M02 = 0;\n YawM.M10 = Math.Sin(yaw); YawM.M11 = Math.Cos(yaw); YawM.M12 = 0;\n YawM.M20 = 0; YawM.M21 = 0; YawM.M22 = 1;\n\n Matrix3 Rot = new Matrix3();\n\n \/\/rot = yaw * pitch * roll\n Rot = Matrix3.Dot(YawM, Matrix3.Dot(PitchM, RollM));\n\n double rotSum = Rot.M00 + Rot.M11 + Rot.M22 - 1;\n double alpha = Math.Acos(rotSum \/ 2);\n double theta = 0;\n if (roll >= 0)\n theta = alpha;\n else\n theta = 2 * Math.PI - alpha;\n double my = 1d \/ (2 * Math.Sin(theta));\n\n double rx = my * (Rot.M21 - Rot.M12) * theta;\n double ry = my * (Rot.M02 - Rot.M20) * theta;\n double rz = my * (Rot.M10 - Rot.M01) * theta;\n\n Vector3 rotationVector = new Vector3();\n rotationVector.X = (float)rx;\n rotationVector.Y = (float)ry;\n rotationVector.Z = (float)rz;\n return rotationVector;\n}\n\n<\/pre>\n
Disclaimer:<\/strong> While the Zacobria Pte. Ltd. believes that information and guidance provided is correct, parties must rely upon their skill and judgement when making use of them. Zacobria Pte. Ltd. assumes no liability for loss or damage caused by error or omission, whether such an error or omission is the result of negligence or any other cause. Where reference is made to legislation it is not to be considered as legal advice. Any and all such liability is disclaimed.<\/p>\n
If you need specific advice (for example, medical, legal, financial or risk management), please seek a professional who is licensed or knowledgeable in that area.<\/p>\n
Author:
\nBy Zacobria Lars Skovsgaard<\/a>
\nAccredited 2015-2018 Universal Robots support Centre and Forum.<\/p>\n
<\/div>\n
<\/div>\n