Singularity ... again

More
13 Oct 2023 18:00 #282915 by bkt
Replied by bkt on topic Singularity ... again
rewamp these discussion .... after some view of you tube an use of some commercial robot ... think normally singularity transparent robot think normally stay on joint move and only occasionally go to coordinates mode .... so problem become operate normally with gcode in joint mode and than switch to world .... that is completely inverse mode of cartesian robot. .....

regards

Please Log in or Create an account to join the conversation.

More
13 Oct 2023 18:18 #282920 by Aciera
Replied by Aciera on topic Singularity ... again
Yes, that is why 'switchable kinematics' is such an important feature of linuxcnc. It let's you do exactly that, switch between joint/cartesian kinematics in Gcode.

Please Log in or Create an account to join the conversation.

More
13 Oct 2023 21:40 - 14 Oct 2023 12:07 #282943 by bkt
Replied by bkt on topic Singularity ... again
and in your opinion is possible perform the switch, chage singularity konst value with custom hal pin on kinematics file, than switch again ro world without any issue? (ok may be need acc. limit3 because eblow change at max acc to new point ....

TESTED:

you go in joint mode on "zero position of machinery/robot" than perform switch of singularity and return in world mode again .... these for 100 time for sure ... tested on real robot .... in my case (not little robot) we can positioning with an error of 0.05mm whitout particular issue when you perform the switch of singularity .... obviusly the tiltening because kins zero pos remain ... i use these kins for scara .... note DX pin ... switch from eBlow to right and eBlow to left direction ..

code:

/*****************************************************************
* 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); }
Last edit: 14 Oct 2023 12:07 by bkt.

Please Log in or Create an account to join the conversation.

More
14 Oct 2023 12:25 #282988 by bkt
Replied by bkt on topic Singularity ... again
See some video where that inversion appear durin J0 motion and J1 stop motion .... with J0 and J1 to 0deg .... not try if possible here on linuxcnc ....
other stuck is mechanical vibration near world mode zero position .... or when we reach maximum radius in world mode ... these because cos 0.1, cos 0.01, cos 0.001 is too little and become too mutch segment to moove in little real space for TP .... so need some function for limit vel and acc during these "spaces" .... 

*****

these is desired result ... and I test only in MANUAL mode

About inverted kins, is possible to use switchkins of dgarret, for
  1. run gcode with desired starting kins geometry and perform some gcode row in WORLD mode
  2. switchkins in gcode for put machine in exact position and exact geometry everytime you need to invert geometry, using JOINT mode
  3. invert kins in JOINT mode
  4. switchkins to WORLD mode and perform some other row of gcode in WORLD mode
Think is possible repeat from 1 to 4 point many time without error ...
 

Please Log in or Create an account to join the conversation.

Time to create page: 0.081 seconds
Powered by Kunena Forum