How do I convert link parameters and angles (in kinematics) into transformation matrices in programming logic? - Robotics Stack Exchange most recent 30 from robotics.stackexchange.com 2019-11-15T00:15:24Z https://robotics.stackexchange.com/feeds/question/940 https://creativecommons.org/licenses/by-sa/4.0/rdf https://robotics.stackexchange.com/q/940 9 How do I convert link parameters and angles (in kinematics) into transformation matrices in programming logic? Grace https://robotics.stackexchange.com/users/916 2013-02-18T11:18:22Z 2013-03-18T15:43:13Z <p>I'm doing robotics research as an undergraduate, and I understand the conceptual math for the most part; however, when it comes to actually implementing code to calculate the forward kinematics for my robot, I am stuck. I'm just not getting the way the book or websites I've found explain it.</p> <p>I would like to calculate the X-Y-Z angles given the link parameters (Denavit-Hartenberg parameters), such as the <a href="https://i.stack.imgur.com/j6Cf6.png" rel="nofollow noreferrer">following</a>:</p> <p>$$\begin{array}{ccc} \bf{i} &amp; \bf{\alpha_i-1} &amp; \bf{a_i-1} &amp; \bf{d_i} &amp; \bf{\theta_i}\\ \\ 1 &amp; 0 &amp; 0 &amp; 0 &amp; \theta_1\\ 2 &amp; -90^{\circ} &amp; 0 &amp; 0 &amp; \theta_2\\ 3 &amp; 0 &amp; a_2 &amp; d_3 &amp; \theta_3\\ 4 &amp; -90^{\circ} &amp; a_3 &amp; d_4 &amp; \theta_4\\ 5 &amp; 90^{\circ} &amp; 0 &amp; 0 &amp; \theta_5\\ 6 &amp; -90^{\circ} &amp; 0 &amp; 0 &amp; \theta_6\\ \end{array}$$</p> <p>I don't understand how to turn this table of values into the proper transformation matrices needed to get $^0T_N$, the Cartesian position and rotation of the last link. From there, I'm hoping I can figure out the X-Y-Z angle(s) from reading my book, but any help would be appreciated.</p> https://robotics.stackexchange.com/questions/940/-/945#945 6 Answer by DaemonMaker for How do I convert link parameters and angles (in kinematics) into transformation matrices in programming logic? DaemonMaker https://robotics.stackexchange.com/users/177 2013-02-18T23:24:21Z 2013-02-19T16:48:57Z <p>The <a href="http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters" rel="nofollow">DH Matrix</a> section of the DH page on wikipedia has the details.</p> <p>Basically you want to use the information in your table to create a set of homogeneous transformation matrices. We do so because homogeneous transformations can be multiplied to find the relation between frames seperated by one or more others. For example, $^0T_1$ represents the transformation from frame 1 to frame 0 while $^1T_2$ represents the transformation from frame 2 to frame 1. By multiplying them we get the transformation from frame 2 to frame 0, i.e. $^0T_2 = ^0T_1^1T_2$.</p> <p>An easy way to create each of the transformations is to make a homogeneous transformation or homogeneous rotation matrix for each column in the table and multiply them together. For example, the transformation from 1 to 0 (e.g. $^{i-1}T_i, i = 1$) is</p> <p>$^0T_1 = Trans(d_1)*Rot(\theta_1)*Trans(a_2)*Rot(\alpha_2)$</p> <p>where</p> <p>$Trans(d_1) = \begin{bmatrix}1 &amp; 0 &amp; 0 &amp; 0 \\ 0 &amp; 1 &amp; 0 &amp; 0 \\ 0 &amp; 0 &amp; 1 &amp; \bf{d_1 = 0} \\ 0 &amp; 0 &amp; 0 &amp; 1 \end{bmatrix},$</p> <p>$Rot(\theta_1) = \begin{bmatrix} \text{cos}(\bf{\theta_1}) &amp; - \text{sin}(\bf{\theta_1}) &amp; 0 &amp; 0 \\ \text{sin}(\bf{\theta_1}) &amp; \text{cos}(\bf{\theta_1}) &amp; 0 &amp; 0 \\ 0 &amp; 0 &amp; 1 &amp; 0 \\ 0 &amp; 0 &amp; 0 &amp; 1 \end{bmatrix},$</p> <p>$Trans(a_2) = \begin{bmatrix} 1 &amp; 0 &amp; 0 &amp; \bf{a_2 = 0} \\ 0 &amp; 1 &amp; 0 &amp; 0 \\ 0 &amp; 0 &amp; 1 &amp; 0 \\ 0 &amp; 0 &amp; 0 &amp; 1 \end{bmatrix},$</p> <p>$Rot(\alpha_2) = \begin{bmatrix} 1 &amp; 0 &amp; 0 &amp; 0 \\ 0 &amp; \text{cos}(\bf{\alpha_2 = 0}) &amp; -\text{sin}(\bf{\alpha_2 = 0}) &amp; 0 \\ 0 &amp; \text{sin}(\bf{\alpha_2 = 0}) &amp; \text{cos}(\bf{\alpha_2 = 0}) &amp; 0 \\ 0 &amp; 0 &amp; 0 &amp; 1 \end{bmatrix}$.</p> <p>In this case</p> <p>$^0T1 = Rot(\theta_1)$.</p> <p>Once you have all your transformations you multiply them togther, e.g.</p> <p>$^0T_N = ^0T_1*^1T_2...^{N-1}T_N$.</p> <p>Finally you can read the displacement vector out of the homogenous transform $^0T_N$ (i.e. $d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$). Similarly you can read out the rotation matrix from $^0T_N$ to find the X-Y-Z angles.</p>