{"id":100,"date":"2014-07-03T21:30:28","date_gmt":"2014-07-03T21:30:28","guid":{"rendered":"http:\/\/www.anninaruest.com\/pie\/?p=100"},"modified":"2014-07-13T03:25:09","modified_gmt":"2014-07-13T03:25:09","slug":"inverse-kinematics-and-the-m100rak","status":"publish","type":"post","link":"https:\/\/www.anninaruest.com\/pie\/2014\/07\/inverse-kinematics-and-the-m100rak\/","title":{"rendered":"Forward Kinematics, Inverse Kinematics, and the M100RAK Robot Arm"},"content":{"rendered":"<div style=\"width: 400px;\" class=\"wp-video\"><video class=\"wp-video-shortcode\" id=\"video-100-1\" width=\"400\" height=\"225\" preload=\"metadata\" controls=\"controls\"><source type=\"video\/mp4\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/inverse_kinematics-Cellular.m4v?_=1\" \/><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/inverse_kinematics-Cellular.m4v\">http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/inverse_kinematics-Cellular.m4v<\/a><\/video><\/div>\n<p>The video above shows the M100RAK drawing a rudimentary pie chart onto a pie with a &#8220;FoodWriter&#8221;, a marker containing food coloring. The pie chart depicts the gender ratio among Google&#8217;s staff (women: 30%, men 70%).<\/p>\n<p>The food color marker does not draw well on chocolate, especially in hot weather. Tests with hard-drying <a href=\"http:\/\/www.foodnetwork.com\/recipes\/alton-brown\/royal-icing-recipe.html\" target=\"_blank\">royal icing<\/a> and\/or rice paper will follow.<\/p>\n<p>The marker is connected to aluminum piping with a spring. This means that it can draw on uneven surfaces.<br \/>\n<a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-5.18.45-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone size-medium wp-image-104\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-5.18.45-PM-300x99.png\" alt=\"Screen Shot 2014-07-03 at 5.18.45 PM\" width=\"300\" height=\"99\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-5.18.45-PM-300x99.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-5.18.45-PM-624x207.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-5.18.45-PM.png 686w\" sizes=\"auto, (max-width: 300px) 100vw, 300px\" \/><\/a><\/p>\n<p>The robot drawing itself is not yet optimal. If you compare it to the the perfectly round simulation below, you will see why.<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-3.28.25-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone size-medium wp-image-234\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-3.28.25-PM-300x218.png\" alt=\"Screen Shot 2014-07-03 at 3.28.25 PM\" width=\"300\" height=\"218\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-3.28.25-PM-300x218.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-3.28.25-PM-1024x745.png 1024w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-3.28.25-PM-624x454.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-03-at-3.28.25-PM.png 1568w\" sizes=\"auto, (max-width: 300px) 100vw, 300px\" \/><\/a><\/p>\n<p>The simulation is made up of points that the robot connects. The two points that are floating above are not a mistake. They make sure that when the pen touches down, it touches down almost vertically. If these points were not there, the marker would leave a trail.<\/p>\n<p>I don&#8217;t know yet why the robot draws more of a rectangle than a circle. It might be because the servos are slipping under the weight of the robot parts, or because I am falsely adapting the angles returned by the inverse kinematics function to the machine. The spacing between the gears is relatively wide, this might also negatively influence precision. In the meantime, here is an explanation of Inverse Kinematics and Forward Kinematics and how it relates to the code.<\/p>\n<p><strong>Kinematics &amp; Forward Kinematics: The short explanation<br \/>\n<\/strong><\/p>\n<p>Inverse Kinematics describes equations that produce angles to position a robotic arm on a specific xyz-coordinate. In other words: it is an algorithm (a function for example) that takes an xyz-coordinate as well as the length of the arm segments as parameters and returns the angles of the arm joints.<\/p>\n<p>Forward Kinematics does the opposite. It takes angles and the length of the arm segments as inputs and then outputs an xyz-coordinate.<\/p>\n<p>Inverse Kinematics and Forward Kinematics are used in robotics as well as in computer animation to animate objects in physical and in virtual spaces.<\/p>\n<p>All the code in this project is in Python and requires the SciPy and vPython libraries for math and visualization respectively. It also requires the serial library for interfacing with the SC-32.<\/p>\n<p>The Inverse Kinematics function in Python as well as a visualization function using vPython was written by <a href=\"http:\/\/people.brandeis.edu\/~yffily\/html\/index.html\" target=\"_blank\"> Yaouen Fily <\/a> specifically for this project.<\/p>\n<p>&nbsp;<\/p>\n<p><strong>Forward Kinematics<\/strong><\/p>\n<p>In order to get the arm to a certain point, we need an inverse kinematics function. However, to get there, it is easier to first derive the forward kinematics function. We will also need the forward kinematics function to draw the robot in 3D using vPython.<\/p>\n<p>To get started, look at the image below. a) depicts a 3D space. b) depicts a 3D space with an additional direction r. The unit vectors on the xyz axes are <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-d2305137cf2c5cf8143166355c34a8c0_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#120;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"18\" width=\"18\" style=\"vertical-align: 0px;\"\/>, <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-570ae1a74f37b2a06bce00411d270121_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#121;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"17\" style=\"vertical-align: -4px;\"\/>, <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-c51c2121f9d9b1df0753ef938df75094_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#122;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"18\" width=\"17\" style=\"vertical-align: 0px;\"\/>. Unit vectors are just vectors of length 1. Along r there is an additional unit vector <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-bc2676357805d64c513697884ed0b8a6_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#114;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"18\" width=\"17\" style=\"vertical-align: 0px;\"\/> with angle a1. Why is this vector important to our problem? Assuming that your targetThis could be the direction that the base of the robot arm rotates. This also makes things easier because now, we do not need the x and y axes anymore and can deal with this as a 2D trigonometry problem, not a 3D trigonometry problem in the following steps.<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-04-at-7.01.06-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone wp-image-127 size-large\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-04-at-7.01.06-PM-1024x515.png\" alt=\"Screen Shot 2014-07-04 at 7.01.06 PM\" width=\"625\" height=\"314\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-04-at-7.01.06-PM-1024x515.png 1024w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-04-at-7.01.06-PM-300x151.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-04-at-7.01.06-PM-624x314.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-04-at-7.01.06-PM.png 1810w\" sizes=\"auto, (max-width: 625px) 100vw, 625px\" \/><\/a><\/p>\n<p>For the next step, look at the image below. It depicts a vector <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-641e9511492a62b5bd4131bc67a7a0bb_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#66;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"27\" style=\"vertical-align: 0px;\"\/> of length 1 in a plane. <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-641e9511492a62b5bd4131bc67a7a0bb_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#66;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"27\" style=\"vertical-align: 0px;\"\/> makes an angle <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-356a08e839ab6974a16448e16e56745d_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#116;&#104;&#101;&#116;&#97;\" title=\"Rendered by QuickLaTeX.com\" height=\"12\" width=\"9\" style=\"vertical-align: 0px;\"\/> (theta) with the r axis. The r coordinate of the vector <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-641e9511492a62b5bd4131bc67a7a0bb_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#66;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"27\" style=\"vertical-align: 0px;\"\/> is <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-b3e07a395da2636221d2ba6add0dfc29_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#99;&#111;&#115;&#92;&#116;&#104;&#101;&#116;&#97;\" title=\"Rendered by QuickLaTeX.com\" height=\"12\" width=\"36\" style=\"vertical-align: 0px;\"\/>, and its z coordinate is <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-0961bbc0cc2a7b6def9c9c1760b9fdf7_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#115;&#105;&#110;&#92;&#116;&#104;&#101;&#116;&#97;\" title=\"Rendered by QuickLaTeX.com\" height=\"12\" width=\"34\" style=\"vertical-align: 0px;\"\/> (see <a title=\"wikipedia\" href=\"http:\/\/en.wikipedia.org\/wiki\/Trigonometry\">wikipedia<\/a> for more detail). If the vector&#8217;s length is not 1, but some length AB, its r and z coordinates are <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-ff92a14c28dd287bfa96dba2706e2ada_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#65;&#66;&#92;&#99;&#111;&#115;&#92;&#116;&#104;&#101;&#116;&#97;\" title=\"Rendered by QuickLaTeX.com\" height=\"12\" width=\"66\" style=\"vertical-align: 0px;\"\/> and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-6d7b2df06f85bbfb10890b85d441a3e7_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#65;&#66;&#92;&#115;&#105;&#110;&#92;&#116;&#104;&#101;&#116;&#97;\" title=\"Rendered by QuickLaTeX.com\" height=\"12\" width=\"64\" style=\"vertical-align: 0px;\"\/> instead.<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/trigo_definitions.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone size-medium wp-image-116\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/trigo_definitions-300x291.png\" alt=\"trigo_definitions\" width=\"300\" height=\"291\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/trigo_definitions-300x291.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/trigo_definitions-624x605.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/trigo_definitions.png 705w\" sizes=\"auto, (max-width: 300px) 100vw, 300px\" \/><\/a><\/p>\n<p>&nbsp;<\/p>\n<p>The next image (below) depicts our two axes, the z axis and the r axis as well as the unit vectors. It also depicts two new vectors <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-45f109ad572fe8f741ef3694978738d1_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#66;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"27\" style=\"vertical-align: 0px;\"\/> and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-2b862eafe20e08fca2bc5d6e5c9f7d39_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#66;&#67;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"28\" style=\"vertical-align: 0px;\"\/> with length l2 and l3. These are the robot arm segments. A and B are the joints. C could be a joint as well if it had another segment attached (in the case of the M100RAK it does have an additional segment). Below are the forward kinematics equations. They give the coordinates of <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-fd3a1bca3da606cf474dc05bba08d415_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#67;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"27\" style=\"vertical-align: 0px;\"\/> as functions of the arm lengths l2, l3 and joint angles a2 and a3.<\/p>\n<p>&nbsp;<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" class=\"alignnone wp-image-175 size-large\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-3.11.47-PM-1024x839.png\" alt=\"Screen Shot 2014-07-07 at 3.11.47 PM\" width=\"625\" height=\"512\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-3.11.47-PM-1024x839.png 1024w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-3.11.47-PM-300x246.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-3.11.47-PM-624x511.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-3.11.47-PM.png 1224w\" sizes=\"auto, (max-width: 625px) 100vw, 625px\" \/><\/p>\n<p>The equation below describes the vector <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-641e9511492a62b5bd4131bc67a7a0bb_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#66;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"27\" style=\"vertical-align: 0px;\"\/>.<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-ad2b041570539317f290ec7938addfc8_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#66;&#125;&#32;&#61;&#32;&#108;&#95;&#50;&#32;&#92;&#108;&#101;&#102;&#116;&#091;&#32;&#92;&#99;&#111;&#115;&#40;&#97;&#95;&#50;&#41;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#114;&#32;&#43;&#32;&#92;&#115;&#105;&#110;&#40;&#97;&#95;&#50;&#41;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#122;&#32;&#92;&#114;&#105;&#103;&#104;&#116;&#093; \" title=\"Rendered by QuickLaTeX.com\" height=\"27\" width=\"255\" style=\"vertical-align: -5px;\"\/><\/p>\n<p>The next equation describes the vector <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-2b862eafe20e08fca2bc5d6e5c9f7d39_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#66;&#67;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"28\" style=\"vertical-align: 0px;\"\/>.<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-ba3e1da899329b9d4d4dd38e704fe16a_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#66;&#67;&#125;&#32;&#61;&#32;&#108;&#95;&#51;&#32;&#92;&#108;&#101;&#102;&#116;&#091;&#32;&#92;&#99;&#111;&#115;&#40;&#97;&#95;&#50;&#43;&#97;&#95;&#51;&#41;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#114;&#32;&#43;&#32;&#92;&#115;&#105;&#110;&#40;&#97;&#95;&#50;&#43;&#97;&#95;&#51;&#41;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#122;&#32;&#92;&#114;&#105;&#103;&#104;&#116;&#093; \" title=\"Rendered by QuickLaTeX.com\" height=\"27\" width=\"333\" style=\"vertical-align: -5px;\"\/><\/p>\n<p>However what really interests us is the Vector <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-fd3a1bca3da606cf474dc05bba08d415_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#67;&#125;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"27\" style=\"vertical-align: 0px;\"\/> since it tells us where the end of the arm will end up.<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-c04dd9ee7069612fad793b95b7079a7b_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#67;&#125;&#32;&#61;&#32;&#92;&#108;&#101;&#102;&#116;&#091;&#32;&#108;&#95;&#50;&#32;&#92;&#99;&#111;&#115;&#40;&#97;&#95;&#50;&#41;&#32;&#43;&#32;&#108;&#95;&#51;&#32;&#92;&#99;&#111;&#115;&#40;&#97;&#95;&#50;&#43;&#97;&#95;&#51;&#41;&#32;&#92;&#114;&#105;&#103;&#104;&#116;&#093;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#114;&#32;&#43;&#32;&#92;&#108;&#101;&#102;&#116;&#091;&#32;&#108;&#95;&#50;&#32;&#92;&#115;&#105;&#110;&#40;&#97;&#95;&#50;&#41;&#32;&#43;&#32;&#108;&#95;&#51;&#32;&#92;&#115;&#105;&#110;&#40;&#97;&#95;&#50;&#43;&#97;&#95;&#51;&#41;&#32;&#92;&#114;&#105;&#103;&#104;&#116;&#093;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#122; \" title=\"Rendered by QuickLaTeX.com\" height=\"27\" width=\"547\" style=\"vertical-align: -5px;\"\/><\/p>\n<p>If you are more of a coder than a mathematician, you can see this reflected in parts of the code. Specifically, in the function arms. The function takes angles a1, a2, a3 and a4 as well as the length of the arm segments as inputs and outputs a set of coordinates.<\/p>\n<pre>def arms(a1,a2,a3,a4,l2,l3,l4):\r\n\ter = cos(a1)*ex + sin(a1)*ey\r\n\tu2 = l2 * ( cos(a2)*er + sin(a2)*ez )\r\n\tu3 = l3 * ( cos(a2+a3)*er + sin(a2+a3)*ez )\r\n\tu4 = l4 * ( cos(a2+a3+a4)*er + sin(a2+a3+a4)*ez )\r\n#\tu4 = l4 * er\r\n\treturn u2,u3,u4\r\n\r\n<\/pre>\n<p>You can also see it in the function draw_robot. In line 5, Yaouen calls invert, the inverse kinematics function to receive the angles of the arms. In line 6 he calls the arms function which does the forward kinematics. Both are necessary to draw the robot in 3D.<\/p>\n<pre>def draw_robot(x,y,z,l2,l3,l4):\r\n\t# first draw x,y,z axes\r\n\tp0 = 10*array([-1,-1,0])\r\n\tfor e in ex,ey,ez:\tvp.arrow(pos=p0,axis=5*e,shaftwidth=0.5,color=e)\r\n\ta1,a2,a3,a4 = invert(x,y,z,l2,l3,l4)                  # compute joint angles\r\n\tu2,u3,u4 = arms(a1,a2,a3,a4,l2,l3,l4)                    # compute arm vectors\r\n\tw = 0.2                                          # arm radius and base thickness\r\n\tu0 = w*ez \r\n\tv0 = cos(a1)*ex + sin(a1)*ey                     # \"front\" direction\r\n\tr = 3                                            # radius of the base\r\n\tvp.cylinder(pos=-u0,axis=2*u0,radius=r)         # draw base\r\n\tvp.cylinder(pos=-u0+0.8*r*v0,axis=2*u0,radius=0.1*r,color=(0.5,0.5,0.5)) # draw grey circle that indicates base direction\r\n\tvp.cylinder(pos=(0,0,0),axis=u2,radius=w,color=(0.7,1,0.7)) # draw arm 2\r\n\tvp.sphere(pos=u2,radius=w)                                  # draw joint 2\r\n\tvp.cylinder(pos=u2,axis=u3,radius=w,color=(0.7,0.7,1))      # draw arm 3\r\n\tvp.sphere(pos=u2+u3,radius=w)                               # draw joint 3\r\n\tvp.cylinder(pos=u2+u3,axis=u4,radius=w,color=(0.7,0.7,1))   # draw arm 4\r\n\tvp.sphere(pos=u2+u3+u4,radius=2*w,color=(0,1,0))            # draw end of arm 4\r\n\tvp.sphere(pos=(x,y,z),radius=2*w,color=(1,0,0))             # draw target point\r\n\t#the last two lines are doing the same but are there for debugging \r\n<\/pre>\n<p>&nbsp;<\/p>\n<p><b>Inverse Kinematics<\/b><\/p>\n<p>Big picture: In order to get the end of the robot arm to point to an xyz-coordinate of our choice we need an inverse kinematics function. We also need the length of the arm segments. We can then give those two sets of parameters to the function and the function will calculate what angles we have to create between the arm segments (using motors). Below is an explanation of the math that Yaouen used to figure out how to to write the inverse kinematics function in Python. Its all trigonometry. However, this problem could also be solved using other mathematical methods.<\/p>\n<p>Ok. Here we go: Below, you see the 3D space again. As previously said, we want to figure out angles as a function of arm lengths and an xyz coordinate. This coordinate is called C in the drawing below. To get started, we need to determine <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-4a98cd76696f70d2b005e8efdea1211a_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#114;&#95;&#99;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"14\" style=\"vertical-align: -3px;\"\/>,<img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-81dcac279d4f81944aab311565a5371b_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#97;&#95;&#49;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"12\" width=\"15\" style=\"vertical-align: -4px;\"\/>, and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-e57d600e902127f25406fd2503ae62ee_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#122;&#95;&#99;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"14\" style=\"vertical-align: -3px;\"\/>. We can do this as a 2D problem as shown in b) by defining the point H which is a projection of C onto r.<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-3.15.31-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone wp-image-220 size-large\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-3.15.31-PM-1024x618.png\" alt=\"Screen Shot 2014-07-09 at 3.15.31 PM\" width=\"625\" height=\"377\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-3.15.31-PM-1024x618.png 1024w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-3.15.31-PM-300x181.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-3.15.31-PM-624x376.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-3.15.31-PM.png 1600w\" sizes=\"auto, (max-width: 625px) 100vw, 625px\" \/><\/a><\/p>\n<p>This is the definition of <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-7fca8ff09b51d89d8222192b3adac41b_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#114;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"21\" width=\"24\" style=\"vertical-align: -3px;\"\/><\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-d726e46181d50cb883b6d8b4b46f0751_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#114;&#32;&#61;&#32;&#92;&#99;&#111;&#115;&#40;&#97;&#95;&#49;&#41;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#120;&#32;&#43;&#32;&#92;&#115;&#105;&#110;&#40;&#97;&#95;&#49;&#41;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#121; \" title=\"Rendered by QuickLaTeX.com\" height=\"24\" width=\"229\" style=\"vertical-align: -6px;\"\/><\/p>\n<p>This is the definition of <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-8f5d0c1be9d99f9052ee35ab81132f8b_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#72;&#125;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"29\" style=\"vertical-align: 0px;\"\/><\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-fcfa73cc81a76ffe29113d2ff72fc536_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#72;&#125;&#32;&#61;&#32;&#114;&#95;&#99;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#114;&#32;&#61;&#32;&#120;&#95;&#99;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#120;&#32;&#43;&#32;&#121;&#95;&#99;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#121; \" title=\"Rendered by QuickLaTeX.com\" height=\"28\" width=\"222\" style=\"vertical-align: -6px;\"\/><\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-1ed19a0f995e7d531ca3921035fa7447_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#114;&#95;&#99;&#32;&#61;&#32;&#92;&#115;&#113;&#114;&#116;&#123;&#120;&#95;&#99;&#94;&#50;&#43;&#121;&#95;&#99;&#94;&#50;&#125; \" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"113\" style=\"vertical-align: -6px;\"\/><\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-074ba4d57d044a5d0c061eb41ab156a6_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#92;&#99;&#111;&#115;&#40;&#97;&#95;&#49;&#41;&#61;&#92;&#102;&#114;&#97;&#99;&#123;&#120;&#95;&#99;&#125;&#123;&#114;&#95;&#99;&#125; \" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"94\" style=\"vertical-align: -8px;\"\/><br \/>\n<img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-40c78f4137ebd9153d98e451bf5d3c89_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#92;&#115;&#105;&#110;&#40;&#97;&#95;&#49;&#41;&#61;&#92;&#102;&#114;&#97;&#99;&#123;&#121;&#95;&#99;&#125;&#123;&#114;&#95;&#99;&#125; \" title=\"Rendered by QuickLaTeX.com\" height=\"22\" width=\"92\" style=\"vertical-align: -8px;\"\/><\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-b5e730b134c9782480ec1e74e4f3147a_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\" &#97;&#95;&#49;&#32;&#61;&#32;&#92;&#97;&#114;&#99;&#116;&#97;&#110;&#32;&#50;&#32;&#92;&#108;&#101;&#102;&#116;&#40;&#32;&#92;&#102;&#114;&#97;&#99;&#123;&#121;&#95;&#99;&#125;&#123;&#114;&#95;&#99;&#125;&#44;&#32;&#92;&#102;&#114;&#97;&#99;&#123;&#120;&#95;&#99;&#125;&#123;&#114;&#95;&#99;&#125;&#32;&#92;&#114;&#105;&#103;&#104;&#116;&#41; \" title=\"Rendered by QuickLaTeX.com\" height=\"33\" width=\"166\" style=\"vertical-align: -12px;\"\/><\/p>\n<p>Now that we have <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-31a6f6ee223d980133130d129e07e0db_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#97;&#49;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"13\" width=\"17\" style=\"vertical-align: -1px;\"\/> and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-4a98cd76696f70d2b005e8efdea1211a_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#114;&#95;&#99;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"14\" style=\"vertical-align: -3px;\"\/>, the next step will be to determine AC and and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-a4306749a1a62a769b17b849d10edba8_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#51;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>.<\/p>\n<p>The distance AC doesn&#8217;t depend on <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-0429f295f30eec4db774f087e7f85c21_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>, nor on <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-460f5abc45b558edf34fe288dc0a9979_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#49;\" title=\"Rendered by QuickLaTeX.com\" height=\"12\" width=\"15\" style=\"vertical-align: -4px;\"\/>, only on <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-a4306749a1a62a769b17b849d10edba8_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#51;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>. Therefore computing AC will give us an equation that relates <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-0429f295f30eec4db774f087e7f85c21_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>, <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-d9b151214729674666cb64bbb45c10f2_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#108;&#95;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"16\" width=\"12\" style=\"vertical-align: -3px;\"\/> and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-879f549292bdaa3cb680fc5c434443e4_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#108;&#95;&#51;\" title=\"Rendered by QuickLaTeX.com\" height=\"16\" width=\"12\" style=\"vertical-align: -3px;\"\/> to <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-cbd03b95a8977fb7aeb842e7f7846ab2_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#114;&#95;&#99;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"14\" style=\"vertical-align: -3px;\"\/> and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-b713dd8fcf41fd764595e20200555d22_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#122;&#95;&#99;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"14\" style=\"vertical-align: -3px;\"\/>, that we can then solve for <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-0429f295f30eec4db774f087e7f85c21_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>.<\/p>\n<p>First we express AC in terms of the coordinates of C:<br \/>\n<img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-05d6be36a04135f3274aa332ac81f73f_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#65;&#67;&#125;&#32;&#61;&#32;&#114;&#95;&#99;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#114;&#32;&#43;&#32;&#122;&#95;&#99;&#32;&#92;&#111;&#118;&#101;&#114;&#114;&#105;&#103;&#104;&#116;&#97;&#114;&#114;&#111;&#119;&#123;&#101;&#125;&#95;&#122;\" title=\"Rendered by QuickLaTeX.com\" height=\"25\" width=\"152\" style=\"vertical-align: -3px;\"\/><\/p>\n<p><img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-038ff0a159c2cfc79f0e564d720ddd0e_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#65;&#67;&#94;&#50;&#32;&#61;&#32;&#114;&#95;&#99;&#94;&#50;&#32;&#43;&#32;&#122;&#95;&#99;&#94;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"19\" width=\"112\" style=\"vertical-align: -4px;\"\/><\/p>\n<p>Then in terms of the arms lengths and joint angles, using the forward kinematics expressions from earlier:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-12-at-11.24.10-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone wp-image-252 size-large\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-12-at-11.24.10-PM-1024x264.png\" alt=\"Screen Shot 2014-07-12 at 11.24.10 PM\" width=\"625\" height=\"161\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-12-at-11.24.10-PM-1024x264.png 1024w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-12-at-11.24.10-PM-300x77.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-12-at-11.24.10-PM-624x160.png 624w\" sizes=\"auto, (max-width: 625px) 100vw, 625px\" \/><\/a><\/p>\n<p>To get the last line we used the trigonometric relations (see wikipedia)<br \/>\n<img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-dc464d703972fa6cae7afd9fed9508ef_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#92;&#99;&#111;&#115;&#40;&#97;&#41;&#94;&#50;&#43;&#92;&#115;&#105;&#110;&#94;&#50;&#40;&#97;&#41;&#32;&#61;&#32;&#49;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"20\" width=\"161\" style=\"vertical-align: -4px;\"\/><\/p>\n<p>and<br \/>\n<img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-28b3aa67202ee7900aaf0ca8a83be6b9_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#92;&#99;&#111;&#115;&#40;&#97;&#41;&#92;&#99;&#111;&#115;&#40;&#98;&#41;&#32;&#43;&#32;&#92;&#115;&#105;&#110;&#40;&#97;&#41;&#92;&#115;&#105;&#110;&#40;&#98;&#41;&#32;&#61;&#32;&#92;&#99;&#111;&#115;&#40;&#98;&#45;&#97;&#41;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"18\" width=\"306\" style=\"vertical-align: -4px;\"\/><\/p>\n<p>&nbsp;<\/p>\n<p>Comparing the two expressions of <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-5953a081465ef4b4de31aef70b09858b_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#65;&#67;&#94;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"15\" width=\"34\" style=\"vertical-align: 0px;\"\/>, we get:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.12.59-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone wp-image-189 size-medium\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.12.59-PM-300x185.png\" alt=\"Screen Shot 2014-07-07 at 5.12.59 PM\" width=\"300\" height=\"185\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.12.59-PM-300x185.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.12.59-PM-624x386.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.12.59-PM.png 630w\" sizes=\"auto, (max-width: 300px) 100vw, 300px\" \/><\/a><\/p>\n<p>Look at the drawing below. It shows <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-7ad68fec4ca5a36028b1fe5d75fe3321_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#97;&#95;&#51;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>, the &#8220;elbow&#8221; of the robot arm shown in green in a). In a) <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-7ad68fec4ca5a36028b1fe5d75fe3321_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#97;&#95;&#51;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>\u00a0 is negative. \u00a0 <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-7ad68fec4ca5a36028b1fe5d75fe3321_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#97;&#95;&#51;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/> could in principle be positive or negative as shown in b). We chose the negative option because we do not want the arm to go below the base (the &#8220;shoulder&#8221; joint) and violently bang into the table as shown in c) (because the motors and everything else would suffer). Therefore we want B to be above C (B and C are shown in the previous images &#8211; not in the ones below).<\/p>\n<div id=\"attachment_222\" style=\"width: 635px\" class=\"wp-caption alignnone\"><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-4.14.43-PM.png\"><img loading=\"lazy\" decoding=\"async\" aria-describedby=\"caption-attachment-222\" class=\"wp-image-222 size-large\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-4.14.43-PM-1024x387.png\" alt=\"Screen Shot 2014-07-09 at 4.14.43 PM\" width=\"625\" height=\"236\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-4.14.43-PM-1024x387.png 1024w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-4.14.43-PM-300x113.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-4.14.43-PM-624x235.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-4.14.43-PM.png 1798w\" sizes=\"auto, (max-width: 625px) 100vw, 625px\" \/><\/a><p id=\"caption-attachment-222\" class=\"wp-caption-text\">We want a3 to always be negative because else, the arm would be colliding with a surface below the table as shown in c).<\/p><\/div>\n<p>Looking at the the two expressions of AC above, we can write<br \/>\n<a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.22.11-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-195\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.22.11-PM-300x70.png\" alt=\"Screen Shot 2014-07-07 at 5.22.11 PM\" width=\"218\" height=\"51\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.22.11-PM-300x70.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.22.11-PM.png 502w\" sizes=\"auto, (max-width: 218px) 100vw, 218px\" \/><\/a><\/p>\n<p>To disentangle <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-3aea1219ae8c2816f9bf610936a2b2f6_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#97;&#95;&#50;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/> and <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-7ad68fec4ca5a36028b1fe5d75fe3321_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#32;&#97;&#95;&#51;&#32;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/>, we now use the trigonometric formulae:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.29-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-197\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.29-PM-300x48.png\" alt=\"Screen Shot 2014-07-07 at 5.23.29 PM\" width=\"294\" height=\"47\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.29-PM-300x48.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.29-PM-624x101.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.29-PM.png 662w\" sizes=\"auto, (max-width: 294px) 100vw, 294px\" \/><\/a><\/p>\n<p>The result is:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.33-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-198\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.33-PM-300x55.png\" alt=\"Screen Shot 2014-07-07 at 5.23.33 PM\" width=\"262\" height=\"48\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.33-PM-300x55.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.33-PM-624x116.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.23.33-PM.png 806w\" sizes=\"auto, (max-width: 262px) 100vw, 262px\" \/><\/a><\/p>\n<p>If we now think cos( <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-0429f295f30eec4db774f087e7f85c21_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/> ) and sin( <img loading=\"lazy\" decoding=\"async\" src=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/ql-cache\/quicklatex.com-0429f295f30eec4db774f087e7f85c21_l3.png\" class=\"ql-img-inline-formula quicklatex-auto-format\" alt=\"&#97;&#95;&#50;\" title=\"Rendered by QuickLaTeX.com\" height=\"11\" width=\"16\" style=\"vertical-align: -3px;\"\/> ) as two independent unknowns, this is a system of two linear equations, which can always be solved, using for example Cramer\u2019s method (see <a href=\"http:\/\/en.wikipedia.org\/wiki\/Cramer%27s_rule\" target=\"_blank\">wikipedia<\/a>).<\/p>\n<p>For the sake of notational brevity, we now introduce<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.29.05-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-202\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.29.05-PM-300x109.png\" alt=\"Screen Shot 2014-07-07 at 5.29.05 PM\" width=\"165\" height=\"60\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.29.05-PM-300x109.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.29.05-PM.png 330w\" sizes=\"auto, (max-width: 165px) 100vw, 165px\" \/><\/a><\/p>\n<p>so that our equations can be shortened to:<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-210\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.33.18-PM1-300x70.png\" alt=\"Screen Shot 2014-07-07 at 5.33.18 PM\" width=\"210\" height=\"49\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.33.18-PM1-300x70.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-07-at-5.33.18-PM1.png 466w\" sizes=\"auto, (max-width: 210px) 100vw, 210px\" \/><\/p>\n<p>On Wikipedia, the unknowns are x and y and the equations are:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.08.21-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-227\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.08.21-PM-300x137.png\" alt=\"Screen Shot 2014-07-09 at 5.08.21 PM\" width=\"120\" height=\"55\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.08.21-PM-300x137.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.08.21-PM.png 310w\" sizes=\"auto, (max-width: 120px) 100vw, 120px\" \/><\/a><\/p>\n<p>and the solution is:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.10.37-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-228\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.10.37-PM-300x241.png\" alt=\"Screen Shot 2014-07-09 at 5.10.37 PM\" width=\"104\" height=\"84\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.10.37-PM-300x241.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.10.37-PM.png 316w\" sizes=\"auto, (max-width: 104px) 100vw, 104px\" \/><\/a><\/p>\n<p>Using the following correspondence table:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.12.09-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone  wp-image-229\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.12.09-PM-163x300.png\" alt=\"Screen Shot 2014-07-09 at 5.12.09 PM\" width=\"78\" height=\"143\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.12.09-PM-163x300.png 163w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.12.09-PM.png 226w\" sizes=\"auto, (max-width: 78px) 100vw, 78px\" \/><\/a><\/p>\n<p>We obtain the solution to our equations:<\/p>\n<p><a href=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.15.02-PM.png\"><img loading=\"lazy\" decoding=\"async\" class=\"alignnone size-medium wp-image-231\" src=\"http:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.15.02-PM-300x59.png\" alt=\"Screen Shot 2014-07-09 at 5.15.02 PM\" width=\"300\" height=\"59\" srcset=\"https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.15.02-PM-300x59.png 300w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.15.02-PM-624x122.png 624w, https:\/\/www.anninaruest.com\/pie\/wp-content\/uploads\/2014\/07\/Screen-Shot-2014-07-09-at-5.15.02-PM.png 772w\" sizes=\"auto, (max-width: 300px) 100vw, 300px\" \/><\/a><\/p>\n<p>Here is the function that does the inverse kinematics. You will see that it is similar to the math done in this section of the blog post:<\/p>\n<pre># input: coordinates x,y,z of the target point, lengths b,c of the arms\r\n# output: angles a1,a2,a3 of the joints, in degrees\r\ndef invert(x,y,z,l2,l3,l4):\r\ns\u00a0 = \"(%g,%g,%g) is out of range.\" % tuple(around([x,y,z],2))\r\na1 = arctan2(y,x)\r\nr\u00a0 = hypot(x,y) - l4\r\nu3 = ( r**2 + z**2 - l2**2 - l3**2 ) \/ ( 2*l2*l3 )\r\nif abs(u3)&gt;1:\u00a0\u00a0 \u00a0raise Exception(s)\r\na3 = -arccos(u3)\r\nv\u00a0 = l2 + l3*u3\r\nw\u00a0 = -l3 * sqrt(1-u3**2)\u00a0 # this is sin(a3), &gt;0 assuming 0&lt;a3&lt;pi\r\na2 = arctan2(v*z-w*r,v*r+w*z)\r\n#\u00a0\u00a0 \u00a0# at this point a2&lt;0 =&gt; take the other solution\r\n#\u00a0\u00a0 \u00a0a2 = 2*arctan2(z,r) - a2\r\n#\u00a0\u00a0 \u00a0a3 = -a3\r\nif a2&lt;0 or a2&gt;pi:\u00a0\u00a0 \u00a0raise Exception(s)\r\na4 = - a2 - a3\r\n#\u00a0\u00a0 \u00a0return r2d(a1,a2,a3)\r\nreturn a1,a2,a3,a4\r\n\r\n<\/pre>\n","protected":false},"excerpt":{"rendered":"<p>The video above shows the M100RAK drawing a rudimentary pie chart onto a pie with a &#8220;FoodWriter&#8221;, a marker containing food coloring. The pie chart depicts the gender ratio among Google&#8217;s staff (women: 30%, men 70%). The food color marker does not draw well on chocolate, especially in hot weather. Tests with hard-drying royal icing [&hellip;]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"closed","ping_status":"closed","sticky":false,"template":"","format":"standard","meta":{"_jetpack_memberships_contains_paid_content":false,"footnotes":""},"categories":[1],"tags":[],"class_list":["post-100","post","type-post","status-publish","format-standard","hentry","category-uncategorized"],"jetpack_sharing_enabled":true,"jetpack_featured_media_url":"","_links":{"self":[{"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/posts\/100","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/comments?post=100"}],"version-history":[{"count":117,"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/posts\/100\/revisions"}],"predecessor-version":[{"id":254,"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/posts\/100\/revisions\/254"}],"wp:attachment":[{"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/media?parent=100"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/categories?post=100"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.anninaruest.com\/pie\/wp-json\/wp\/v2\/tags?post=100"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}