Singularity ... again

/***************************************************************** * Description: inv_scarakins.c *   Kinematics for scara typed robots *   Set the params using HAL to fit your robot * *   Derived from a work by Sagar Behere *   Bkt add bit pin for perform inversion of eBlow position during motion *       WARNING USAGE: before perform the infersion of geometry go to J0=0deg ,J1=0deg and J2=0deg * ****************************************************************************************************** *       WARNING ** GO TO ZERO MACHINE POSITION BEFORE INVERT GEOMETRY - IF NOT CAN DAMAGE ** WARNING * ****************************************************************************************************** * * Author: BKT * License: GPL Version 2 * System: Linux * * Copyright (c) 2023 All rights reserved. * * Last change: ******************************************************************* */ #include "posemath.h" #include "rtapi_math.h" #include "kinematics.h"             /* decls for kinematicsForward, etc. */ #include "rtapi.h"        /* RTAPI realtime OS API */ #include "rtapi_app.h"        /* RTAPI realtime module decls */ #include "hal.h" struct scara_data {     hal_float_t *d1, *d2, *d3, *d4, *d5, *d6;     hal_bit_t   *dx; } *haldata = 0; /* key dimensions    joint[0] = Entire arm rotates around a vertical axis at its inner end         which is attached to the earth.  A value of zero means the         inner arm is pointing along the X axis.    D1 = Vertical distance from the ground plane to the center of the inner         arm.    D2 = Horizontal distance between joint[0] axis and joint[1] axis, ie.         the length of the inner arm.    joint[1] = Outer arm rotates around a vertical axis at its inner end         which is attached to the outer end of the inner arm.  A         value of zero means the outer arm is parallel to the         inner arm (and extending outward).    D3 = Vertical distance from the center of the inner arm to the center         of the outer arm.  May be positive or negative depending         on the structure of the robot.    joint[2] = End effector slides along a vertical axis at the outer end         of the outer arm.  A value of zero means the end effector         is at the same height as the center of the outer arm, and         positive values mean downward movement.    D4 = Horizontal distance between joint[1] axis and joint[2] axis, ie.         the length of the outer arm    joint[3] = End effector rotates around the same vertical axis that it         slides along.  A value of zero means that the tooltip (if         offset from the axis) is pointing in the same direction         as the centerline of the outer arm.    D5 = Vertical distance from the end effector to the tooltip.  Positive         means the tooltip is lower than the end effector, and is         the normal case.    D6 = Horizontal distance from the centerline of the end effector (and         the joints 2 and 3 axis) and the tooltip.  Zero means the         tooltip is on the centerline.  Non-zero values should be         positive, if negative they introduce a 180 degree offset         on the value of joint[3].    DX = kinematics identity flag. If dx=0 scara put elbow to left, if dx=1 put elbow to right */ #define D1 (*(haldata->d1)) #define D2 (*(haldata->d2)) #define D3 (*(haldata->d3)) #define D4 (*(haldata->d4)) #define D5 (*(haldata->d5)) #define D6 (*(haldata->d6)) #define DX (*(haldata->dx)) /* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ int kinematicsForward(const double * joint,                       EmcPose * world,                       const KINEMATICS_FORWARD_FLAGS * fflags,                       KINEMATICS_INVERSE_FLAGS * iflags) {     double a0, a1, a3;     double x, y, z, c; /* convert joint angles to radians for sin() and cos() */     a0 = joint[0] * ( PM_PI / 180 );     a1 = joint[1] * ( PM_PI / 180 );     a3 = joint[3] * ( PM_PI / 180 );     //*iflags = DX; /* convert angles into world coords */     a1 = a1 + a0;     a3 = a3 + a1;     x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3);     y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3);     z = D1 + D3 - joint[2] - D5;     c = a3;          if(DX == 0){         *iflags = 0;         if (joint[1] < 90)         *iflags = 1;     } else {         *iflags = 1;         if (joint[1] < 180)         *iflags = 0;     }     //*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); } int kinematicsInverse(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;     x = world->tran.x;     y = world->tran.y;     z = world->tran.z;     c = world->c;     /* convert degrees to radians */     a3 = c * ( PM_PI / 180 );     /* center of end effector (correct for D6) */     xt = x - D6*cos(a3);     yt = y - D6*sin(a3);     /* horizontal distance (squared) from end effector centerline     to main column centerline */     rsq = xt*xt + yt*yt;     /* joint 1 angle needed to make arm length match sqrt(rsq) */     cc = (rsq - D2*D2 - D4*D4) / (2*D2*D4);     if(cc < -1) cc = -1;     if(cc > 1) cc = 1;     q1 = acos(cc);     //if (*iflags == 1){q1 = -q1;}     //else {q1 = q1;}     if (*iflags)         q1 = -q1;     /* angle to end effector */     q0 = atan2(yt, xt);     /* end effector coords in inner arm coord system */     xt = D2 + D4*cos(q1);     yt = D4*sin(q1);     /* inner arm angle */     q0 = q0 - atan2(yt, xt);     /* q0 and q1 are still in radians. convert them to degrees */     q0 = q0 * (180 / PM_PI);     q1 = q1 * (180 / PM_PI);     joint[0] = q0;     joint[1] = q1;     joint[2] = D1 + D3 - D5 - z;     joint[3] = c - ( q0 + q1);     joint[4] = world->a;     joint[5] = world->b;     if (DX == 1){*fflags = 1;}     else {*fflags = 0;}     //*fflags = DX;     //*fflags = 0;     return (0); } int kinematicsHome(EmcPose * world,                    double * joint,                    KINEMATICS_FORWARD_FLAGS * fflags,                    KINEMATICS_INVERSE_FLAGS * iflags) {   /* use joints, set world */   return kinematicsForward(joint, world, fflags, iflags); } KINEMATICS_TYPE kinematicsType() {   return KINEMATICS_BOTH; } #define DEFAULT_D1 490 #define DEFAULT_D2 340 #define DEFAULT_D3  50 #define DEFAULT_D4 250 #define DEFAULT_D5  50 #define DEFAULT_D6  50 #define DEFAULT_DX  0 EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsHome); MODULE_LICENSE("GPL"); int comp_id; int rtapi_app_main(void) {     int res=0;          comp_id = hal_init("gc_scarakins");     if (comp_id < 0) return comp_id;          haldata = hal_malloc(sizeof(*haldata));     if (!haldata) goto error;     if((res = hal_pin_float_new("gc_scarakins.D1", HAL_IO, &;(haldata->d1), comp_id)) < 0) goto error;     if((res = hal_pin_float_new("gc_scarakins.D2", HAL_IO, &;(haldata->d2), comp_id)) < 0) goto error;     if((res = hal_pin_float_new("gc_scarakins.D3", HAL_IO, &;(haldata->d3), comp_id)) < 0) goto error;     if((res = hal_pin_float_new("gc_scarakins.D4", HAL_IO, &;(haldata->d4), comp_id)) < 0) goto error;     if((res = hal_pin_float_new("gc_scarakins.D5", HAL_IO, &;(haldata->d5), comp_id)) < 0) goto error;     if((res = hal_pin_float_new("gc_scarakins.D6", HAL_IO, &;(haldata->d6), comp_id)) < 0) goto error;     if((res = hal_pin_bit_new("gc_scarakins.DX", HAL_IN, &;(haldata->dx), comp_id)) < 0) goto error;          D1 = DEFAULT_D1;     D2 = DEFAULT_D2;     D3 = DEFAULT_D3;     D4 = DEFAULT_D4;     D5 = DEFAULT_D5;     D6 = DEFAULT_D6;     DX = DEFAULT_DX;     hal_ready(comp_id);     return 0;      error:     hal_exit(comp_id);     return res; } void rtapi_app_exit(void) { hal_exit(comp_id); }