Added inverse kinematics

Assets/Scenes/SampleScene.unity

@@ -737,6 +737,8 @@ MonoBehaviour:
   baseObject: {fileID: 660799480}
   useKeyboardControl: 1
   angleAdjustSpeed: 20
+  testPosition: {x: 0.458, y: 0.148, z: 0}
+  testGoToKey: 116
 --- !u!4 &660799482
 Transform:
   m_ObjectHideFlags: 0
@@ -1128,6 +1130,114 @@ MeshFilter:
   m_PrefabAsset: {fileID: 0}
   m_GameObject: {fileID: 924139361}
   m_Mesh: {fileID: 10207, guid: 0000000000000000e000000000000000, type: 0}
+--- !u!1 &1085390984
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 1085390988}
+  - component: {fileID: 1085390987}
+  - component: {fileID: 1085390986}
+  - component: {fileID: 1085390985}
+  m_Layer: 0
+  m_Name: Cube
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!65 &1085390985
+BoxCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1085390984}
+  m_Material: {fileID: 0}
+  m_IncludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_ExcludeLayers:
+    serializedVersion: 2
+    m_Bits: 0
+  m_LayerOverridePriority: 0
+  m_IsTrigger: 0
+  m_ProvidesContacts: 0
+  m_Enabled: 1
+  serializedVersion: 3
+  m_Size: {x: 1, y: 1, z: 1}
+  m_Center: {x: 0, y: 0, z: 0}
+--- !u!23 &1085390986
+MeshRenderer:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1085390984}
+  m_Enabled: 1
+  m_CastShadows: 1
+  m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
+  m_StaticShadowCaster: 0
+  m_MotionVectors: 1
+  m_LightProbeUsage: 1
+  m_ReflectionProbeUsage: 1
+  m_RayTracingMode: 2
+  m_RayTraceProcedural: 0
+  m_RayTracingAccelStructBuildFlagsOverride: 0
+  m_RayTracingAccelStructBuildFlags: 1
+  m_SmallMeshCulling: 1
+  m_RenderingLayerMask: 1
+  m_RendererPriority: 0
+  m_Materials:
+  - {fileID: 2100000, guid: 31321ba15b8f8eb4c954353edc038b1d, type: 2}
+  m_StaticBatchInfo:
+    firstSubMesh: 0
+    subMeshCount: 0
+  m_StaticBatchRoot: {fileID: 0}
+  m_ProbeAnchor: {fileID: 0}
+  m_LightProbeVolumeOverride: {fileID: 0}
+  m_ScaleInLightmap: 1
+  m_ReceiveGI: 1
+  m_PreserveUVs: 0
+  m_IgnoreNormalsForChartDetection: 0
+  m_ImportantGI: 0
+  m_StitchLightmapSeams: 1
+  m_SelectedEditorRenderState: 3
+  m_MinimumChartSize: 4
+  m_AutoUVMaxDistance: 0.5
+  m_AutoUVMaxAngle: 89
+  m_LightmapParameters: {fileID: 0}
+  m_SortingLayerID: 0
+  m_SortingLayer: 0
+  m_SortingOrder: 0
+  m_AdditionalVertexStreams: {fileID: 0}
+--- !u!33 &1085390987
+MeshFilter:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1085390984}
+  m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
+--- !u!4 &1085390988
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1085390984}
+  serializedVersion: 2
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0.458, y: 0.148, z: 0}
+  m_LocalScale: {x: 0.1, y: 0.1, z: 0.1}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1 &1140624451
 GameObject:
   m_ObjectHideFlags: 0
@@ -2073,3 +2183,4 @@ SceneRoots:
   - {fileID: 410087041}
   - {fileID: 832575519}
   - {fileID: 660799482}
+  - {fileID: 1085390988}

Assets/Scripts/HydraulicArmController.cs

@@ -1,3 +1,4 @@
+using System;
 using UnityEngine;
 
 public class HydraulicArmController : MonoBehaviour
@@ -11,10 +12,38 @@ public class HydraulicArmController : MonoBehaviour
     [Header("Control")]
     public bool useKeyboardControl = true;
     public float angleAdjustSpeed = 20f;
-
+
+    [Header("Testing")]
+    public Vector3 testPosition = new Vector3(0.3f, 0.2f, 0.3f);
+    public KeyCode testGoToKey = KeyCode.T;
+
     private int selectedJoint = 0;
-
+
+    /// <summary>
+    /// Unity's Update method called once per frame. Equivalent to the Arduino's loop() function.
+    /// Handles keyboard control and would handle serial communication in hardware implementation.
+    /// </summary>
     void Update()
+    {
+        // Unity exclusive functionality! Control the joints with the keyboard
+        KeyboardControl();
+
+        //// here, in Arduino, there would be a
+        // for (int i = 0; i < joints.Length; i++)
+        //     joints[i].update();
+
+        //// there would also be a
+        // checkSerialBuffer();
+        //// and
+        // Vector3 position = getNextGCodeCommand();
+        // moveTip(position); // equivalent to MoveTip(position)
+        //// or something similar
+    }
+
+    /// <summary>
+    /// Provides the keyboard control functionality while Unity sim is running.
+    /// </summary>
+    void KeyboardControl()
     {
         if (!useKeyboardControl) return;
 
@@ -66,8 +95,103 @@ void Update()
                 joint.resetToInit();
             }
         }
+
+        // Test GoTo function
+        if (Input.GetKeyDown(testGoToKey))
+        {
+            Debug.Log($"Testing GoTo with position: {testPosition}");
+            MoveTip(testPosition);
+        }
     }
-
+
+    /// <summary>
+    /// Moves the arm tip to the specified position using inverse kinematics.
+    /// Adjusts for the end effector length and delegates to MoveEndEffectorOrigin.
+    /// </summary>
+    /// <param name="position">The target position in world space for the arm tip.</param>
+    void MoveTip(Vector3 position)
+    {
+        float e_magnitude = 0.07f;
+        // We need to remove the vector that is the end effector from the
+        // equation, and forward it to MoveEndEffectorOrigin
+        Debug.Log($"Position: {position}");
+        Vector3 P_vector = position - joints[0].transform.position;
+        P_vector.y = 0;
+        Debug.Log($"P_vector: {P_vector}");
+        Vector3 P_vector_normalized = P_vector.normalized;
+        Vector3 P_vector_normalized_scaled = P_vector_normalized * e_magnitude;
+        MoveEndEffectorOrigin(position - P_vector_normalized_scaled);
+
+    }
+
+    /// <summary>
+    /// Performs inverse kinematics calculations to position the end effector origin at the specified position.
+    /// Calculates joint angles theta_1, theta_2, and theta_3 using geometric methods and applies them to the arm.
+    /// </summary>
+    /// <param name="position">The target position in world space for the end effector origin.</param>
+    void MoveEndEffectorOrigin(Vector3 position)
+    {
+        //renaming axis. Unity uses y up instead of z up. y and z are swapped.
+        float x_3 = position.x;
+        float y_3 = position.z;
+        float z_3 = position.y;
+
+        // Horizontal plane:
+        // arm projected onto x-y plane, x+ towards you, y+ towards the right
+
+        double theta_1_rad = Math.Atan2(x_3, y_3);
+        // add pi if not within the wanted range
+
+        float a_1 = 0.098f;
+        float a_2 = 0.270f;
+        float a_3 = 0.320f;
+
+        double r = Math.Sqrt(x_3*x_3 + (z_3 - a_1)*(z_3 - a_1));
+
+        double phi_1_rad = Math.Acos((a_2*a_2 + r*r - a_3*a_3)/(2*a_2*r));
+        double phi_2_rad = Math.Acos((a_2*a_2 + a_3*a_3 - r*r)/(2*a_2*a_3));
+        double phi_3_rad = Math.Atan((z_3-a_1)/x_3);
+
+        double theta_2_rad = phi_3_rad + phi_1_rad;
+        double theta_3_rad = phi_2_rad - Math.PI;
+
+
+        float phi_1 = (180f/(float)Math.PI * (float)phi_1_rad);
+        float phi_2 = (180f/(float)Math.PI * (float)phi_2_rad);
+        float phi_3 = (180f/(float)Math.PI * (float)phi_3_rad);
+
+        float theta_1 = (180f/(float)Math.PI * (float)theta_1_rad);
+        float theta_2 = (180f/(float)Math.PI * (float)theta_2_rad);
+        float theta_3 = (180f/(float)Math.PI * (float)theta_3_rad);
+
+        baseObject.transform.localRotation = Quaternion.Euler(0f, (float)theta_1_rad, 0f);
+        joints[0].targetAngle = theta_2 - 90f;
+        joints[1].targetAngle = theta_3;
+
+        // Calculate the angle of the end effector joint
+        joints[2].targetAngle =
+            (
+                180
+                - phi_1
+                - phi_2
+            ) - phi_3;
+
+        Debug.Log($"Phi_1: {phi_1}");
+        Debug.Log($"Phi_2: {phi_2}");
+        Debug.Log($"Phi_3: {phi_3}");
+
+        Debug.Log($"Theta_1: {theta_1}");
+        Debug.Log($"Theta_2: {theta_2}");
+        Debug.Log($"Theta_3: {theta_3}");
+
+
+        // t = 180-i
+    }
+
+    /// <summary>
+    /// Renders the GUI overlay showing controller status and instructions.
+    /// Displays selected joint, control keys, and current/target angles for all joints.
+    /// </summary>
     void OnGUI()
     {
         GUILayout.BeginArea(new Rect(10, 10, 300, 500));
@@ -77,6 +201,7 @@ void OnGUI()
         GUILayout.Label("Up/Down: Adjust angle");
         GUILayout.Label("Left/Right: Rotate base");
         GUILayout.Label("R: Reset all");
+        GUILayout.Label($"T: GoTo test position");
 
         GUILayout.Space(10);
 
@@ -91,4 +216,22 @@ void OnGUI()
 
         GUILayout.EndArea();
     }
+
+    /// <summary>
+    /// Draws debug gizmos in the Scene view showing the test target position and coordinate axes.
+    /// </summary>
+    void OnDrawGizmos()
+    {
+        // Draw the test target position
+        Gizmos.color = Color.green;
+        Gizmos.DrawWireSphere(testPosition, 0.05f);
+
+        // Draw coordinate axes at the target position for reference
+        Gizmos.color = Color.red;
+        Gizmos.DrawLine(testPosition, testPosition + Vector3.right * 0.03f); // X axis
+        Gizmos.color = Color.green;
+        Gizmos.DrawLine(testPosition, testPosition + Vector3.up * 0.03f); // Y axis
+        Gizmos.color = Color.blue;
+        Gizmos.DrawLine(testPosition, testPosition + Vector3.forward * 0.03f); // Z axis
+    }
 }