Added forward kinematics, replacing PlanarArmFK.

Author: Sashmit29

src/kinematics/ArmFK2026.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "../navtypes.h"
+#include "ForwardArmKinematics.h"
+
+#include <Eigen/Core>
+#include <cmath>
+
+namespace kinematics {
+
+/**
+ * @brief Kinematics object for the 2026 arm in 3D space.
+ */
+class ArmFK2026 : public ForwardArmKinematics<3, 3> {
+public:
+    /**
+     * @brief Construct a new kinematics object.
+     *
+     * @param segLens The length of each arm segment (only shoulder and elbow).
+     * @param jointMin The minimum angle of each joint.
+     * @param jointMax The maximum angle of each joint.
+     */
+    ArmFK2026(const navtypes::Vectord<3>& segLens, 
+               const navtypes::Vectord<3>& jointMin,
+               const navtypes::Vectord<3>& jointMax)
+        : segLens(segLens), jointMin(jointMin), jointMax(jointMax) {}
+
+    navtypes::Vectord<3> getSegLens() const override {
+        return segLens;
+    }
+
+    bool satisfiesConstraints(const navtypes::Vectord<3>& jointPos) const override {
+        for (unsigned int i = 0; i < 3; i++) {
+            if (jointPos[i] < jointMin[i] || jointPos[i] > jointMax[i]) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    navtypes::Vectord<3> jointPosToEEPos(const navtypes::Vectord<3>& jointPos) const override {
+        double theta_B = jointPos[0];
+        double theta_S = jointPos[1];
+        double theta_E = jointPos[2];
+
+        double l = segLens[1];
+        double t = segLens[2];
+
+        double x = std::cos(theta_B) * (l * std::sin(theta_S) + t * std::cos(theta_E));
+        double y = std::sin(theta_B) * (l * std::sin(theta_S) + t * std::cos(theta_E));
+        double z = l * std::cos(theta_S) - t * std::sin(theta_E);
+
+        return Eigen::Vector3d(x, y, z);
+    }
+
+    navtypes::Matrixd<3, 3> getJacobian(const navtypes::Vectord<3>& jointPos) const override {
+        double theta_B = jointPos[0];
+        double theta_S = jointPos[1];
+        double theta_E = jointPos[2];
+
+        double l = segLens[1];
+        double t = segLens[2];
+
+        Eigen::Matrix<double, 3, 3> J;
+
+        // Partial derivatives for X
+        J(0, 0) = -std::sin(theta_B) * (l * std::sin(theta_S) + t * std::cos(theta_E));
+        J(0, 1) =  std::cos(theta_B) * (l * std::cos(theta_S));
+        J(0, 2) =  std::cos(theta_B) * (-t * std::sin(theta_E));
+
+        // Partial derivatives for Y
+        J(1, 0) =  std::cos(theta_B) * (l * std::sin(theta_S) + t * std::cos(theta_E));
+        J(1, 1) =  std::sin(theta_B) * (l * std::cos(theta_S));
+        J(1, 2) =  std::sin(theta_B) * (-t * std::sin(theta_E));
+
+        // Partial derivatives for Z
+        J(2, 0) =  0.0; // The base rotation does not affect the Z height
+        J(2, 1) = -l * std::sin(theta_S);
+        J(2, 2) = -t * std::cos(theta_E);
+
+        return J;
+    }
+
+private:
+    navtypes::Vectord<3> segLens;
+    navtypes::Vectord<3> jointMin;
+    navtypes::Vectord<3> jointMax;
+};
+
+} // namespace kinematics

src/kinematics/2026IKSolver.h src/kinematics/IKSolver2026.hsrc/kinematics/2026IKSolver.h renamed to src/kinematics/IKSolver2026.h

@@ -5,9 +5,9 @@
 
 namespace kinematics {
 
-class 2026IKSolver : public InverseArmKinematics<3, 3> {
+class IKSolver2026 : public InverseArmKinematics<3, 3> {
 public:
-    2026IKSolver(std::shared_ptr<const ForwardArmKinematics<3, 3>> fk) 
+    IKSolver2026(std::shared_ptr<const ForwardArmKinematics<3, 3>> fk) 
         : InverseArmKinematics<3, 3>(fk) {}
 
 protected:
@@ -21,39 +21,32 @@ class 2026IKSolver : public InverseArmKinematics<3, 3> {
         double y = eePos[1];
         double z = eePos[2];
 
-        const double l = 0.5; // Shoulder-to-elbow length 
-        const double t = 0.4; // Elbow-to-wrist length
+        navtypes::Vectord<3> segLens = this->fk->getSegLens();
+        const double l = segLens[1]; 
+        const double t = segLens[2]; 
 
-        // Pre-calculate reused terms
         double r_xy = std::sqrt(x*x + y*y);
         double r_xyz = std::sqrt(x*x + y*y + z*z);
 
-        // Calculate Base Angle
         double theta_B = std::atan2(y, x);
 
-        // Calculate Shoulder Angle (with physical limit check)
-        // The value inside the arccos function:
         double acos_inner = (l*l + x*x + y*y + z*z - t*t) / (2 * l * r_xyz);
 
-        // Check if the target is physically reachable
         if (acos_inner < -1.0 || acos_inner > 1.0) {
             success = false; 
-            break;
+            return targetAngles; // Return early on failure
         }
 
         double theta_S = (M_PI / 2.0) - std::atan2(z, r_xy) - std::acos(acos_inner);
 
-        // Calculate Elbow Angle
         double elbow_y = -(z - l * std::cos(theta_S));
         double elbow_x = r_xy - l * std::sin(theta_S);
         double theta_E = std::atan2(elbow_y, elbow_x);
 
-        // Assign to the output vector
         targetAngles[0] = theta_B;
         targetAngles[1] = theta_S;
         targetAngles[2] = theta_E;
 
-        // If made it this far, the calculation was physically possible
         success = true; 
         return targetAngles;
     }