{"id":1862,"date":"2016-12-13T15:35:18","date_gmt":"2016-12-13T07:35:18","guid":{"rendered":"http:\/\/www.zacobria.com\/universal-robots-knowledge-base-tech-support-forum-hints-tips\/?page_id=1862"},"modified":"2023-01-21T05:38:27","modified_gmt":"2023-01-21T05:38:27","slug":"python-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\/python-code-example-of-converting-rpyeuler-angles-to-rotation-vectorangle-axis-for-universal-robots\/","title":{"rendered":"Python code example of converting RPY\/Euler angles to Rotation Vector\/Angle Axis for Universal-Robots."},"content":{"rendered":"

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

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

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

Mind you that when all input values are 0, there will be a division by zero error at this line:
\n# multi = 1 \/ (2 * math.sin(theta))
\nIn this case theta will be 0 and the sin of 0 is 0, so you\u2019ll probably have to make a special case for this and set the rx, ry and rz to 0 directly.<\/p>\n

import math\nimport numpy as np\n\nroll = 2.6335\npitch = 0.4506\nyaw = 1.1684\n\nprint \"roll = \", roll\nprint \"pitch = \", pitch\nprint \"yaw = \", yaw\nprint \"\"\n\nyawMatrix = np.matrix([\n[math.cos(yaw), -math.sin(yaw), 0],\n[math.sin(yaw), math.cos(yaw), 0],\n[0, 0, 1]\n])\n\npitchMatrix = np.matrix([\n[math.cos(pitch), 0, math.sin(pitch)],\n[0, 1, 0],\n[-math.sin(pitch), 0, math.cos(pitch)]\n])\n\nrollMatrix = np.matrix([\n[1, 0, 0],\n[0, math.cos(roll), -math.sin(roll)],\n[0, math.sin(roll), math.cos(roll)]\n])\n\nR = yawMatrix * pitchMatrix * rollMatrix\n\ntheta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1) \/ 2)\nmulti = 1 \/ (2 * math.sin(theta))\n\nrx = multi * (R[2, 1] - R[1, 2]) * theta\nry = multi * (R[0, 2] - R[2, 0]) * theta\nrz = multi * (R[1, 0] - R[0, 1]) * theta\n\nprint rx, ry, rz\n\n<\/pre>\n
Testing angle calculation and compare to robot values.<\/strong><\/p>\n
RPY values tested<\/p>\n
roll = 0
\npitch = 180
\nyaw = 20<\/p>\n
import math\nimport numpy as np\n\n#roll = 2.6335\n#pitch = 0.4506\n#yaw = 1.1684\n\nroll = 0\npitch = 180\nyaw = 20\n\nprint \"roll = \", roll\nprint \"pitch = \", pitch\nprint \"yaw = \", yaw\nprint \"\"\n\nyawMatrix = np.matrix([\n[math.cos(yaw), -math.sin(yaw), 0],\n[math.sin(yaw), math.cos(yaw), 0],\n[0, 0, 1]\n])\n\npitchMatrix = np.matrix([\n[math.cos(pitch), 0, math.sin(pitch)],\n[0, 1, 0],\n[-math.sin(pitch), 0, math.cos(pitch)]\n])\n\nrollMatrix = np.matrix([\n[1, 0, 0],\n[0, math.cos(roll), -math.sin(roll)],\n[0, math.sin(roll), math.cos(roll)]\n])\n\nR = yawMatrix * pitchMatrix * rollMatrix\n\ntheta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1) \/ 2)\nmulti = 1 \/ (2 * math.sin(theta))\n\nrx = multi * (R[2, 1] - R[1, 2]) * theta\nry = multi * (R[0, 2] - R[2, 0]) * theta\nrz = multi * (R[1, 0] - R[0, 1]) * theta\n\nprint rx, ry, rz<\/pre>\n
\"universal<\/a><\/p>\n
\"universal<\/a><\/p>\n
Rx – Ry and Rz show the same values as calculated by the Python program.<\/p>\n
\"universal<\/a><\/p>\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