Advanced Search

#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()

Unsupported HAL pin type (1) in mesa_modbus definition file

[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.

 

        case 0:
            pos->tran.x = j[0];
            pos->tran.y = j[1];
            pos->tran.z = j[2];
            pos->a      = j[3];
            break;
        case 1:
            pos->tran.x = j[2];
            pos->tran.y = -j[1];
            pos->tran.z = j[0];
            pos->a      = j[3];
            break;
    }
    // unused coordinates:
    pos->b = 0;
    pos->c = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;

        case 0:
            pos->tran.x = j[0];
            pos->tran.y = j[1];
            pos->tran.z = j[2];
            break;
        case 1:
            pos->tran.x = -j[0];
            pos->tran.y = j[2];
            pos->tran.z = j[1];
            break;
    }
    // unused coordinates:
    pos->a = 0;
    pos->b = 0;
    pos->c = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;