Custom scara kinematic.

[attachment=67048]2025-01-1918_53_54-_scara-FreeCAD1.0.0.png[/attachment] [code]import math import linuxcnc def forward_kinematics(joint):     """     Forward kinematics for SCARA with parallelogram mechanism and tension-controlled Z movement.     joint[0] = Theta1 (Base rotation)     joint[1] = Theta2 (Second arm rotation)     joint[2] = Z movement via parallelogram     """     L1 = 1.2  # First arm length (m)     L2 = 1.5  # Second arm length (m) to achieve 1.8m z reach     Z_parallelogram = 0.6  # Parallelogram tension control length (m)     Z_min, Z_max = -0.9, 0.9  # Define valid Z range          # Clamp Z within limits     z = max(Z_min, min(joint[2], Z_max))          # Compute effective second arm length with bidirectional movement     L2_projected = math.sqrt(L2**2 - z**2)          # Compute correct tension-controlled parallelogram movement    delta_z = math.sqrt((Z_parallelogram - z)**2 + L2_projected**2)          # Compute end-effector position     x = L1 * math.cos(joint[0]) + L2_projected * math.cos(joint[0] + joint[1])     y = L1 * math.sin(joint[0]) + L2_projected * math.sin(joint[0] + joint[1])          return (x, y, delta_z) def inverse_kinematics(x, y, z):     """     Inverse kinematics for SCARA with parallelogram mechanism and tension-controlled Z movement.     """     L1 = 1.2  # First arm length (m)     L2 = 1.5  # Second arm length (m) for 1.8m reach     Z_parallelogram = 0.6  # Parallelogram tension control length (m)     Z_min, Z_max = -0.9, 0.9  # Define valid Z range          # Clamp Z within limits     z = max(Z_min, min(z, Z_max))          # Adjust for parallelogram tension effect using correct formula     L2_projected = math.sqrt(L2**2 - z**2)     adjusted_z = math.sqrt((Z_parallelogram - z)**2 + L2_projected**2)          # Compute effective second arm length with bidirectional movement     L2_eff = math.sqrt(L2**2 - adjusted_z**2)          # Compute inverse kinematics     D = (x**2 + y**2 - L1**2 - L2_eff**2) / (2 * L1 * L2_eff)          if abs(D) > 1:         raise ValueError("Position out of reach")          theta2 = math.acos(D)     theta1_1 = math.atan2(y, x) - math.atan2(L2_eff * math.sin(theta2), L1 + L2_eff * math.cos(theta2))     theta1_2 = math.atan2(y, x) - math.atan2(-L2_eff * math.sin(theta2), L1 + L2_eff * math.cos(theta2))          return [(theta1_1, theta2, adju [attachment=67048]2025-01-19 18_53_54-_ scara - FreeCAD 1.0.0.png[/attachment]sted_z), (theta1_2, theta2, adjusted_z)]  # Two possible solutions And Z axis should be controlled by some rope wire tensioning paralelogram part.  

#include "rtapi.h" #include "rtapi_math.h" #include "rtapi_string.h" #include "posemath.h" #include "hal.h" #include "kinematics.h" #include "switchkins.h" struct scara_data {     hal_float_t *d1, *d2, *d3, *d4, *d5, *d6; } *haldata = 0; #define D1 (*(haldata->d1)) #define D2 (*(haldata->d2)) #define D3 (*(haldata->d3)) // Now used as Z_parallelogram height #define D4 (*(haldata->d4)) // Keep as fixed second arm length #define D5 (*(haldata->d5)) #define D6 (*(haldata->d6)) static int scaraKinematicsForward(const double * joint,                       EmcPose * world,                       const KINEMATICS_FORWARD_FLAGS * fflags,                       KINEMATICS_INVERSE_FLAGS * iflags) {     double a0, a1, a3;     double x, y, z, c;     a0 = joint[0] * ( PM_PI / 180 );     a1 = joint[1] * ( PM_PI / 180 );     a3 = joint[3] * ( PM_PI / 180 );     a1 = a1 + a0;     a3 = a3 + a1;     // Compute projected second arm length dynamically     double D4_projected = sqrt(D4 * D4 - joint[2] * joint[2]);          x = D2*cos(a0) + D4_projected*cos(a1) + D6*cos(a3);     y = D2*sin(a0) + D4_projected*sin(a1) + D6*sin(a3);          // Compute parallelogram-based Z     z = sqrt((D3 - joint[2]) * (D3 - joint[2]) + D4_projected * D4_projected);     c = a3;     *iflags = 0;     if (joint[1] < 90)         *iflags = 1;     world->tran.x = x;     world->tran.y = y;     world->tran.z = z;     world->c = c * 180 / PM_PI;     world->a = joint[4];     world->b = joint[5];     return (0); } static int scaraKinematicsInverse(const EmcPose * world,                                   double * joint,                                   const KINEMATICS_INVERSE_FLAGS * iflags,                                   KINEMATICS_FORWARD_FLAGS * fflags) {     double a3;     double q0, q1;     double xt, yt, rsq, cc;     double x, y, z, c;          double D4_projected, z_transformed;          x = world->tran.x;     y = world->tran.y;     z = world->tran.z;     c = world->c;     a3 = c * ( PM_PI / 180 );     xt = x - D6*cos(a3);     yt = y - D6*sin(a3);          z_transformed = z - D1 + D5 ; // should return as at 0 level of arms for -900     rsq = xt*xt + yt*yt;          D4_projected = sqrt(D4 * D4 - z_transformed * z_transformed);          cc = (rsq - D2 * D2 - D4_projected * D4_projected) / (2 * D2 * D4_projected);     if(cc < -1) cc = -1;     if(cc > 1) cc = 1;     q1 = acos(cc);     if (*iflags)         q1 = -q1;     q0 = atan2(yt, xt);     xt = D2 + D4_projected*cos(q1);     yt = D4_projected*sin(q1);     q0 = q0 - atan2(yt, xt);     q0 = q0 * (180 / PM_PI);     q1 = q1 * (180 / PM_PI);     joint[0] = q0;     joint[1] = q1;          // Compute inverse for parallelogram Z          joint[2] = sqrt(z_transformed * z_transformed - D4_projected * D4_projected);          joint[3] = c - ( q0 + q1);     joint[4] = world->a;     joint[5] = world->b;     *fflags = 0;     return (0); }    //scaraKinematicsInverse() #define DEFAULT_D1 1200 #define DEFAULT_D2 1200 #define DEFAULT_D3 600 #define DEFAULT_D4 1500 #define DEFAULT_D5 300 #define DEFAULT_D6 0 static int scaraKinematicsSetup(const  int   comp_id,                                 const  char* coordinates,                                 kparms*      kp) {     int res=0;     haldata = hal_malloc(sizeof(*haldata));     if (!haldata) goto error;     res += hal_pin_float_newf(HAL_IN, &(haldata->d1), comp_id,"%s.D1",kp->halprefix);     res += hal_pin_float_newf(HAL_IN, &(haldata->d2), comp_id,"%s.D2",kp->halprefix);     res += hal_pin_float_newf(HAL_IN, &(haldata->d3), comp_id,"%s.D3",kp->halprefix);     res += hal_pin_float_newf(HAL_IN, &(haldata->d4), comp_id,"%s.D4",kp->halprefix);     res += hal_pin_float_newf(HAL_IN, &(haldata->d5), comp_id,"%s.D5",kp->halprefix);     res += hal_pin_float_newf(HAL_IN, &(haldata->d6), comp_id,"%s.D6",kp->halprefix);     if (res) { goto error; }     D1 = DEFAULT_D1;     D2 = DEFAULT_D2;     D3 = DEFAULT_D3;     D4 = DEFAULT_D4;     D5 = DEFAULT_D5;     D6 = DEFAULT_D6;     return 0; error:     return -1; }    // scaraKinematicsSetup() int switchkinsSetup(kparms* kp,                     KS* kset0, KS* kset1, KS* kset2,                     KF* kfwd0, KF* kfwd1, KF* kfwd2,                     KI* kinv0, KI* kinv1, KI* kinv2                    ) {     kp->kinsname    = "scarakins"; // !!! must agree with filename     kp->halprefix   = "scarakins"; // hal pin names     kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts     kp->allow_duplicates     = 0;     kp->max_joints = strlen(kp->required_coordinates);     rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);     *kset0 = scaraKinematicsSetup;     *kfwd0 = scaraKinematicsForward;     *kinv0 = scaraKinematicsInverse;     *kset1 = identityKinematicsSetup;     *kfwd1 = identityKinematicsForward;     *kinv1 = identityKinematicsInverse;     *kset2 = userkKinematicsSetup;     *kfwd2 = userkKinematicsForward;     *kinv2 = userkKinematicsInverse;     return 0; }     // switchkinsSetup()