32:T691,tex` \qquad T_{01}(\theta_1) = \begin{bmatrix} \cos(\theta_1) & -\sin(\theta_1) & 0 & L_{0} \\ \sin(\theta_1) & \cos(\theta_1) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \qquad T_{12}(\theta_2) = \begin{bmatrix} \cos(\theta_2) & -\sin(\theta_2) & 0 & L_{1} \\ \sin(\theta_2) & \cos(\theta_1) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \qquad T_{23}(\theta_3) = \begin{bmatrix} \cos(\theta_3) & -\sin(\theta_3) & 0 & 0 \\ \sin(\theta_3) & \cos(\theta_3) & 0 & -L_{2} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \\ \; \\ \qquad T_{34} = \begin{bmatrix} 1 & 0 & 0 & -r \\ 0 & 1 & 0 & -h \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \\ \quad \\ \qquad \textrm{where } L_{0} = {\bf ${L0.toFixed(0)}} \textrm{ mm is the distance in the x-direction from the origin to Joint 1 (J1).} \\ \qquad \qquad \;\;\; L_{1} = {\bf ${L1.toFixed(0)}} \textrm{ mm is the distance between Joint 1 (J1) and Joint 2 (J2). It is along the x-axis in the rest position.} \\ \qquad \qquad \;\;\; L_{2} = {\bf ${L2.toFixed(0)}} \textrm{ mm is the distance between Joint 2 (J2) and Joint 3 (J3). It is along the y-axis in the rest position.} \\ \qquad \qquad \;\;\;\;\; r = {\bf ${r.toFixed(0)}} \textrm{ mm is the distance in the x-direction between Joint 3 (J3) and the end effector.} \\ \qquad \textrm{and } \;\;\;\;\; h = {\bf ${h.toFixed(0)}} \textrm{ mm is the distance in the y-direction between Joint 3 (J3) and the end effector.} `33:T533,

The transformation matrix ${tex`T_{01}`} represents the rotation around the z-axis at Joint 1 (J1). ${tex`T_{12}`} represents the rotation around Joint 2 (J1). ${tex`T_{23}`} represents the rotation around Joint 3 (J2). ${tex`T_{34}`} accounts for the position of the end effector (i.e., the blue ball) with respect to Joint 3 (J3).

The complete transformation matrix ${tex`T_{04}`} is computed from the product of the individual transformation matrices:
${tex`\qquad T_{04} = T_{01}T_{12}T_{23}T_{34} `}

The resulting expression for ${tex`T_{04}`} is:
${tex` \qquad T_{04} = \begin{bmatrix} 1 & 0 & 0 & L_0 + L_1 \cos(\theta_1) + L_2 \sin(\theta_1+\theta_2) -r\\ 0 & 1 & 0 & L_1 \sin(\theta_1) - L_2 \cos(\theta_1+\theta_2) - h \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} `}

The upper-left 3×3 portion of this matrix represents rotation. Since no rotation is applied to the end effector, it is expected that this 3×3 matrix remains as the identity.

The cartesian coordinates ${tex`(x, y, z)`} are obtained from the last column of this matrix:

${tex` \qquad \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = T_{04} \cdot \begin{bmatrix} 0 \\ 0 \\ 0 \\ 1 \end{bmatrix} \\ `}34:T8fb,load_success = (api) => { api.start(function () { api.addEventListener("viewerready", function () { api.getNodeMap(function (err, nodes) { // console.log(nodes); const end_effector = getNodeByName(nodes, end_effector_ID); const L1A = getNodeByName(nodes, L1A_ID); const L2A = getNodeByName(nodes, L2A_ID); const L1B = getNodeByName(nodes, L1B_ID); const L2B = getNodeByName(nodes, L2B_ID); // Directly invoke the function with the current value on page load (function initialize() { updateMatrix(); // This ensures the initial values are applied immediately })(); // 3D model is reactive to the slider value changes. // For IK, we recalculate the transformation on mouse events with a slight delay. // This ensures the 3D model updates correctly, even if the slider is clicked rather than dragged. [x, y].forEach(slider => { slider.addEventListener('input', updateMatrix); slider.addEventListener("mousedown", () => { setTimeout(updateMatrix, 100); // Adjust the delay (ms) as needed }); slider.addEventListener("change", updateMatrix); // Improves the behavior if the slider is changed via text input }); function updateMatrix() { mutable x_value = x.value; mutable y_value = y.value; mutable y_value_offset = mutable y_value - y_offset - y_offset_model; // Orientation of the 3D model differs from notebook: (x,y,z) -> (x,z,-y) translateNode(api, end_effector.instanceID, [mutable x_value, 0, -mutable y_value_offset]); const angle1A = -degToRad(theta_1_A.value); const angle1B = -degToRad(theta_1_B.value); const angle2A = -degToRad(theta_2_A.value - theta_2_offset_model); const angle2B = -degToRad(theta_2_B.value + theta_2_offset_model); rotateNode(api, L1A.instanceID, angle1A, 0, 1, 0); rotateNode(api, L1B.instanceID, angle1B, 0, 1, 0); rotateNode(api, L2A.instanceID, angle2A, 0, 1, 0); rotateNode(api, L2B.instanceID, angle2B, 0, 1, 0); } }); }); }); };35:[{"id":"fa33bf8c0e60adb5fe7adc48a737ab732b53e8e1f1feb6cc00e3b1fd36fe2149aa599002963eec9159dcad999f72135f389fc359819541b9fc2d410ec3ab7b84","href":"https://static.observableusercontent.com/files/fa33bf8c0e60adb5fe7adc48a737ab732b53e8e1f1feb6cc00e3b1fd36fe2149aa599002963eec9159dcad999f72135f389fc359819541b9fc2d410ec3ab7b84","name":"sketchfab-viewer-1.11.0.js","type":"application/javascript","lastModified":1724268736427.122,"size":6587},{"id":"7017a0530b39a27808efabfcbbf1e6bace9c0da13f67f8734d07bbcefd8acd5e57c74c05a8b0424dc2bdc1d3923180367605626a78b4ea5d2f79bce2a72b230d","href":"https://static.observableusercontent.com/files/7017a0530b39a27808efabfcbbf1e6bace9c0da13f67f8734d07bbcefd8acd5e57c74c05a8b0424dc2bdc1d3923180367605626a78b4ea5d2f79bce2a72b230d","name":"kinematic_diagram_mechanism@3.svg","type":"image/svg+xml","lastModified":1724316169207.675,"size":12892}] 30:{"title":"Inverse Kinematics: Animation of a basic 3D model with the Sketchfab Viewer API","cells":[{"id":0,"value":"# Inverse Kinematics: Animation of a basic 3D model with the Sketchfab Viewer API","pinned":false,"hidden":false,"mode":"md"},{"id":263,"value":"The Sketchfab 3D model prepared for this demonstration was intentionally kept simple to effectively showcase animation using the **Sketchfab Viewer API**. The end effector, represented by a blue ball, can be moved within the XY plane using two sliders. As the end effector moves, the bar linkages rotate accordingly, with their angles determined through **inverse kinematics (IK)** calculations.","pinned":false,"hidden":false,"mode":"md"},{"id":1057,"value":"interactive_control = md`\n---\n### Interactive control\n*Use the sliders to manipulate the 3D model.*\n`","pinned":false,"hidden":false,"mode":"ojs"},{"id":141,"value":"container = html`\n

\n Position of the end effector (IK) \n

\n ${x}\n ${y}\n
↳ Calculated angles:\n ${theta_1_A}\n ${theta_2_A}\n ${theta_1_B}\n ${theta_2_B}\n

\n`","pinned":false,"hidden":false,"mode":"ojs"},{"id":7,"value":"\n\n

\n \n

","pinned":false,"hidden":false,"mode":"html"},{"id":306,"value":"---\n## Forward Kinematics (FK)\n\nForward kinematics (FK) is used to determine the position and orientation of an end effector based on the joint angles of a mechanism.\n\nHere, the mechanism consists of two symmetrical articulated structures, each made up of bar linkages with lengths L1 and L2. These linkages can freely rotate around pivots, represented as red cylinders in the 3D model.\n\n### Kinematic diagram\nLet's concentrate on the right side of the mechanism (index **B** in the calculations), as illustrated in the following kinematic diagram. For simplicity in calculations, the rest position assumes that the bar linkage L2 is positioned at a right angle.","pinned":false,"hidden":false,"mode":"md"},{"id":311,"value":"// This SVG file was created with Adobe Illustrator\n\nkinematic_diagram = { \n const text = await FileAttachment(\"kinematic_diagram_mechanism@3.svg\").text();\n const document = (new DOMParser).parseFromString(text, \"image/svg+xml\");\n const svg = d3.select(document.documentElement).remove();\n\n // Apply a translation transformation to offset the SVG to the right\n const offsetX = 50;\n svg.attr(\"transform\", `translate(${offsetX}, 0)`);\n \n return svg.node();\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":327,"value":"Let's calculate the position (*x*,*y*,*z*) of the end effector based on three input angles (θ₁ to θ₃), under the constraint that the end effector doesn't rotate. \n\nGiven this constraint, the following relation for θ₃ can be easily derived:","pinned":false,"hidden":false,"mode":"md"},{"id":371,"value":"tex`\n \\qquad \\theta_3 = -\\theta_1 - \\theta_2 \n`","pinned":false,"hidden":false,"mode":"ojs"},{"id":366,"value":"### Transformation matrices\nThe transformation matrices for the three joints and for the end effector are defined as follows:","pinned":false,"hidden":false,"mode":"md"},{"id":335,"value":"$32","pinned":false,"hidden":false,"mode":"ojs"},{"id":362,"value":"$33","pinned":false,"hidden":false,"mode":"html"},{"id":401,"value":"This yields the coordinates of the end effector:","pinned":false,"hidden":false,"mode":"md"},{"id":379,"value":"tex`\n \\qquad x = L_0 + L_1 \\cos(\\theta_1) + L_2 \\sin(\\theta_1+\\theta_2) -r \\\\\n\n \\qquad y = L_1 \\sin(\\theta_1) - L_2 \\cos(\\theta_1+\\theta_2) - h \\\\\n\n \\qquad z = 0\n\n`","pinned":false,"hidden":false,"mode":"ojs"},{"id":1047,"value":"## Inverse Kinematics (IK)\n\nInverse kinematics (IK) is a crucial concept in robotics, allowing for the determination of joint angles (here, θ₁ to θ₃) based on the desired position (*x*,*y*,*z*) and orientation (*here, no rotation*) of the end effector.\n\nAs stated above, θ₃ can be determined from θ₁ and θ₂. Let's now determine these angles.","pinned":false,"hidden":false,"mode":"md"},{"id":436,"value":"### Joint 2, ${tex`\\theta_2`}","pinned":false,"hidden":false,"mode":"md"},{"id":417,"value":"To solve for ${tex`\\theta_2`}, one can observe that the distance ${tex`r_{13}`} from joint J1 to joint J3 only depends on ${tex`\\theta_2`}. We have:\n\n ${tex`\n \\qquad r_{13}^2 = L_1^2 + 2 L_1 L_2 \\sin(\\theta_2) + L_2^2 \\\\\n`}","pinned":false,"hidden":false,"mode":"md"},{"id":430,"value":"On the other hand, this distance can easily be expressed as a function of the coordinates (*x*,*y*) of the end effector:\n\n ${tex`\n \\qquad r_{13}^2 = (x+r-L_0)^2 + (y+h)^2 \\\\\n`}\n","pinned":false,"hidden":false,"mode":"md"},{"id":424,"value":"Hence, the solution for ${tex`\\theta_2`} is obtained by solving:\n\n${tex`\n \\qquad \\begin{aligned}\n \\theta_2 &= \\pm \\arcsin\\Big(\\frac{r_{13}^2 - L_1^2 - L_2^2}{2 L_1 L_2}\\Big) \\\\\n &= \\pm \\arcsin\\Big(\\frac{(x+r-L_0)^2 + (y+h)^2 - L_1^2 - L_2^2}{2 L_1 L_2}\\Big) \n \\end{aligned}\n`}","pinned":false,"hidden":false,"mode":"md"},{"id":438,"value":"theta_2_solutions = {\n const r_13_sqr = (x_value+r-L0)**2 + (y_value+h)**2\n const theta_2_1 = Math.asin((r_13_sqr- L1**2 - L2**2)/(2*L1*L2))\n const theta_2_2 = -theta_2_1\n\n return htl.html`

With the numerical inputs selected above, the possible solutions for ${tex`\\theta_{\\textrm{2B}}`} are:\n
\n ${tex`\\quad \\theta_{2,1} = ${radToDeg(theta_2_1).toFixed(2)}°`} and \n ${tex`\\quad \\theta_{2,2} = ${radToDeg(theta_2_2).toFixed(2)}°`}.\n

`;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1087,"value":"Considering the rotation range of J2, we store the potential candidates in an array, theta_2_B_IK:","pinned":false,"hidden":false,"mode":"md"},{"id":985,"value":"theta_2_B_IK = {\n const r_13_sqr = (x_value+r-L0)**2 + (y_value+h)**2;\n const theta_2_1 = Math.asin((r_13_sqr- L1**2 - L2**2)/(2*L1*L2));\n const theta_2_2 = -theta_2_1;\n var theta_2_IK = [];\n \n theta_2_IK.push(radToDeg(theta_2_1));\n theta_2_IK.push(radToDeg(theta_2_2));\n \n return theta_2_IK = theta_2_IK.filter(angle => angle >= theta_2_Min && angle <= theta_2_Max)\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":911,"value":"### Joint 1, ${tex`\\theta_1`}","pinned":false,"hidden":false,"mode":"md"},{"id":916,"value":"To solve for ${tex`\\theta_1`}, we start by manipulating the previous equations (refer to Section FK) to eliminate the ${tex`(\\theta_1 + \\theta_2)`} term. We begin by rearranging the equations that define the Cartesian coordinates:\n\n ${tex`\n \\qquad \\begin{aligned}\n x - L_0 + r &= L_1 \\cos(\\theta_1) + L_2 \\sin(\\theta_1+\\theta_2) \\\\\n y + h &= L_1 \\sin(\\theta_1) - L_2 \\cos(\\theta_1+\\theta_2) \n \\end{aligned}\n`}","pinned":false,"hidden":false,"mode":"md"},{"id":926,"value":"The following result can then be deduced:\n\n ${tex`\n \\qquad \\begin{aligned}\n (x - L_0 + r) \\cos(\\theta_1) + (y +h) \\sin(\\theta_1) &= L_1 + L_2 \\sin(\\theta_2) \\\\\n A \\cos(\\theta_1) + B \\sin(\\theta_1) &= L_1 + L_2 \\sin(\\theta_2)\n\\end{aligned} \\\\\n\\qquad \\textrm{with } A = x - L_0 + r \\quad \\textrm{ and } \\quad B = y+ h\\\\\n`}","pinned":false,"hidden":false,"mode":"md"},{"id":942,"value":"This expression can be rewritten as:\n\n${tex`\n \\qquad L_1 + L_2 \\sin(\\theta_2) = R\\cos(\\theta_1-\\phi) \\\\\n \\qquad \\textrm{with } \\; R = \\sqrt{A^2+B^2} \\; \\textrm{ and } \\; \\phi = \\arctan(\\frac{B}{A})\n`}","pinned":false,"hidden":false,"mode":"md"},{"id":945,"value":"Hence, the solution for ${tex`\\theta_1`} is obtained by solving:\n\n${tex`\n \\qquad \\theta_1 = \\phi \\pm \\arccos(\\frac{L_1 + L_2 \\sin(\\theta_2)}{R}) + k\\pi\\\\\n \\qquad \\textrm{with } k \\in \\{-1,0,1\\}\n\n`}","pinned":false,"hidden":false,"mode":"md"},{"id":968,"value":"theta_1_A_solutions = {\n const A = x_value - L0 + r\n const B = y_value + h \n const R = Math.pow(A**2 + B**2,0.5)\n const phi = Math.atan(B/A)\n\n const theta_2_1 = Math.asin((R**2- L1**2 - L2**2)/(2*L1*L2))\n\n const theta_1_A_1 = phi + Math.acos((L1+L2*Math.sin(theta_2_1))/R)\n const theta_1_A_2 = phi - Math.acos((L1+L2*Math.sin(theta_2_1))/R)\n \n return htl.html`

With the numerical inputs selected above, the possible solutions for ${tex`\\theta_{\\textrm{1B}}`} are:\n
\n ${tex`\\quad \\theta_{1,1} = ${radToDeg(theta_1_A_1).toFixed(2)}°`} \n ${tex`\\quad \\theta_{1,2} = ${radToDeg(theta_1_A_2).toFixed(2)}°`} \n ${tex`\\quad \\theta_{1,3} = ${radToDeg(theta_1_A_1 + Math.PI).toFixed(2)}°`} \n ${tex`\\\\\\quad \\theta_{1,4} = ${radToDeg(theta_1_A_2 + Math.PI).toFixed(2)}°`} \n ${tex`\\quad \\theta_{1,5} = ${radToDeg(theta_1_A_1 - Math.PI).toFixed(2)}°`} \n ${tex`\\quad \\theta_{1,6} = ${radToDeg(theta_1_A_2 - Math.PI).toFixed(2)}°`} \n

`\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1097,"value":"Considering the rotation range of J1, we store the potential candidates in an array, theta_1_B_IK:","pinned":false,"hidden":false,"mode":"md"},{"id":1018,"value":"theta_1_B_IK = {\n \n const A = x_value - L0 + r\n const B = y_value + h \n const R = Math.pow(A**2 + B**2,0.5)\n const phi = Math.atan(B/A)\n\n const theta_2_1 = Math.asin((R**2 - L1**2 - L2**2)/(2*L1*L2))\n\n const theta_1_1 = phi + Math.acos((L1+L2*Math.sin(theta_2_1))/R)\n const theta_1_2 = phi - Math.acos((L1+L2*Math.sin(theta_2_1))/R)\n\n var theta_1_IK = []\n \n theta_1_IK.push(radToDeg(theta_1_1))\n theta_1_IK.push(radToDeg(theta_1_2))\n theta_1_IK.push(radToDeg(theta_1_1 + Math.PI))\n theta_1_IK.push(radToDeg(theta_1_2 + Math.PI))\n theta_1_IK.push(radToDeg(theta_1_1 - Math.PI))\n theta_1_IK.push(radToDeg(theta_1_2 - Math.PI))\n\n return theta_1_IK = theta_1_IK.filter(angle => angle >= theta_1_Min && angle <= theta_1_Max)\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1101,"value":"### IK solution\n\nIn contrast to Forward Kinematics (FK), Inverse Kinematics (IK) may not yield a unique solution. For a given set of Cartesian coordinates, multiple sets of joint angles can potentially satisfy the same end effector position, depending on the mechanism's configuration.","pinned":false,"hidden":false,"mode":"md"},{"id":1105,"value":"#### IK solution for the right side of the mechanism\nAll potential solutions for ${tex`\\theta_{\\textrm{1B}}`} and ${tex`\\theta_{\\textrm{2B}}`} have been stored in arrays theta_1_B_IK and theta_2_B_IK, respectively. Finding the IK solution requires up to ×${theta_1_B_IK.length*theta_2_B_IK.length} calculations (${theta_1_B_IK.length}×${theta_2_B_IK.length}) in the current case. We use nested loops to compute the overall transformation matrix for each set of joint angles and check if the resulting position matches the input. When a solution is found that satisfies the position criteria, we break out of the loops and save the corresponding transformation matrix as T04_IK, as well as the matching solution indices:","pinned":false,"hidden":false,"mode":"md"},{"id":1110,"value":"IK_solution_B = { \n const tolerance = 0.1;\n let T04_IK = null;\n let indices = null;\n\n for (let i = 0; i < theta_1_B_IK.length; i++) {\n for (let j = 0; j < theta_2_B_IK.length; j++) { \n\n const T01_IK = T01(theta_1_B_IK[i]);\n const T12_IK = T12(theta_2_B_IK[j]);\n const T02_IK = multiplyMatrices4x4(T01_IK, T12_IK);\n const T23_IK = T23(-theta_1_B_IK[i] - theta_2_B_IK[j]);\n const T03_IK = multiplyMatrices4x4(T02_IK, T23_IK);\n const T04_IK_ij = multiplyMatrices4x4(T03_IK, T34);\n \n if (Math.abs(x_value - T04_IK_ij[0][3]) < tolerance &&\n Math.abs(y_value - T04_IK_ij[1][3]) < tolerance) {\n T04_IK = T04_IK_ij;\n indices = [i, j];\n mutable T04_B = T04_IK;\n break; // Break the innermost loop\n \n }\n if (T04_IK !== null) break;\n }\n if (T04_IK !== null) break;\n }\n return {T04_IK, indices};\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1196,"value":"The solution found is used to impose the following rotation angles to the right side of the mechanism:\n\n${tex`\n \\quad \\theta_{\\textrm{1B}} = ${theta_1_B_IK[IK_solution_B.indices[0]].toFixed(2)}° \\\\\n \\quad \\theta_{\\textrm{2B}} = ${theta_2_B_IK[IK_solution_B.indices[1]].toFixed(2)}°\n`} \n","pinned":false,"hidden":false,"mode":"md"},{"id":1466,"value":"#### IK solution for the left side of the mechanism\nBecause of the mechanism's symmetry, the calculations for the left side are similar. \nHowever, the transformation matrices must be adjusted to reflect the orientations of the bar linkages.\n\nThe results of these calculations are stored in the following variables (index **A** is used for this part of the mechanism):","pinned":false,"hidden":false,"mode":"md"},{"id":1454,"value":"theta_1_A_IK = {\n \n const A = x_value + L0 - r\n const B = y_value + h \n const R = Math.pow(A**2 + B**2,0.5)\n const phi = Math.atan(B/A)\n\n const theta_2_1 = Math.asin((R**2 - L1**2 - L2**2)/(2*L1*L2))\n\n const theta_1_1 = phi + Math.acos((L1+L2*Math.sin(theta_2_1))/R)\n const theta_1_2 = phi - Math.acos((L1+L2*Math.sin(theta_2_1))/R)\n\n var theta_1_IK = []\n \n theta_1_IK.push(radToDeg(theta_1_1))\n theta_1_IK.push(radToDeg(theta_1_2))\n theta_1_IK.push(radToDeg(theta_1_1 + Math.PI))\n theta_1_IK.push(radToDeg(theta_1_2 + Math.PI))\n theta_1_IK.push(radToDeg(theta_1_1 - Math.PI))\n theta_1_IK.push(radToDeg(theta_1_2 - Math.PI))\n\n return theta_1_IK = theta_1_IK.filter(angle => angle >= theta_1_Min && angle <= theta_1_Max)\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1452,"value":"theta_2_A_IK = {\n const r_13_sqr = (x_value-r+L0)**2 + (y_value+h)**2;\n const theta_2_1 = Math.asin((r_13_sqr- L1**2 - L2**2)/(2*L1*L2));\n const theta_2_2 = -theta_2_1;\n var theta_2_IK = [];\n \n theta_2_IK.push(radToDeg(theta_2_1));\n theta_2_IK.push(radToDeg(theta_2_2));\n \n return theta_2_IK = theta_2_IK.filter(angle => angle >= theta_2_Min && angle <= theta_2_Max)\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1469,"value":"IK_solution_A = { \n const tolerance = 0.1;\n let T04_IK = null;\n let indices = null;\n\n for (let i = 0; i < theta_1_A_IK.length; i++) {\n for (let j = 0; j < theta_2_A_IK.length; j++) { \n\n const T01_IK = T01A(theta_1_A_IK[i]);\n const T12_IK = T12A(theta_2_A_IK[j]);\n const T02_IK = multiplyMatrices4x4(T01_IK, T12_IK);\n const T23_IK = T23(-theta_1_A_IK[i] - theta_2_A_IK[j]);\n const T03_IK = multiplyMatrices4x4(T02_IK, T23_IK);\n const T04_IK_ij = multiplyMatrices4x4(T03_IK, T34A);\n \n if (Math.abs(x_value - T04_IK_ij[0][3]) < tolerance &&\n Math.abs(y_value - T04_IK_ij[1][3]) < tolerance) {\n T04_IK = T04_IK_ij;\n indices = [i, j];\n mutable T04_A = T04_IK;\n break; // Break the innermost loop\n \n }\n if (T04_IK !== null) break;\n }\n if (T04_IK !== null) break;\n }\n return {T04_IK, indices};\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1491,"value":"The solution found is used to impose the following rotation angles to the left side of the mechanism:\n\n${tex`\n \\quad \\theta_{\\textrm{1A}} = ${theta_1_A_IK[IK_solution_A.indices[0]].toFixed(2)}° \\\\\n \\quad \\theta_{\\textrm{2A}} = ${theta_2_A_IK[IK_solution_A.indices[1]].toFixed(2)}°\n`} \n","pinned":false,"hidden":false,"mode":"md"},{"id":9,"value":"---\n## Appendices","pinned":false,"hidden":false,"mode":"md"},{"id":36,"value":"#### Dimensions / parameters","pinned":false,"hidden":false,"mode":"md"},{"id":11,"value":"L0 = 200","pinned":false,"hidden":false,"mode":"ojs"},{"id":13,"value":"L1 = 350","pinned":false,"hidden":false,"mode":"ojs"},{"id":18,"value":"L2 = 800","pinned":false,"hidden":false,"mode":"ojs"},{"id":21,"value":"r = 96/2","pinned":false,"hidden":false,"mode":"ojs"},{"id":320,"value":"h = 112","pinned":false,"hidden":false,"mode":"ojs"},{"id":1661,"value":"// Initial position of the mechanism: [x,y,θ1A,θ2A,θ1B,θ2B]\ninitial_position = [331.5,-806.5,54.43,4.75,14.83,-26.28]","pinned":true,"hidden":false,"mode":"ojs"},{"id":862,"value":"x_offset = L0+L1-r","pinned":false,"hidden":false,"mode":"ojs"},{"id":1225,"value":"x_offset_model = - x_offset","pinned":false,"hidden":false,"mode":"ojs"},{"id":958,"value":"y_offset = -(L2+h)","pinned":false,"hidden":false,"mode":"ojs"},{"id":461,"value":"y_offset_model = 177","pinned":false,"hidden":false,"mode":"ojs"},{"id":1000,"value":"theta_2_offset_model = 38.85","pinned":false,"hidden":false,"mode":"ojs"},{"id":53,"value":"x_Min = -x_Max","pinned":false,"hidden":false,"mode":"ojs"},{"id":56,"value":"x_Max = 550","pinned":false,"hidden":false,"mode":"ojs"},{"id":59,"value":"y_Min = -700","pinned":false,"hidden":false,"mode":"ojs"},{"id":62,"value":"y_Max = -1000","pinned":false,"hidden":false,"mode":"ojs"},{"id":42,"value":"theta_1_Min = -120","pinned":false,"hidden":false,"mode":"ojs"},{"id":45,"value":"theta_1_Max = 120","pinned":false,"hidden":false,"mode":"ojs"},{"id":281,"value":"theta_2_Min = -120","pinned":false,"hidden":false,"mode":"ojs"},{"id":284,"value":"theta_2_Max = 120","pinned":false,"hidden":false,"mode":"ojs"},{"id":65,"value":"x = Inputs.range([x_Min, x_Max], {value: initial_position[0], step: 0.1, label: htl.html`x [mm]`, disabled: false})","pinned":true,"hidden":false,"mode":"ojs"},{"id":553,"value":"mutable x_value = 0","pinned":true,"hidden":false,"mode":"ojs"},{"id":69,"value":"y = Inputs.range([y_Min, y_Max], {value: initial_position[1], step: 0.1, label: htl.html`y [mm]`, disabled: false})","pinned":true,"hidden":false,"mode":"ojs"},{"id":556,"value":"mutable y_value = 0","pinned":true,"hidden":false,"mode":"ojs"},{"id":1351,"value":"mutable y_value_offset = 0","pinned":true,"hidden":false,"mode":"ojs"},{"id":197,"value":"theta_1_A = Inputs.range([theta_1_Min, theta_1_Max], {value: initial_position[2], step: 0.01, label: htl.html` θ_1A [°]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs"},{"id":1498,"value":"theta_1_A.value = theta_1_A_IK[IK_solution_A.indices[0]]","pinned":true,"hidden":false,"mode":"ojs"},{"id":202,"value":"theta_2_A = Inputs.range([theta_2_Min, theta_2_Max], {value: initial_position[3], step: 0.01, label: htl.html` θ_2A [°]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs"},{"id":1603,"value":"theta_2_A.value = theta_2_A_IK[IK_solution_A.indices[1]]","pinned":true,"hidden":false,"mode":"ojs"},{"id":204,"value":"theta_1_B = Inputs.range([theta_1_Min, theta_1_Max], {value: initial_position[4], step: 0.01, label: htl.html` θ_1B [°]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs"},{"id":1016,"value":"theta_1_B.value = theta_1_B_IK[IK_solution_B.indices[0]]","pinned":true,"hidden":false,"mode":"ojs"},{"id":207,"value":"theta_2_B = Inputs.range([theta_2_Min, theta_2_Max], {value: initial_position[5], step: 0.01, label: htl.html` θ_2B [°]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs"},{"id":993,"value":"theta_2_B.value = theta_2_B_IK[IK_solution_B.indices[1]]","pinned":true,"hidden":false,"mode":"ojs"},{"id":96,"value":"#### Transformation matrices","pinned":false,"hidden":false,"mode":"md"},{"id":101,"value":"// Function to multiply two 4x4 matrices\n// M = M1.M2\nfunction multiplyMatrices4x4(M1, M2) {\n // Initialize a 4x4 result matrix with zeros\n const M = Array.from({ length: 4 }, () => Array(4).fill(0));\n\n // Perform matrix multiplication\n for (let i = 0; i < 4; i++) {\n for (let j = 0; j < 4; j++) {\n for (let k = 0; k < 4; k++) {\n M[i][j] += M1[i][k] * M2[k][j];\n }\n }\n }\n\n return M;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":104,"value":"// Function to create the transformation matrix T01(theta)\nT01 = function (theta) {\n const thetaRadians = theta * (Math.PI / 180);\n const cosTheta = Math.cos(thetaRadians);\n const sinTheta = Math.sin(thetaRadians);\n\n // Define the transformation matrix T01(theta)\n const T = [\n [cosTheta, -sinTheta, 0, L0],\n [sinTheta, cosTheta, 0, 0],\n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ];\n\n return T;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1475,"value":"// Function to create the transformation matrix T01(theta)\nT01A = function (theta) {\n const thetaRadians = theta * (Math.PI / 180);\n const cosTheta = Math.cos(thetaRadians);\n const sinTheta = Math.sin(thetaRadians);\n\n // Define the transformation matrix T01(theta)\n const T = [\n [cosTheta, -sinTheta, 0, -L0],\n [sinTheta, cosTheta, 0, 0],\n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ];\n\n return T;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":107,"value":"// Function to create the transformation matrix T12(theta)\nT12 = function(theta) {\n const thetaRadians = theta * (Math.PI / 180);\n const cosTheta = Math.cos(thetaRadians);\n const sinTheta = Math.sin(thetaRadians);\n\n // Define the transformation matrix T12(theta)\n const T = [\n [cosTheta, -sinTheta, 0, L1],\n [sinTheta, cosTheta, 0, 0], \n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ];\n\n return T;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":1477,"value":"// Function to create the transformation matrix T12(theta)\nT12A = function(theta) {\n const thetaRadians = theta * (Math.PI / 180);\n const cosTheta = Math.cos(thetaRadians);\n const sinTheta = Math.sin(thetaRadians);\n\n // Define the transformation matrix T12(theta)\n const T = [\n [cosTheta, -sinTheta, 0, -L1],\n [sinTheta, cosTheta, 0, 0], \n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ];\n\n return T;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":483,"value":"// Function to create the transformation matrix T23(theta)\nT23 = function(theta) {\n const thetaRadians = theta * (Math.PI / 180);\n const cosTheta = Math.cos(thetaRadians);\n const sinTheta = Math.sin(thetaRadians);\n\n // Define the transformation matrix T12(theta)\n const T = [\n [cosTheta, -sinTheta, 0, 0],\n [sinTheta, cosTheta, 0, -L2], \n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ];\n\n return T;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":486,"value":"T34 = [\n [1, 0, 0, -r],\n [0, 1, 0, -h],\n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ];","pinned":false,"hidden":false,"mode":"ojs"},{"id":1482,"value":"T34A = [\n [1, 0, 0, r],\n [0, 1, 0, -h],\n [0, 0, 1, 0],\n [0, 0, 0, 1]\n ];","pinned":false,"hidden":false,"mode":"ojs"},{"id":1472,"value":"mutable T04_A = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]","pinned":false,"hidden":false,"mode":"ojs"},{"id":1114,"value":"mutable T04_B = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]","pinned":false,"hidden":false,"mode":"ojs"},{"id":192,"value":"degToRad = (deg) => deg * Math.PI / 180 ;","pinned":false,"hidden":false,"mode":"ojs"},{"id":440,"value":"radToDeg = (rad) => rad * 180 / Math.PI;","pinned":false,"hidden":false,"mode":"ojs"},{"id":110,"value":"#### 3D model parameters\n*Note about IDs: uid is the ID of the model loaded from Sketchfab / IDs of the nodes were automatically generated during conversion of the model in GLB format. Theses are the nodes that are being manipulated in this notebook.*","pinned":false,"hidden":false,"mode":"md"},{"id":74,"value":"// Sketchfab 3D model's reference ID\nuid = \"c044432c755e43fb97d1d097d134ba1b\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":117,"value":"L1A_ID = \"Axis 1A_6\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":121,"value":"L2A_ID = \"Axis 2A_4\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":124,"value":"L1B_ID = \"Axis 1B_11\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":126,"value":"L2B_ID = \"Axis 2B_9\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":130,"value":"end_effector_ID = \"End effector_12\"","pinned":false,"hidden":false,"mode":"ojs"},{"id":71,"value":"#### Sketchfab Viewer API","pinned":false,"hidden":false,"mode":"md"},{"id":78,"value":"Sketchfab = require(await FileAttachment(\"sketchfab-viewer-1.11.0.js\").url())","pinned":false,"hidden":false,"mode":"ojs"},{"id":186,"value":"loadSketchfab(uid, \"api-frame\");","pinned":true,"hidden":false,"mode":"ojs"},{"id":81,"value":"loadSketchfab = (sceneuid, elementId) => {\n const iframe = document.getElementById(elementId);\n const client = new Sketchfab(\"1.11.0\", iframe);\n\n client.init(sceneuid, {\n success: load_success,\n error: () => console.error(\"Sketchfab API error\"),\n ui_controls: 0,\n ui_infos: 0,\n ui_watermark: 0,\n ui_stop: 0,\n preload: 1,\n autostart: 1,\n orbit_constraint_pan: 1,\n });\n};","pinned":false,"hidden":false,"mode":"ojs"},{"id":83,"value":"getNodeByName = (nodemap, nodename) => {\n return Object.values(nodemap).find((node) => {\n if (node.type === \"MatrixTransform\" && node.name === nodename) {\n return node;\n }\n });\n};","pinned":false,"hidden":false,"mode":"ojs"},{"id":85,"value":"function getNodeCoordinates(api, instanceID) {\n api.getMatrix(instanceID, function(err, matrix) {\n\n // Log the entire matrix for debugging\n console.log(\"Matrix:\", matrix);\n\n // Extract the translation part of the matrix\n const x = matrix.world[12];\n const y = matrix.world[13];\n const z = matrix.world[14];\n\n // Update the coordinates display\n updateCoordinatesDisplay(x, y, z);\n\n });\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":87,"value":"// Function to update the coordinates display in the

\nfunction updateCoordinatesDisplay(x, y, z) {\n const displayDiv = document.getElementById('coordinates-display');\n displayDiv.innerHTML = `Coordinates: X=${x.toFixed(2)}, Y=${y.toFixed(2)}, Z=${z.toFixed(2)}`;\n}","pinned":false,"hidden":false,"mode":"ojs"},{"id":89,"value":"// Rotate a node by a certain angle (in radians)\nrotateNode = (api, instanceID, angle, RX, RY, RZ) => {\n api.rotate(\n instanceID,\n [angle, RX, RY, RZ],\n {\n duration: 0.0,\n easing: \"linear\"\n }\n );\n};","pinned":false,"hidden":false,"mode":"ojs"},{"id":91,"value":"translateNode = (api, instanceID, translationVector) => {\n api.translate(\n instanceID, \n translationVector, \n {\n duration: 0.0,\n easing: \"linear\"\n }\n );\n};","pinned":false,"hidden":false,"mode":"ojs"},{"id":93,"value":"$34","pinned":true,"hidden":false,"mode":"ojs"}],"files":"$W35","stdlib":"1","titling":"auto"}