29:null 2a:["$","title",null,{"children":"Inverse Kinematics: Go2 Robot's Leg | Christophe Yamahata | Observable"}] 2f:T426,

This notebook showcases the animation of the leg mechanism of a robot dog using sliders, leveraging Sketchfab's Viewer API to embed and animated 3D models into web applications. We explore two key approaches for controlling the leg's movement:

Forward Kinematics (FK): In the FK method, the joint angles — ${tex`\theta_1,\theta_2,\theta_3`} — are controlled via sliders. Adjusting these angles directly animates the 3D model. Additionally, the overall transformation matrix of the robotic system is used to calculate the position of the end effector, represented by a cube placed beneath the dog's paw.
Inverse Kinematics (IK): With the IK approach, the user specifies the desired final position — ${tex`x,y,z`} — of the foot (the dog's paw) using sliders. The underlying script then automatically computes the necessary joint angles (hip, thigh, and calf) to move the leg to the targeted position.

30:T55c,container = html`

Position B (IK)

${x_cube_B} ${y_cube_B} ${z_cube_B}
↳ Calculated angles: ${theta_1_B} ${theta_2_B} ${theta_3_B}

`31:T5fa,tex` \qquad T_{01}(\theta_1) = \begin{bmatrix} \cos(\theta_1) & -\sin(\theta_1) & 0 & 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} 1 & 0 & 0 & 0 \\ 0 & \cos(\theta_2) & -\sin(\theta_2) & 0 \\ 0 & \sin(\theta_2) & \cos(\theta_2) & L_{1} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \qquad T_{23}(\theta_3) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos(\theta_3) & -\sin(\theta_3) & L_{2} \\ 0 & \sin(\theta_3) & \cos(\theta_3) & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \\ \qquad T_{34} = \begin{bmatrix} 1 & 0 & 0 & L_{4} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & L_{3} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \\ \quad \\ \qquad \textrm{where } L_{1} = {\bf ${L1.toFixed(0)}} \textrm{ mm is the distance in the z-direction from the body base to Joint 2 (J2).} \\ \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 \;\;\; L_{3} = {\bf ${L3.toFixed(0)}} \textrm{ mm is the distance between Joint 3 (J3) and the paw. It is along the z-axis in the rest position.} \\ \qquad \textrm{and } \;\;\;\, L_{4} = {\bf ${L4.toFixed(0)}} \textrm{ mm is the off-axis distance in the x-direction between Joint 1 (J1) and the paw.} `32:T433,tex` \qquad θ_1 = ${theta_1_A.value}°, \; θ_2 = ${theta_2_A.value}°, θ_3 = ${theta_3_A.value}° \\ \; \\ \qquad T_{04} = \begin{bmatrix} ${T04_cube_A[0][0].toFixed(3)} & ${T04_cube_A[0][1].toFixed(3)} & ${T04_cube_A[0][2].toFixed(3)} & {\bf ${T04_cube_A[0][3].toFixed(1)} } \\ ${T04_cube_A[1][0].toFixed(3)} & ${T04_cube_A[1][1].toFixed(3)} & ${T04_cube_A[1][2].toFixed(3)} & {\bf ${T04_cube_A[1][3].toFixed(1)} } \\ ${T04_cube_A[2][0].toFixed(3)} & ${T04_cube_A[2][1].toFixed(3)} & ${T04_cube_A[2][2].toFixed(3)} & {\bf ${T04_cube_A[2][3].toFixed(1)} } \\ ${T04_cube_A[3][0].toFixed(0)} & ${T04_cube_A[3][1].toFixed(0)} & ${T04_cube_A[3][2].toFixed(0)} & ${T04_cube_A[3][3].toFixed(0)} \\ \end{bmatrix} \qquad \Rightarrow \quad x = {\bf ${x_cube_A_value.toFixed(1)} } \textrm{ mm, } y = {\bf ${y_cube_A_value.toFixed(1)} } \textrm{ mm, } z = {\bf ${z_cube_A_value.toFixed(1)} } \textrm{ mm, } ` 33:T427,digraph1 = dot` digraph { node [shape=plaintext]; rankdir=LR; a [label=<

θ₁

θ₂

θ₃

>]; b [label=<

>]; FK [label=<

Forward
Kinematics
(FK)

>]; a -> FK -> b; }`; 34:T88c,theta_1_A_solutions = { const phi_A = Math.atan(y_cube_A_value/x_cube_A_value) const R = Math.pow(x_cube_A_value**2 +y_cube_A_value**2,0.5) const theta_1_A_1 = phi_A + Math.acos(L4/R) const theta_1_A_2 = phi_A - Math.acos(L4/R) const res = 0.1 const bf1 = Math.abs(radToDeg(theta_1_A_1).toFixed(1) - theta_1_A.value) <= res? 1:0 const bf2 = Math.abs(radToDeg(theta_1_A_2).toFixed(1) - theta_1_A.value) <= res? 1:0 const bf3 = Math.abs(radToDeg(theta_1_A_1 + Math.PI).toFixed(1) - theta_1_A.value) <= res? 1:0 const bf4 = Math.abs(radToDeg(theta_1_A_2 + Math.PI).toFixed(1) - theta_1_A.value) <= res? 1:0 const bf5 = Math.abs(radToDeg(theta_1_A_1 - Math.PI).toFixed(1) - theta_1_A.value) <= res? 1:0 const bf6 = Math.abs(radToDeg(theta_1_A_2 - Math.PI).toFixed(1) - theta_1_A.value) <= res? 1:0 return htl.html`

With the numerical FK inputs selected above (Position A), the possible solutions for ${tex`\theta_1`} are:
${tex`\quad `} ${bf1 ? tex`\bf{\theta_{1,1} = ${radToDeg(theta_1_A_1).toFixed(1)}°}` : tex`\theta_{1,1} = ${radToDeg(theta_1_A_1).toFixed(1)}°`}, ${tex`\quad `} ${bf2 ? tex`\bf{\theta_{1,2} = ${radToDeg(theta_1_A_2).toFixed(1)}°}` : tex`\theta_{1,2} = ${radToDeg(theta_1_A_2).toFixed(1)}°`}, ${tex`\quad `} ${bf3 ? tex`\bf{\theta_{1,3} = ${radToDeg(theta_1_A_1 + Math.PI).toFixed(1)}°}` : tex`\theta_{1,3} = ${radToDeg(theta_1_A_1 + Math.PI).toFixed(1)}°`}, ${tex`\\\quad `} ${bf4 ? tex`\bf{\theta_{1,4} = ${radToDeg(theta_1_A_2 + Math.PI).toFixed(1)}°}` : tex`\theta_{1,4} = ${radToDeg(theta_1_A_2 + Math.PI).toFixed(1)}°`}, ${tex`\quad `} ${bf5 ? tex`\bf{\theta_{1,5} = ${radToDeg(theta_1_A_1 - Math.PI).toFixed(1)}°}` : tex`\theta_{1,5} = ${radToDeg(theta_1_A_1 - Math.PI).toFixed(1)}°`}, ${tex`\quad `} ${bf6 ? tex`\bf{\theta_{1,6} = ${radToDeg(theta_1_A_2 - Math.PI).toFixed(1)}°}` : tex`\theta_{1,6} = ${radToDeg(theta_1_A_2 - Math.PI).toFixed(1)}°`}.

` }35:T94d,theta_2_A_solutions = { const r_24_sqr = x_cube_A_value**2 + y_cube_A_value**2 + (z_cube_A_value - L1)**2 const theta_3_A_1 =Math.asin((L2**2+L3**2+L4**2-r_24_sqr)/(2*L2*L3)) const A = L3*Math.cos(theta_3_A_1) const B = L2 - L3*Math.sin(theta_3_A_1) const S = Math.pow(A**2+B**2,0.5) const psi_A = Math.atan(B/A) const theta_2_A_1 = psi_A + Math.acos((z_cube_A_value-L1)/S) const theta_2_A_2 = psi_A - Math.acos((z_cube_A_value-L1)/S) const res = 0.1 const bf1 = Math.abs(radToDeg(theta_2_A_1).toFixed(1) - theta_2_A.value) <= res? 1:0 const bf2 = Math.abs(radToDeg(theta_2_A_2).toFixed(1) - theta_2_A.value) <= res? 1:0 const bf3 = Math.abs(radToDeg(theta_2_A_1 + Math.PI).toFixed(1) - theta_2_A.value) <= res? 1:0 const bf4 = Math.abs(radToDeg(theta_2_A_2 + Math.PI).toFixed(1) - theta_2_A.value) <= res? 1:0 const bf5 = Math.abs(radToDeg(theta_2_A_1 - Math.PI).toFixed(1) - theta_2_A.value) <= res? 1:0 const bf6 = Math.abs(radToDeg(theta_2_A_2 - Math.PI).toFixed(1) - theta_2_A.value) <= res? 1:0 return htl.html`

With the numerical inputs selected for Position A, the possible solutions for ${tex`\theta_2`} are:
${tex`\quad `} ${bf1 ? tex`\bf{\theta_{2,1} = ${radToDeg(theta_2_A_1).toFixed(1)}°}` : tex`\theta_{2,1} = ${radToDeg(theta_2_A_1).toFixed(1)}°`}, ${tex`\quad `} ${bf2 ? tex`\bf{\theta_{2,2} = ${radToDeg(theta_2_A_2).toFixed(1)}°}` : tex`\theta_{2,2} = ${radToDeg(theta_2_A_2).toFixed(1)}°`}, ${tex`\quad `} ${bf3 ? tex`\bf{\theta_{2,3} = ${radToDeg(theta_2_A_1 + Math.PI).toFixed(1)}°}` : tex`\theta_{2,3} = ${radToDeg(theta_2_A_1 + Math.PI).toFixed(1)}°`}, ${tex`\\\quad `} ${bf4 ? tex`\bf{\theta_{2,4} = ${radToDeg(theta_2_A_2 + Math.PI).toFixed(1)}°}` : tex`\theta_{2,4} = ${radToDeg(theta_2_A_2 + Math.PI).toFixed(1)}°`}, ${tex`\quad `} ${bf5 ? tex`\bf{\theta_{2,5} = ${radToDeg(theta_2_A_1 - Math.PI).toFixed(1)}°}` : tex`\theta_{2,5} = ${radToDeg(theta_2_A_1 - Math.PI).toFixed(1)}°`}, ${tex`\quad `} ${bf6 ? tex`\bf{\theta_{2,6} = ${radToDeg(theta_2_A_2 - Math.PI).toFixed(1)}°}` : tex`\theta_{2,6} = ${radToDeg(theta_2_A_2 - Math.PI).toFixed(1)}°`}.

` }36:T40f,### Solution In 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 robot's configuration. For the Go2 robot's leg, given the joint angle limits and segment lengths, the IK should typically result in either no solution or a unique solution. The strategy for finding a feasible IK solution, if one exists, is as follows: 1. **Ensure reachability**: Ensure that the target Cartesian coordinates are within the maximum reach of the leg. If the target is out of reach, no valid solution exists. 2. **Check joint angle limits**: For each calculated solution, verify that all joint angles fall within their allowable ranges. Discard solutions that exceed these limits. 3. **Perform final validation**: After calculating the joint angles, validate the resulting position of the end effector (the leg’s paw) to ensure it matches the target coordinates. 37:T42d,IK_solution_A = { const tolerance = 0.1; let T04_IK = null; let indices = null; for (let i = 0; i < theta_1_A_IK.length; i++) { for (let j = 0; j < theta_2_A_IK.length; j++) { for (let k = 0; k < theta_3_A_IK.length; k++) { const T01_IK = T01(theta_1_A_IK[i]); const T12_IK = T12(theta_2_A_IK[j]); const T02_IK = multiplyMatrices4x4(T01_IK, T12_IK); const T23_IK = T23(theta_3_A_IK[k]); const T03_IK = multiplyMatrices4x4(T02_IK, T23_IK); const T04_IK_ijk = multiplyMatrices4x4(T03_IK, T34); if (Math.abs(x_cube_A_value - T04_IK_ijk[0][3]) < tolerance && Math.abs(y_cube_A_value - T04_IK_ijk[1][3]) < tolerance && Math.abs(z_cube_A_value - T04_IK_ijk[2][3]) < tolerance) { T04_IK = T04_IK_ijk; indices = [i, j, k]; mutable T04_cube_A = T04_IK; break; // Break the innermost loop } } if (T04_IK !== null) break; } if (T04_IK !== null) break; } return {T04_IK, indices}; } 38:T422,The following graph summarizes the various steps followed for these calculations: 1. **Forward Kinematics (FK)** - User Inputs ${tex`(\theta_1,\theta_2,\theta_3)`}: Joint angles are provided by the user via sliders. - Compute Transformation Matrix: Calculate the overall transformation matrix based on the input joint angles. - Extract Cartesian Coordinates ${tex`(x,y,z)`}: Derive the Cartesian coordinates from the transformation matrix. 2. **Inverse Kinematics (IK)** - Inputs ${tex`(x,y,z)`}: Use the Cartesian coordinates calculated for Position A (from FK). *Note that these coordinates could also be directly provided by the user.* - Generate Potential Solutions: Identify all possible sets of joint angles. - Compute Transformation Matrices: Sequentially calculate the transformation matrix for each potential set of joint angles. - Match and Select Solution: Compare the Cartesian coordinates obtained through IK with the target coordinates to identify and select the matching solution ${tex`(\theta_1,\theta_2,\theta_3)`}.39:T6c7,digraph2 = dot` digraph { node [shape=plaintext]; rankdir=LR; a [label=<

θ₁

θ₂

θ₃

>]; b [label=<

Position A

>]; FK [label=<

Forward
Kinematics
(FK)

>]; c [label=<

θ₁

θ₂

θ₃

>]; IK [label=<

Inverse
Kinematics
(IK)

>]; a -> FK -> b -> IK -> c ; }` 3a:T48f,// This SVG file was created with Adobe Illustrator // Here is a notebook that describes how to manipulate an SVG attached as a file: https://observablehq.com/@mbostock/manipulating-svg-files // See also: https://observablehq.com/@christophe-yamahata/orbital-elements?collection=@christophe-yamahata/astronomy kinematic_diagram = { const text = await FileAttachment("kinematics_robot_leg@12.svg").text(); const document = (new DOMParser).parseFromString(text, "image/svg+xml"); const svg = d3.select(document.documentElement).remove(); // The following dimensions are updated using the parameters saved in this notebook svg.select("#L1 text").text("L1 = "+L1+" mm"); svg.select("#L2 text").text("L2 = "+L2+" mm"); svg.select("#L3 text").text("L3 = "+L3+" mm"); svg.select("#L4 text").text("L4 = "+L4+" mm"); // Adapt the Cartesian coordinate system svg.select("#x1 text").text("z"); svg.select("#x2 text").text("x"); svg.select("#x3 text").text("y"); // Apply a translation transformation to offset the SVG to the right const offsetX = 50; svg.attr("transform", `translate(${offsetX}, 0)`); return svg.node(); }3b:T46f,references = md`
--- ## References * Leg manipulation with [Forward Kinematics](https://observablehq.com/@christophe-yamahata/sketchfab-api-example-go2-robot-leg) → Forked Observable notebook * CodePen examples by Klaas Nienhuis: * [Sketchfab Livestream API basics](https://codepen.io/klaasnienhuis/pen/KKeyrZQ) * [Devfab Hero skull](https://codepen.io/klaasnienhuis/pen/MWzMEva) → Example that inspired this Observable notebook * Sketchfab resources for developers: [Sketchfab Viewer API](https://sketchfab.com/developers/viewer) * YouTube video: [Introduction to the Sketchfab Viewer API](https://www.youtube.com/watch?v=mVQNDCwbXMM) with Klaas Nienhuis * 3D models of the Unitree Go2 Robot's Leg * [Low resolution](https://sketchfab.com/3d-models/unitree-go2-robot-ik-with-sketchfab-viewer-api-adba00549b0d49eaad5ffa3729ce6589) (version used in this notebook) * [Higher resolution](https://sketchfab.com/3d-models/unitree-go2-robots-leg-sketchfab-viewer-api-d84607a0dc864921826cb092364d3990) * [SymPy](https://docs.sympy.org/latest/tutorials/intro-tutorial/intro.html) library for symbolic computation `3c:Tca9,python_script_sympy = ` ''' This Python script calculates the Inverse Kinematics (IK) of a 3-DOF robotic arm with three rotational joints (angles θ1, θ2, θ3) using # SymPy for symbolic mathematics. The input is the Cartesian coordinates # (x, y, z) of the end effector. ________ | | (x, y, z) ==> | IK | ==> (θ1, θ2, θ3) |________| ''' from sympy import * # Define symbols for Cartesian coordinates, joint angles, and link lengths x, y, z, θ1, θ2, θ3 = symbols('x y z θ1 θ2 θ3') L1, L2, L3, L4 = symbols('L1 L2 L3 L4') # ANSI escape codes for text styling COLOR = '\\033[38;2;255;165;0m' # Orange color ITALIC = '\\033[3m' # Italic text RESET = '\\033[0m' # Reset text formatting # Define the transformation matrices for each joint T01 = Matrix([ [cos(θ1), -sin(θ1), 0, 0], [sin(θ1), cos(θ1), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1] ]) T12 = Matrix([ [1, 0, 0, 0], [0, cos(θ2), -sin(θ2), 0], [0, sin(θ2), cos(θ2), L1], [0, 0, 0, 1] ]) T23 = Matrix([ [1, 0, 0, 0], [0, cos(θ3), -sin(θ3), L2], [0, sin(θ3), cos(θ3), 0], [0, 0, 0, 1] ]) T34 = Matrix([ [1, 0, 0, L4], [0, 1, 0, 0], [0, 0, 1, L3], [0, 0, 0, 1] ]) # Compute the overall transformation matrix from base to end-effector T04 = simplify(T01 * T12 * T23 * T34) # Compute Cartesian coordinates (x, y, z) from the transformation matrix V = T04 * Matrix([0, 0, 0, 1]) x_expr, y_expr, z_expr = V[0], V[1], V[2] # Display Forward Kinematics results print(f"{COLOR}\\nForward Kinematics: (θ1, θ2, θ3) ==> (x, y, z){RESET}") print(" φ = θ2 + θ3") print(f" x = {x_expr.subs({θ2 + θ3: symbols('φ')})}") print(f" y = {y_expr.subs({θ2 + θ3: symbols('φ')})}") print(f" z = {z_expr.subs({θ2 + θ3: symbols('φ')})}\\n") # Inverse Kinematics Solutions print(f"{COLOR} ________ {RESET}") print(f"{COLOR} | | {RESET}") print(f"{COLOR} (x, y, z) ==> | IK | ==> (θ1, θ2, θ3) {RESET}") print(f"{COLOR} |________| {RESET}\\n") # Solution for θ1 resultForθ1 = x_expr * cos(θ1) + y_expr * sin(θ1) print(f"{COLOR}\\nIK solution for θ1: θ1 = f(x, y){RESET}") print(f" x*cos(θ1) + y*sin(θ1) = {simplify(resultForθ1)}") # Solution for θ3 resultForθ3 = x_expr**2 + y_expr**2 + (z_expr - L1)**2 print(f"{COLOR}\\nIK solution for θ3: θ3 = f(x, y, z){RESET}") print(f" x**2 + y**2 + (z - L1)**2 = {simplify(resultForθ3)}") # Solution for θ2 A = L3 * cos(θ3) B = L2 - L3 * sin(θ3) S_expr = sqrt(A**2 + B**2) ψ_expr = atan2(B, A) z_minus_L1 = S_expr * cos(θ2 - ψ_expr) print(f"{COLOR}\\nIK solution for θ2: θ2 = f(z, θ3) = f(x, y, z){RESET}") print(f" z - L1 = {expand_trig(z_expr - L1)}") print(f" ↳ {ITALIC}This expression can be rewritten as:{RESET}") print(f" z - L1 = {z_minus_L1.subs({S_expr: symbols('S'), ψ_expr: symbols('ψ')})}") print(f" with S = {simplify(S_expr)}") print(f" and ψ = {ψ_expr}") print(f" ↳ {ITALIC}Verification with SymPy:{RESET}") print(f" z - L1 - S*cos(θ2 - ψ) = {simplify(z_expr - L1 - z_minus_L1)}\\n") `3d:T4af,IK_solution_B = { const tolerance = 1; let T04_IK = null; let indices = null; mutable x_cube_B_value = x_cube_B.value; mutable y_cube_B_value = y_cube_B.value; mutable z_cube_B_value = z_cube_B.value; for (let i = 0; i < theta_1_B_IK.length; i++) { for (let j = 0; j < theta_2_B_IK.length; j++) { for (let k = 0; k < theta_3_B_IK.length; k++) { const T01_IK = T01(theta_1_B_IK[i]); const T12_IK = T12(theta_2_B_IK[j]); const T02_IK = multiplyMatrices4x4(T01_IK, T12_IK); const T23_IK = T23(theta_3_B_IK[k]); const T03_IK = multiplyMatrices4x4(T02_IK, T23_IK) const T04_IK_ijk = multiplyMatrices4x4(T03_IK, T34); if (Math.abs(x_cube_B_value - T04_IK_ijk[0][3]) < tolerance && Math.abs(y_cube_B_value - T04_IK_ijk[1][3]) < tolerance && Math.abs(z_cube_B_value - T04_IK_ijk[2][3]) < tolerance) { T04_IK = T04_IK_ijk; indices = [i, j, k]; mutable T04_cube_B = T04_IK; break; // Break the innermost loop } } if (T04_IK !== null) break; } if (T04_IK !== null) break; } return {T04_IK, indices}; }3e:T69f,disabled_AB = { mutable position_value = position.value mutable animation_loop_value = animation_loop.value const fieldsetA = document.getElementById('posA'); const fieldsetB = document.getElementById('posB'); const display_AB = document.getElementById('display_position'); // Toggle fieldset properties based on the values of "Position" and "Animation loop" if (animation_loop_value) { // Disable both fieldsets if animation_loop is active fieldsetA.setAttribute('disabled', true); fieldsetA.style.border = '1px solid #ccc'; fieldsetB.setAttribute('disabled', true); fieldsetB.style.border = '1px solid #ccc'; display_AB.style.visibility = 'hidden'; } else { display_AB.style.visibility = 'visible'; // Enable or disable fieldsets based on position_value if (position_value === "A") { fieldsetA.removeAttribute('disabled'); fieldsetA.style.border = '1px solid #00f'; fieldsetB.setAttribute('disabled', true); fieldsetB.style.border = '1px solid #ccc'; } else { fieldsetA.setAttribute('disabled', true); fieldsetA.style.border = '1px solid #ccc'; fieldsetB.removeAttribute('disabled'); fieldsetB.style.border = '1px solid #00f'; } } /* if (position_value == "A" ) { fieldsetA.removeAttribute('disabled'); fieldsetA.style.border = '1px solid #00f'; fieldsetB.setAttribute('disabled', true); fieldsetB.style.border = '1px solid #ccc'; } else { fieldsetA.setAttribute('disabled', true); fieldsetA.style.border = '1px solid #ccc'; fieldsetB.removeAttribute('disabled'); fieldsetB.style.border = '1px solid #00f'; } */ }3f:T21f5,load_success = (api) => { api.start(function () { api.addEventListener("viewerready", function () { api.getNodeMap(function (err, nodes) { // console.log(nodes); const hip = getNodeByName(nodes, hip_ID); const thigh = getNodeByName(nodes, thigh_ID); const gear1 = getNodeByName(nodes, gear1_ID); const gear2 = getNodeByName(nodes, gear2_ID); const lever = getNodeByName(nodes, lever_ID); const calf = getNodeByName(nodes, calf_ID); const paw = getNodeByName(nodes, paw_ID); // Update once at load: // Directly invoke the function with the current value on page load (function initialize() { const wireframeEnabled = wireframe.value; api.setWireframe(wireframeEnabled); updateOpacity(); cubeVisibility(); updateMatrix(); })(); // Enable/disable wireframe wireframe.addEventListener('input', function (event) { const wireframeEnabled = wireframe.value; api.setWireframe(wireframeEnabled); }); // Hide/show cube Hide_cube.addEventListener('input', cubeVisibility); function cubeVisibility() { const visibilityValue = (Hide_cube.value || animation_loop.value)? 0 : 1; api.getMaterialList(function (err, materials) { const cubeMaterial_1 = materials.find(material => material.name === "cube"); const cubeMaterial_2 = materials.find(material => material.name === "cube_red"); const cubeMaterial_3 = materials.find(material => material.name === "cube_green"); const cubeMaterial_4 = materials.find(material => material.name === "cube_blue"); cubeMaterial_1.channels["Opacity"].factor = visibilityValue; cubeMaterial_2.channels["Opacity"].factor = visibilityValue; cubeMaterial_3.channels["Opacity"].factor = visibilityValue; cubeMaterial_4.channels["Opacity"].factor = visibilityValue; api.setMaterial(cubeMaterial_1); api.setMaterial(cubeMaterial_2); api.setMaterial(cubeMaterial_3); api.setMaterial(cubeMaterial_4); }) } // Retrieve the materials from the model and set opacity of the thigh opacity.addEventListener('input', updateOpacity); function updateOpacity() { const opacityValue = opacity.value; api.getMaterialList(function (err, materials) { const thighMaterial_1 = materials.find(material => material.name === "Thigh"); const thighMaterial_2 = materials.find(material => material.name === "thigh_protection"); thighMaterial_1.channels["Opacity"].factor = opacityValue; thighMaterial_2.channels["Opacity"].factor = opacityValue; api.setMaterial(thighMaterial_1); api.setMaterial(thighMaterial_2); }) } // 3D model is reactive to the slider value changes [position, theta_1_A, theta_2_A, theta_3_A, x_cube_B, y_cube_B, z_cube_B].forEach(slider => { slider.addEventListener('input', updateMatrix); }); // 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_cube_B, y_cube_B, z_cube_B].forEach(slider => { slider.addEventListener("mouseup", () => { 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() { const pos_A = (position.value == "A")? 1: 0; const T01_cube = pos_A? T01(theta_1_A.value) : T01(theta_1_B.value); const T12_cube = pos_A? T12(theta_2_A.value) : T12(theta_2_B.value); const T02_cube = multiplyMatrices4x4(T01_cube,T12_cube); const T23_cube = pos_A? T23(theta_3_A.value) : T23(theta_3_B.value); const T03_cube = multiplyMatrices4x4(T02_cube,T23_cube); const T04 = multiplyMatrices4x4(T03_cube,T34); pos_A? mutable T04_cube_A = T04 : mutable T04_cube_B = T04; const angle1 = pos_A? -degToRad(theta_1_A.value) : -degToRad(theta_1_B.value); const angle2 = pos_A? -degToRad(theta_2_A.value) : -degToRad(theta_2_B.value); const angle3 = pos_A? -degToRad(theta_3_A.value) : -degToRad(theta_3_B.value); // Orientation of the 3D model differs from notebook: (x,y,z) -> (z,x,y) <=> (0,1,2) -> (2,0,1) translateNode(api, paw.instanceID, [mm_to_inch*T04[2][3], mm_to_inch*T04[0][3], mm_to_inch*T04[1][3]]); // Hip rotation (Joint J1): Action on the node "hip" affects all children objects. rotateNode(api, hip.instanceID, angle1, 1, 0, 0); // Thigh rotation (Joint J2): Action on the node "thigh" affects all children objects. rotateNode(api, thigh.instanceID, angle2, 0, 1, 0); // Calf rotation (Joint J3): The mechanism consists of // (i) Two gears (gear 1, gear 2 / gear ratio 2:1); (ii) Lever; (iii) Calf // Workaround for GLB format limitation: // The lever is defined as a child of gear 2. // However, we need to compensate for the rotation, as we only want to translate the lever. rotateNode(api, lever.instanceID, -angle3, 0, 1, 0); rotateNode(api, gear1.instanceID, -2 * angle3, 0, 1, 0); rotateNode(api, gear2.instanceID, angle3, 0, 1, 0); rotateNode(api, calf.instanceID, angle3, 0, 1, 0); } // Event listener to start the animation loop animation_loop.addEventListener('input', function (event) { cubeVisibility(); // We hide/show the cube animation(); // We start/stop the animation loop }); // Animation loop setup const n = 150; let step = 0; // Declare step at the top level for scope visibility let isAnimating = false; // Flag to control the animation loop function animation() { // Start the animation only if not already running if (!isAnimating) { isAnimating = true; const startTime = performance.now(); const animate = (currentTime) => { // Check if the animation_loop is still active if (animation_loop.value) { // Calculate elapsed time and progress const elapsedTime = currentTime - startTime; const progress = (elapsedTime / (n * 15)) % 1; // Calculate angles based on the current step const angle1 = updateTheta(theta_1_A.value, theta_1_B.value, progress); const angle2 = updateTheta(theta_2_A.value, theta_2_B.value, progress); const angle3 = updateTheta(theta_3_A.value, theta_3_B.value, progress); // Apply transformations applyTransformations(angle1, angle2, angle3); // Continue the animation loop requestAnimationFrame(animate); } else { // Stop the animation when animation_loop is unchecked isAnimating = false; updateMatrix(); // At the end of animation, reset the model to selected position (A or B) } }; // Start the animation requestAnimationFrame(animate); } } // Function to update theta with smooth behavior function updateTheta(theta_A, theta_B, progress) { let phase = (progress < 0.5) ? 2 * progress : 2 * (1 - progress); let easingValue = 3 * Math.pow(phase, 2) - 2 * Math.pow(phase, 3); return -degToRad(theta_A + (theta_B - theta_A) * easingValue); } // Function to apply transformations to the 3D model function applyTransformations(angle1, angle2, angle3) { rotateNode(api, hip.instanceID, angle1, 1, 0, 0); rotateNode(api, thigh.instanceID, angle2, 0, 1, 0); rotateNode(api, lever.instanceID, -angle3, 0, 1, 0); rotateNode(api, gear1.instanceID, -2 * angle3, 0, 1, 0); rotateNode(api, gear2.instanceID, angle3, 0, 1, 0); rotateNode(api, calf.instanceID, angle3, 0, 1, 0); } }); }); }); };40:T50a,layout = { let _id = 0; return function(layout) { return function * (parts, ...args) { yield html`

...loading...

`; const str = parts.map((s, i) => s + (i < args.length ? args[i] : "")).join(""); const svg = graphviz.layout(str, "svg", layout); const id = ++_id; yield html`\

${svg}

`; setTimeout((id) => { var svg = d3.select(`#id_${id}`).select("svg") .attr("width", `${width_graphviz}px`) ; var g = svg.select("g"); var [x, y, _width, height] = svg.attr("viewBox").split(" "); var zoom = d3.zoom(); svg.call(zoom .extent([[0, 0], [width, height]]) .scaleExtent([0.1, 8]) .on("zoom", ({ transform }) => g.attr("transform", transform)) ); svg.call(zoom.translateTo, width / 2, -height / 2); svg.on(".zoom", null); // This removes the zoom event listeners }, 1000, id); } } }41:[{"id":"beaea5da52b1452197e180305f9125e6ebd4e9365a84ef4c16e3923bbc3de7ae54d5c276cee504a3e8caa2598cc7c043aa41e1775154f75b24e6eec4420d10e0","href":"https://static.observableusercontent.com/files/beaea5da52b1452197e180305f9125e6ebd4e9365a84ef4c16e3923bbc3de7ae54d5c276cee504a3e8caa2598cc7c043aa41e1775154f75b24e6eec4420d10e0","name":"sympy_script_result.PNG","type":"image/png","lastModified":1722599047975.195,"size":23912},{"id":"d4c4c380c7745bfc3baa97dee82675d3ba26be37076be68a067348bae9078dedc2dd3f8c8fb38f31a49097c77eb81dfd2569b38c53a8bfb5f156f4cb21b7bb22","href":"https://static.observableusercontent.com/files/d4c4c380c7745bfc3baa97dee82675d3ba26be37076be68a067348bae9078dedc2dd3f8c8fb38f31a49097c77eb81dfd2569b38c53a8bfb5f156f4cb21b7bb22","name":"graphviz.js","type":"application/javascript","lastModified":1722667658428.338,"size":659747},{"id":"1202f258adeb7b927452cfc7519f820267f48943dc71872274c91ef176a967fadf889728c3952b3dae4f9c7419e7637c3e5f4ca386050e9873c828c556822ad0","href":"https://static.observableusercontent.com/files/1202f258adeb7b927452cfc7519f820267f48943dc71872274c91ef176a967fadf889728c3952b3dae4f9c7419e7637c3e5f4ca386050e9873c828c556822ad0","name":"screen_capture_Blender_Go2_IK.jpg","type":"image/jpeg","lastModified":1722776070697.848,"size":215235},{"id":"2e3bf371affb664229210e70564e5d21f0ebd738b95e7265652de0000db3504da323c796ed7e669f3b0ca536aad8da9a0077f21e430615ad6b68c1381157b507","href":"https://static.observableusercontent.com/files/2e3bf371affb664229210e70564e5d21f0ebd738b95e7265652de0000db3504da323c796ed7e669f3b0ca536aad8da9a0077f21e430615ad6b68c1381157b507","name":"Go2_leg_disassembled_view.png","type":"image/png","lastModified":1722330713301.573,"size":571007},{"id":"fa33bf8c0e60adb5fe7adc48a737ab732b53e8e1f1feb6cc00e3b1fd36fe2149aa599002963eec9159dcad999f72135f389fc359819541b9fc2d410ec3ab7b84","href":"https://static.observableusercontent.com/files/fa33bf8c0e60adb5fe7adc48a737ab732b53e8e1f1feb6cc00e3b1fd36fe2149aa599002963eec9159dcad999f72135f389fc359819541b9fc2d410ec3ab7b84","name":"sketchfab-viewer-1.11.0.js","type":"application/javascript","lastModified":1722330713301.573,"size":6587},{"id":"adb93699809e1bc038cf83c1a03cf1af756d32e967171ab295f643aa511d666356a71e7aac234d9d0e092e0450ba41bce4d9e34b69e38ca559bef64dd6ff9932","href":"https://static.observableusercontent.com/files/adb93699809e1bc038cf83c1a03cf1af756d32e967171ab295f643aa511d666356a71e7aac234d9d0e092e0450ba41bce4d9e34b69e38ca559bef64dd6ff9932","name":"2D_drawing_Go2_leg.PNG","type":"image/png","lastModified":1722341958997.305,"size":216290},{"id":"72dc8a9b2fcbecc0e0478d9d97451bc73a380a6f23f18548031107d970a0acf9d1a29f73c68be36a296a5cedb710a1f96367f4ec7639224cd0e5d73682d550a2","href":"https://static.observableusercontent.com/files/72dc8a9b2fcbecc0e0478d9d97451bc73a380a6f23f18548031107d970a0acf9d1a29f73c68be36a296a5cedb710a1f96367f4ec7639224cd0e5d73682d550a2","name":"kinematics_robot_leg@12.svg","type":"image/svg+xml","lastModified":1722355510201.199,"size":25051}] 2d:{"id":"1dea35115e29048b","slug":"inverse-kinematics-go2-robot","title":"Inverse Kinematics: Go2 Robot's Leg","thumbnail":"5140ffc36f0ad33d2af7fefeb28209f1b7c033bbff234973ce4aac6986482455","access_level":"public","create_time":"$D2024-07-30T09:11:53.301Z","update_time":"$D2024-08-22T12:41:42.212Z","view_time":null,"first_public_time":"$D2024-08-04T16:25:00.617Z","has_unpublished_edits":null,"fork":{"id":"c22007da01448c97","slug":"sketchfab-api-example-go2-robot-leg","title":"Sketchfab API Example: Go2 Robot's Leg","owner":{"id":"dd67891d326bcfb1","login":"christophe-yamahata","name":"Christophe Yamahata"}},"stars":3,"starred":false,"owner":{"type":"team","id":"dd67891d326bcfb1","login":"christophe-yamahata","name":"Christophe Yamahata","avatar_url":"https://avatars-next.observableusercontent.com/8256ac716aa8395babba412b2835913d186874fa4190df57a4a28f2e60129876","tier":"starter_2024"},"version":7699,"publish_version":null,"role":null,"authored":false,"authors":[{"type":"individual","id":"06f083eba60f5a76","login":"christophe-yamahata","name":"Christophe Yamahata","avatar_url":"https://avatars-next.observableusercontent.com/8256ac716aa8395babba412b2835913d186874fa4190df57a4a28f2e60129876","tier":null}],"members":[{"type":"individual","id":"06f083eba60f5a76","login":"christophe-yamahata","name":"Christophe Yamahata","avatar_url":"https://avatars-next.observableusercontent.com/8256ac716aa8395babba412b2835913d186874fa4190df57a4a28f2e60129876","tier":null,"role":"editor"},{"type":"team","id":"dd67891d326bcfb1","login":"christophe-yamahata","name":"Christophe Yamahata","avatar_url":"https://avatars-next.observableusercontent.com/8256ac716aa8395babba412b2835913d186874fa4190df57a4a28f2e60129876","tier":"starter_2024","role":"viewer"}],"comments":[],"custom_thumbnail":true,"trashed":false,"lapsed":null,"subscribed":null,"forks":3,"collection":{"id":"0e48f0602412f50d","slug":"3d-three-js","title":"3D (three.js, Sketchfab)","owner":{"id":"dd67891d326bcfb1","login":"christophe-yamahata","name":"Christophe Yamahata"}},"type":"notebook","body":{"title":"Inverse Kinematics: Go2 Robot's Leg","cells":[{"id":0,"value":"# Inverse Kinematics: Go2 Robot's Leg\n## Animate the leg mechanism of the Unitree Go2 robot dog using sliders","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":114,"value":"$2f","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":7088,"value":"---","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":800,"value":"inputs = md`\n\n### Interactive control\n*Use the sliders and checkboxes to manipulate the 3D model.*\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2450,"value":"opacity = Inputs.range([0,1], {value: 0.15, step: 0.05, label: htl.html`Thigh opacity`})","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2461,"value":"wireframe = Inputs.toggle({label: htl.html`Wireframe`, value: false})","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":7040,"value":"Hide_cube = Inputs.toggle({label: htl.html`Cube hidden`, value: false})","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":7,"value":"\n\n
\n \n
","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3998,"value":"$30","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":7409,"value":"
\n ${position}\n
","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":7260,"value":"animation_loop = Inputs.toggle({label: htl.html`Animation loop`, value: false})","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":7428,"value":"*Start the animation loop to observe the robot's leg moving back and forth between position A and position B. During the animation, the cube will be temporarily hidden, and the fieldsets will be disabled to prevent unintended interactions.*","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":340,"value":"FK = md`---\n## Forward Kinematics (FK)\n\n*To understand the following calculations, please refer to the [kinematic diagram](#kinematic_diagram) depicted below.*\n\nIn robotics and computer animation, forward kinematics is used to determine the position and orientation of an end effector based on the joint angles of a mechanism.\n\nThe leg is a 3-degree-of-freedom (3-DOF) mechanism. Its structure can be broken down into 4 key blocks corresponding to the 3 leg's joints plus the end effector.\n* **Hip** (Joint 1, angle θ₁) — The hip is attached to the robot's body. Joint 1 (J1) allows for hip rotation.\n* **Thigh** (Joint 2, angle θ₂)— Joint 2 (J2) allows for thigh rotation.\n* **Calf** (Joint 3, angle θ₃)— Joint 3 (J3) allows for calf rotation.\n* **Paw** — The paw (foot rubber) is the end effector whose position we are looking to determine.\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":816,"value":"Here, we calculate the positions and orientations of the leg's paw (rubber foot) using three input angles (θ₁ to θ₃). The dimensions of the robot's leg, as retrieved from the 3D model (see [illustration below](#CAD_drawing)), are used in these calculations.\n
\n
\n The transformation matrices for the three joints and for the end effector are defined as follows:\n
","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":825,"value":"$31","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":864,"value":"
The transformation matrix ${tex`T_{01}`} represents the rotation around the z-axis at Joint 1 (J1). \n${tex`T_{12}`} represents the rotation around Joint 2 (J2).\n${tex`T_{23}`} represents the rotation around Joint 3 (J3). ${tex`T_{34}`} accounts for the position of the end effector (i.e., the paw) with respect to Joint 3 (J3).\n
\n
\nThe complete transformation matrix ${tex`T_{04}`} is computed from the product of the individual transformation matrices:\n
${tex`\\qquad T_{04} = T_{01}T_{12}T_{23}T_{34}`}\n
\n
\nThe cartesian coordinates ${tex`(x, y, z)`} are obtained from this matrix:\n
\n ${tex` \n \\qquad \\begin{bmatrix}\n x \\\\ y \\\\ z \\\\ 1\n \\end{bmatrix} = T_{04} \\cdot\n \\begin{bmatrix}\n 0 \\\\ 0 \\\\ 0 \\\\ 1\n \\end{bmatrix}\n `}\n","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1654,"value":"The analytical result is:","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1657,"value":"tex`\n x = -L_2 \\sin(θ_1)\\cos(θ_2) + L_3\\sin(θ_1)\\sin(θ_2 + θ_3) + L_4\\cos(θ_1) \\\\\n\n y = L_2\\cos(θ_1)\\cos(θ_2) - L_3\\cos(θ_1)\\sin(θ_2 + θ_3) + L_4\\sin(θ_1) \\\\\n\n z = L_1 + L_2\\sin(θ_2) + L_3\\cos(θ_2 + θ_3)\n\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":895,"value":"
\n Here are the numerical results for the input angles selected above (refer to the inset for the leg's rest position): \n
","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":897,"value":"$32","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3718,"value":"With Forward Kinematics (FK), the Cartesian coordinates are uniquely specified. They are directly derived from the transformation matrix calculated using the input joint angles:","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":7586,"value":"$33","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1668,"value":"
\n---\n## Inverse Kinematics (IK)\nThe analytical expressions for the Cartesian coordinates ${tex`(x, y, z)`} and the computations detailed hereafter were derived using the Python library SymPy for symbolic mathematics. Click on the button to copy the [Python script](#python_script_sympy) to your clipboard.\n","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1995,"value":"Inputs.button(\"Copy Python script to clipboard\", {value: null, reduce: () => navigator.clipboard.writeText(python_script_sympy)})","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2000,"value":"
\n
\n\n
Symbolic computation with SymPy – Results obtained with the Python script.\n
\n
","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1819,"value":"### Joint 1, ${tex`\\theta_1`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1671,"value":"To solve for ${tex`\\theta_1`}, we use the analytical equations obtained for ${tex`x`} and ${tex`y`}. We find:\n\n${tex`\\qquad x\\cos(\\theta_1)+y\\sin(\\theta_1) = L_4`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1680,"value":"This expression can be rewritten as:\n\n${tex`\n \\qquad L_4 = R\\cos(\\theta_1-\\phi) \\\\\n \\qquad \\textrm{with } \\; R = \\sqrt{x^2+y^2} \\; \\textrm{ and } \\; \\phi = \\arctan(\\frac{y}{x})\n`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1701,"value":"Hence, the solution for ${tex`\\theta_1`} is obtained by solving:\n\n${tex`\n \\qquad \\theta_1 = \\phi \\pm \\arccos(\\frac{L_4}{R}) + k\\pi\\\\\n \\qquad \\textrm{with } k \\in \\{-1,0,1\\}\n\n`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1710,"value":"$34","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1811,"value":"Note that given the angular range of the joint J1, several solutions could be eliminated immediately.","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1822,"value":"### Joint 3, ${tex`\\theta_3`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1824,"value":"To solve for ${tex`\\theta_3`}, one can observe that the distance ${tex`r_{24}`} from the thigh rotation axis (joint J2) to the end effector (paw) only depends on ${tex`\\theta_3`}. We have:\n\n ${tex`\n \\qquad \\begin{aligned}\n r_{24}^2 &= x^2 + y^2 + (z - L_1)^2 \\\\\n & = L_2^2 + L_3^2 + L_4^2 - 2 L_2 L_3 \\sin(\\theta_3) \n \\end{aligned}\n`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1851,"value":"Hence, the solution for ${tex`\\theta_3`} is obtained by solving:\n\n${tex`\n \\qquad \\theta_3 = \\pm \\arcsin\\Big(\\frac{L_2^2 + L_3^2+ L_4^2 - x^2 - y^2 - (z-L_1)^2}{2 L_2 L_3}\\Big)\n`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1865,"value":"theta_3_A_solutions = {\n const r_24_sqr = x_cube_A_value**2 + y_cube_A_value**2 + (z_cube_A_value - L1)**2\n const theta_3_A_1 = Math.asin((L2**2+L3**2+L4**2-r_24_sqr)/(2*L2*L3))\n const theta_3_A_2 = -theta_3_A_1\n const res = 0.1\n const bf1 = Math.abs(radToDeg(theta_3_A_1).toFixed(1) - theta_3_A.value) <= res? 1:0\n const bf2 = Math.abs(radToDeg(theta_3_A_2).toFixed(1) - theta_3_A.value) <= res? 1:0 \n return htl.html`
With the numerical inputs selected for Position A, the possible solutions for ${tex`\\theta_3`} are:\n
\n ${tex`\\quad `} ${bf1 ? tex`\\bf{\\theta_{3,1} = ${radToDeg(theta_3_A_1).toFixed(1)}°}` : tex`\\theta_{3,1} = ${radToDeg(theta_3_A_1).toFixed(1)}°`} and \n ${tex`\\quad `} ${bf2 ? tex`\\bf{\\theta_{3,2} = ${radToDeg(theta_3_A_2).toFixed(1)}°}` : tex`\\theta_{3,2} = ${radToDeg(theta_3_A_2).toFixed(1)}°`}.\n
`;\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1874,"value":"Note that given the angular range of the joint J2 and the chosen rest position, the second solution can be eliminated.","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1877,"value":"### Joint 2, ${tex`\\theta_2`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1880,"value":"Finally, we calculate ${tex`\\theta_2`} using the coordinate ${tex`z`} and the value obtained for ${tex`\\theta_3`}. For that purpose, we rewrite the analytical expression:","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1890,"value":"tex`\n \\qquad \\begin{aligned}\n z &= L_1 + L_2\\sin(\\theta_2) + L_3(-\\sin(\\theta_2)\\sin(\\theta_3) + \\cos(\\theta_2)\\cos(\\theta_3))\\\\\n &= L_1 + ( L_2- L_3\\sin(\\theta_3))\\sin(\\theta_2) + L_3 \\cos(\\theta_3)\\cos(\\theta_2)\n \\end{aligned}\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1904,"value":"tex`\n \\qquad \\begin{aligned}\n \\Rightarrow z - L_1 &= ( L_2- L_3\\sin(\\theta_3))\\sin(\\theta_2) + L_3 \\cos(\\theta_3)\\cos(\\theta_2) \\\\\n &= A \\cos(\\theta_2) + B \\sin(\\theta_2)\n \\end{aligned} \\\\\n \\qquad \\textrm{with } A = L_3 \\cos(\\theta_3) \\textrm{ and } B = L_2- L_3\\sin(\\theta_3)\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1914,"value":"This expression can be rewritten as:\n\n${tex`\n \\qquad z-L_1 = S\\cos(\\theta_2-\\psi) \\\\\n \\qquad \\textrm{with } \\; S = \\sqrt{A^2+B^2} \\; \\textrm{ and } \\; \\psi = \\arctan(\\frac{B}{A})\n`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1917,"value":"Hence, the solution for ${tex`\\theta_2`} is obtained by solving:\n\n${tex`\n \\qquad \\theta_2 = \\psi \\pm \\arccos(\\frac{z-L_1}{S}) + k\\pi\\\\\n \\qquad \\textrm{with } k \\in \\{-1,0,1\\}\n\n`}","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1919,"value":"$35","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1924,"value":"Note that given the angular range of the joint J2, several solutions could be eliminated immediately.","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3185,"value":"$36","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3735,"value":"By correctly adjusting the sliders for the ${tex`(x,y,z)`} coordinates, we can ensure that the target position is within the robot's reachable workspace. Additionally, since the the ${tex`(x,y,z)`} coordinates used for these numerical calculations are derived from FK, reachability is inherently guaranteed. Let's then check joint angle limits. \n\nConcerning joint J3 for Position A, we stated above that the solution should be: ${tex`\\theta_{3} = `} **${theta_3_A_IK[0].toFixed(2)}°** (${MinAngle3}° ${tex`\\le \\theta_{3} \\le`}${MaxAngle3}°). We store this result in the array theta_3_A_IK:","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3741,"value":"theta_3_A_IK = {\n \n const r_24_sqr = x_cube_A_value**2 + y_cube_A_value**2 + (z_cube_A_value - L1)**2\n var theta_3_A_1 =Math.asin((L2**2+L3**2+L4**2-r_24_sqr)/(2*L2*L3))\n theta_3_A_1 = Math.round(radToDeg(theta_3_A_1)*100)/100\n\n // Return the result if it's within the valid angle range\n return (theta_3_A_1 >= MinAngle3 && theta_3_A_1 <= MaxAngle3)? [theta_3_A_1]: null;\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3811,"value":"For joint J2 (at Position A), we calculate ${tex`\\theta_2`} using the coordinate ${tex`z`} and the value obtained for ${tex`\\theta_3`} (*i.e.*, theta_3_A_IK). Among all the calculated solutions, we keep only those that fall within the limits: ${MinAngle2}° ${tex`\\le \\theta_2 \\le`} ${MaxAngle2}°. ${theta_2_A_IK.length > 1? html`There are potentially ${theta_2_A_IK.length} solutions`: html`There is ${theta_2_A_IK.length} solution`}. We store the result in the array theta_2_A_IK:","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3802,"value":"theta_2_A_IK = {\n \n const r_24_sqr = x_cube_A_value**2 + y_cube_A_value**2 + (z_cube_A_value - L1)**2\n const A = L3*Math.cos(degToRad(theta_3_A_IK[0]))\n const B = L2 - L3*Math.sin(degToRad(theta_3_A_IK[0]))\n const S = Math.pow(A**2+B**2,0.5)\n const psi_A = Math.atan(B/A)\n var theta_2_A_IK = []\n \n theta_2_A_IK.push(radToDeg(psi_A + Math.acos((z_cube_A_value-L1)/S)))\n theta_2_A_IK.push(radToDeg(psi_A - Math.acos((z_cube_A_value-L1)/S)))\n theta_2_A_IK.push(radToDeg(psi_A + Math.acos((z_cube_A_value-L1)/S) + Math.PI))\n theta_2_A_IK.push(radToDeg(psi_A - Math.acos((z_cube_A_value-L1)/S) + Math.PI))\n theta_2_A_IK.push(radToDeg(psi_A + Math.acos((z_cube_A_value-L1)/S) - Math.PI))\n theta_2_A_IK.push(radToDeg(psi_A - Math.acos((z_cube_A_value-L1)/S) - Math.PI)) \n\n return theta_2_A_IK = theta_2_A_IK.filter(angle => angle >= MinAngle2 && angle <= MaxAngle2)\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3836,"value":"We proceed similarly for joint J1 (for Position A). We calculate ${tex`\\theta_1`} using the coordinates ${tex`x`} and ${tex`y`}. Among all the calculated solutions, we keep only those that fall within the limits: ${MinAngle1}° ${tex`\\le \\theta_1 \\le`} ${MaxAngle1}°. ${theta_1_A_IK.length > 1? html`There are potentially ${theta_1_A_IK.length} solutions`: html`There is ${theta_1_A_IK.length} solution`}. We store the result in the array theta_1_A_IK:","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3824,"value":"theta_1_A_IK = {\n \n const phi_A = Math.atan(y_cube_A_value/x_cube_A_value)\n const R = Math.pow(x_cube_A_value**2 +y_cube_A_value**2,0.5)\n\n var theta_1_A_IK = []\n \n theta_1_A_IK.push(radToDeg(phi_A + Math.acos(L4/R)))\n theta_1_A_IK.push(radToDeg(phi_A - Math.acos(L4/R)))\n theta_1_A_IK.push(radToDeg(phi_A + Math.acos(L4/R) + Math.PI))\n theta_1_A_IK.push(radToDeg(phi_A - Math.acos(L4/R) + Math.PI))\n theta_1_A_IK.push(radToDeg(phi_A + Math.acos(L4/R) - Math.PI))\n theta_1_A_IK.push(radToDeg(phi_A - Math.acos(L4/R) - Math.PI)) \n \n return theta_1_A_IK = theta_1_A_IK.filter(angle => angle >= MinAngle1 && angle <= MaxAngle1)\n\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3842,"value":"All potential solutions are stored in three arrays, requiring up to ×${theta_3_A_IK.length*theta_2_A_IK.length*theta_1_A_IK.length} calculations (${theta_3_A_IK.length}×${theta_2_A_IK.length}×${theta_1_A_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","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3852,"value":"$37","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3929,"value":"$38","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3884,"value":"$39","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":544,"value":"dimensions = md`\n
\n\n---\n## Robot dimensions\nThe illustration below presents a simplified kinematic diagram of the robot's leg. It highlights the key dimensions necessary for performing Inverse Kinematics (IK) calculations.\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":482,"value":"$3a","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":713,"value":"These dimensions were retrieved from the 3D model available for download via the link provided in the [references](#references). For better visualization, compare it with the following 2D drawing.\n\nPlease note that in the actual design, joint J3 (rotation of the calf) is actuated by a lever mechanism, with the motor positioned concentrically with joint J2 (rotation of the thigh of the leg).","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":716,"value":"CAD_drawing = htl.html`\n
\n
\n \n
2D drawing of the robot's leg. The foot rubber (dog's paw) can be approximated as a hemisphere with a diameter of ${diameter_paw} mm.\n All dimensions are given in millimeters (mm).\n
\n
\n
\n`","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":21,"value":"$3b","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":461,"value":"
\n
\n \n
Unscrewed view of the leg mechanism of the Unitree Go2 robot.\n
This photograph was used to prepare the 3D model shown in this notebook.
\n
\n
","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":7097,"value":"
\n
\n
\n \n
Blender model created for this Sketchfab Viewer API.
\n
\n
","pinned":false,"hidden":false,"mode":"html","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":121,"value":"---\n## Appendices","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":627,"value":"### Robot dimensions and kinematics","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":5451,"value":"#### SymPy script","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":1988,"value":"$3c","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":873,"value":"#### Transformation matrices","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":913,"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","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":876,"value":"// Function to create the transformation matrix T01(theta)\nfunction T01(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, 0],\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","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":879,"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 [1, 0, 0, 0],\n [0, cosTheta, -sinTheta, 0],\n [0, sinTheta, cosTheta, L1],\n [0, 0, 0, 1]\n ];\n\n return T;\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":882,"value":"// Function to create the transformation matrix T23(theta)\nfunction T23(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 T23(theta)\n const T = [\n [1, 0, 0, 0],\n [0, cosTheta, -sinTheta, L2],\n [0, sinTheta, cosTheta, 0],\n [0, 0, 0, 1]\n ];\n\n return T;\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":976,"value":"T34 = [\n [1, 0, 0, L4],\n [0, 1, 0, 0],\n [0, 0, 1, L3],\n [0, 0, 0, 1]\n ];","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":5442,"value":"#### Forward Kinematics (Position A)","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":6509,"value":"x_cube_A_value = T04_cube_A[0][3]","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":6537,"value":"x_cube_A.value = x_cube_A_value","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":6514,"value":"y_cube_A_value = T04_cube_A[1][3]","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":6539,"value":"y_cube_A.value = y_cube_A_value","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":6516,"value":"z_cube_A_value = T04_cube_A[2][3]","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":6541,"value":"z_cube_A.value = z_cube_A_value","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2324,"value":"mutable T04_cube_A = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1985,"value":"#### Inverse Kinematics (Position B)","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":4799,"value":"mutable x_cube_B_value = x_cube_B.value","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4804,"value":"mutable y_cube_B_value = y_cube_B.value","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4806,"value":"mutable z_cube_B_value = z_cube_B.value","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4596,"value":"mutable T04_cube_B = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4631,"value":"$3d","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4614,"value":"theta_1_B_IK = {\n\n mutable x_cube_B_value = x_cube_B.value;\n mutable y_cube_B_value = y_cube_B.value;\n \n const phi_B = Math.atan(y_cube_B_value/x_cube_B_value)\n const R = Math.pow(x_cube_B_value**2 + y_cube_B_value**2,0.5)\n\n var theta_1_B_IK = []\n \n theta_1_B_IK.push(radToDeg(phi_B + Math.acos(L4/R)))\n theta_1_B_IK.push(radToDeg(phi_B - Math.acos(L4/R)))\n theta_1_B_IK.push(radToDeg(phi_B + Math.acos(L4/R) + Math.PI))\n theta_1_B_IK.push(radToDeg(phi_B - Math.acos(L4/R) + Math.PI))\n theta_1_B_IK.push(radToDeg(phi_B + Math.acos(L4/R) - Math.PI))\n theta_1_B_IK.push(radToDeg(phi_B - Math.acos(L4/R) - Math.PI)) \n \n return theta_1_B_IK = theta_1_B_IK.filter(angle => angle >= MinAngle1 && angle <= MaxAngle1)\n\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4678,"value":"theta_1_B.value = theta_1_B_IK[IK_solution_B.indices[0]]","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4612,"value":"theta_2_B_IK = {\n\n mutable x_cube_B_value = x_cube_B.value;\n mutable y_cube_B_value = y_cube_B.value;\n mutable z_cube_B_value = z_cube_B.value;\n \n const r_24_sqr = x_cube_B_value**2 + y_cube_B_value**2 + (z_cube_B_value - L1)**2\n const A = L3*Math.cos(degToRad(theta_3_B_IK[0]))\n const B = L2 - L3*Math.sin(degToRad(theta_3_B_IK[0]))\n const S = Math.pow(A**2+B**2,0.5)\n const psi_B = Math.atan(B/A)\n var theta_2_B_IK = []\n \n theta_2_B_IK.push(radToDeg(psi_B + Math.acos((z_cube_B_value-L1)/S)))\n theta_2_B_IK.push(radToDeg(psi_B - Math.acos((z_cube_B_value-L1)/S)))\n theta_2_B_IK.push(radToDeg(psi_B + Math.acos((z_cube_B_value-L1)/S) + Math.PI))\n theta_2_B_IK.push(radToDeg(psi_B - Math.acos((z_cube_B_value-L1)/S) + Math.PI))\n theta_2_B_IK.push(radToDeg(psi_B + Math.acos((z_cube_B_value-L1)/S) - Math.PI))\n theta_2_B_IK.push(radToDeg(psi_B - Math.acos((z_cube_B_value-L1)/S) - Math.PI)) \n\n return theta_2_B_IK = theta_2_B_IK.filter(angle => angle >= MinAngle2 && angle <= MaxAngle2)\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":5433,"value":"theta_2_B.value = theta_2_B_IK[IK_solution_B.indices[1]]","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4610,"value":"theta_3_B_IK = {\n\n mutable x_cube_B_value = x_cube_B.value;\n mutable y_cube_B_value = y_cube_B.value;\n mutable z_cube_B_value = z_cube_B.value;\n\n const r_24_sqr = x_cube_B_value**2 + y_cube_B_value**2 + (z_cube_B_value - L1)**2;\n let theta_3_B_1 = Math.asin((L2**2 + L3**2 + L4**2 - r_24_sqr) / (2 * L2 * L3));\n theta_3_B_1 = Math.round(radToDeg(theta_3_B_1) * 100) / 100;\n\n // Return the result if it's within the valid angle range\n return (theta_3_B_1 >= MinAngle3 && theta_3_B_1 <= MaxAngle3) ? [theta_3_B_1] : null;\n}","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4681,"value":"theta_3_B.value = theta_3_B_IK[IK_solution_B.indices[2]]","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":686,"value":"#### Dimensions / parameters","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":2107,"value":"mm_to_inch = 0.0393701","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":484,"value":"// L1 = 67 mm\nL1 = 67","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":488,"value":"// L2 = 213 mm\nL2 = 213","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":490,"value":"// L3 = 210 mm\nL3 = 210","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":492,"value":"// L4 = 94 mm\nL4 = 94","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4414,"value":"diameter_paw = 50","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":665,"value":"MinAngle1 = -25;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":671,"value":"MaxAngle1 = 30;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":673,"value":"MinAngle2 = -120;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":675,"value":"MaxAngle2 = 80;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":677,"value":"MinAngle3 = -45;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":679,"value":"MaxAngle3 = 55;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4530,"value":"xMin_A = -120","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4533,"value":"xMax_A = 260","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4537,"value":"yMin_A = -200","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4539,"value":"yMax_A = 410","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4544,"value":"zMin_A = -320","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4548,"value":"zMax_A = 460","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4567,"value":"xMin_B = 60","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4569,"value":"xMax_B = 130","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4572,"value":"yMin_B = 100","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4574,"value":"yMax_B = 300","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4575,"value":"zMin_B = -150","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4577,"value":"zMax_B = 300","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4409,"value":"#### Sliders","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":6607,"value":"mutable position_value = position.value","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":7281,"value":"mutable animation_loop_value = animation_loop.value","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":6593,"value":"$3e","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":6568,"value":"position = Inputs.radio(new Map([[\"A (FK)\", \"A\"], [\"B (IK)\", \"B\"] ]), {label: htl.html`Position`, value: \"A\"})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1420,"value":"theta_1_A = Inputs.range([MinAngle1, MaxAngle1], {value: 0, step: 1, label: htl.html` Hip (J1), θ₁ [°]`})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":905,"value":"theta_2_A = Inputs.range([MinAngle2, MaxAngle2], {value: -40, step: 1, label: htl.html`Thigh (J2), θ₂ [°]`})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":907,"value":"theta_3_A = Inputs.range([MinAngle3, MaxAngle3], {value: 20, step: 1, label: htl.html` Calf (J3), θ₃ [°]`})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2625,"value":"x_cube_A = Inputs.range([xMin_A, xMax_A], {value: 0, step: 0.1, label: htl.html` x [mm]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2642,"value":"y_cube_A = Inputs.range([yMin_A, yMax_A], {value: 0, step: 0.1, label: htl.html` y [mm]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":2651,"value":"z_cube_A = Inputs.range([zMin_A, zMax_A], {value: 0, step: 0.1, label: htl.html` z [mm]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4467,"value":"x_cube_B = Inputs.range([xMin_B, xMax_B], {value: L4, step: 1, label: htl.html` x [mm]`})\n","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4468,"value":"y_cube_B = Inputs.range([yMin_B, yMax_B], {value: L2, step: 1, label: htl.html` y [mm]`})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4472,"value":"z_cube_B = Inputs.range([zMin_B, zMax_B], {value: L1+L3, step: 1, label: htl.html` z [mm]`})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4455,"value":"theta_1_B = Inputs.range([MinAngle1, MaxAngle1], {value: 0, step: 0.1, label: htl.html` Hip (J1), θ₁ [°]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4458,"value":"theta_2_B = Inputs.range([MinAngle2, MaxAngle2], {value: -40, step: 0.1, label: htl.html`Thigh (J2), θ₂ [°]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":4464,"value":"theta_3_B = Inputs.range([MinAngle3, MaxAngle3], {value: 20, step: 0.1, label: htl.html` Calf (J3), θ₃ [°]`, disabled: true})","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":625,"value":"### 3D model and Sketchfab Viewer API","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":632,"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","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":14,"value":"// Sketchfab 3D model's uid\n//uid = \"d84607a0dc864921826cb092364d3990\"\nuid = \"adba00549b0d49eaad5ffa3729ce6589\"","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":638,"value":"hip_ID = \"M0_11\"","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":643,"value":"thigh_ID = \"M1_10\"","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":649,"value":"gear1_ID = \"M2_8\"","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":653,"value":"gear2_ID = \"Gear2_6\"","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":657,"value":"lever_ID = \"Lever_axis_5\"","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":661,"value":"calf_ID = \"Calf_1\"","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":7044,"value":"paw_ID = \"Empty_foot_16\"","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":634,"value":"#### JavaScript / functions","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":364,"value":"degToRad = (deg) => deg * Math.PI / 180 ;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1728,"value":"radToDeg = (rad) => rad * 180 / Math.PI;","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":57,"value":"Sketchfab = require(await FileAttachment(\"sketchfab-viewer-1.11.0.js\").url())","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":167,"value":"loadSketchfab(uid, \"api-frame\");","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":52,"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":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":28,"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":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1288,"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":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1319,"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":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":24,"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":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":1167,"value":"translateNode = (api, instanceID, translationVector) => {\n api.translate(\n instanceID, \n translationVector, \n {\n duration: 0.0,\n easing: \"linear\"\n }\n );\n};","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":48,"value":"$3f","pinned":true,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3688,"value":"### Graphviz\n*Code retrieved from this Observable notebook: \nhttps://observablehq.com/@christophe-yamahata/subtraction-through-addition-the-9s-complement-method*","pinned":false,"hidden":false,"mode":"md","author":"$undefined","summary":"$undefined","output":"","database":"$undefined"},{"id":3707,"value":"width_graphviz = 600","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3302,"value":"$40","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3300,"value":"dot = layout(\"dot\");","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3296,"value":"graphviz = GraphvizModule.Graphviz.load();","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"},{"id":3298,"value":"// GraphvizModule = import ('@hpcc-js/wasm/dist/graphviz');\nGraphvizModule = import(await FileAttachment(\"graphviz.js\").url())","pinned":false,"hidden":false,"mode":"ojs","author":"$undefined","summary":"$undefined","output":"$undefined","database":"$undefined"}],"files":"$W41","stdlib":"1","titling":"auto"}} 2e:"$2d:body"