making 6 axis robotic arm with puma 560 kinematics

More
07 Oct 2010 15:11 #4564 by prabid
i am new user to emc2 i didn't even install it yet
i have some question and please some body replay me

i want to make 6 axis articulated arm robot and i want to attach touch probe to make inspection and make 2d scanning

i search for it all over the internet and read the manual for both hal and integrated emc2 file but i want some straight answers please

1.i can't make custom kinematics equation for my robot so can i use the ready made file in emc2 for puma 560 robot to work with my dimension of my robot and if yes how can i do it or how can i learn to make this changes

2. how can i know if my kinematics are right are there any simulator which i can simulate my robot with this kinematics i draw a cad files with solidworks 2010 to my robot

3.how can i configure the emc2 to work with the touch probe i read the manual and many thread in other forums but i just can't get it

4. if i want to add automatic too change can that be done with emc2 or not and if yes how can i make it ??

and there is the actual code to puma 560 robot
/*****************************************************************
* Description: pumakins.c
*   Kinematics for puma typed robots
*   Set the params using HAL to fit your robot
*
*   Derived from a work by Fred Proctor
*
* Author: 
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change:
*******************************************************************
*/

#include "rtapi_math.h"
#include "posemath.h"
#include "pumakins.h"
#include "kinematics.h"             /* decls for kinematicsForward, etc. */


#ifdef RTAPI
#include "rtapi.h"                /* RTAPI realtime OS API */
#include "rtapi_app.h"                /* RTAPI realtime module decls */
#include "hal.h"

struct haldata {
    hal_float_t *a2, *a3, *d3, *d4;
} *haldata = 0;


#define PUMA_A2 (*(haldata->a2))
#define PUMA_A3 (*(haldata->a3))
#define PUMA_D3 (*(haldata->d3))
#define PUMA_D4 (*(haldata->d4))


int kinematicsForward(const double * joint,
                      EmcPose * world,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{

   double s1, s2, s3, s4, s5, s6;
   double c1, c2, c3, c4, c5, c6;
   double s23;
   double c23;
   double t1, t2, t3, t4, t5;
   double sumSq, k;
   PmHomogeneous hom;
   PmPose worldPose;
   PmRpy rpy;

   /* Calculate sin of joints for future use */
   s1 = sin(joint[0]*PM_PI/180);
   s2 = sin(joint[1]*PM_PI/180);
   s3 = sin(joint[2]*PM_PI/180);
   s4 = sin(joint[3]*PM_PI/180);
   s5 = sin(joint[4]*PM_PI/180);
   s6 = sin(joint[5]*PM_PI/180);

   /* Calculate cos of joints for future use */
   c1 = cos(joint[0]*PM_PI/180);
   c2 = cos(joint[1]*PM_PI/180);
   c3 = cos(joint[2]*PM_PI/180);
   c4 = cos(joint[3]*PM_PI/180);
   c5 = cos(joint[4]*PM_PI/180);
   c6 = cos(joint[5]*PM_PI/180);

   s23 = c2 * s3 + s2 * c3;
   c23 = c2 * c3 - s2 * s3;

   /* Calculate terms to be used in definition of... */
   /* first column of rotation matrix.               */
   t1 = c4 * c5 * c6 - s4 * s6;
   t2 = s23 * s5 * c6;
   t3 = s4 * c5 * c6 + c4 * s6;
   t4 = c23 * t1 - t2;
   t5 = c23 * s5 * c6;

   /* Define first column of rotation matrix */
   hom.rot.x.x = c1 * t4 + s1 * t3;
   hom.rot.x.y = s1 * t4 - c1 * t3;
   hom.rot.x.z = -s23 * t1 - t5;

   /* Calculate terms to be used in definition of...  */
   /* second column of rotation matrix.               */
   t1 = -c4 * c5 * s6 - s4 * c6;
   t2 = s23 * s5 * s6;
   t3 = c4 * c6 - s4 * c5 * s6;
   t4 = c23 * t1 + t2;
   t5 = c23 * s5 * s6;

   /* Define second column of rotation matrix */
   hom.rot.y.x = c1 * t4 + s1 * t3;
   hom.rot.y.y = s1 * t4 - c1 * t3;
   hom.rot.y.z = -s23 * t1 + t5;

   /* Calculate term to be used in definition of... */
   /* third column of rotation matrix.              */
   t1 = c23 * c4 * s5 + s23 * c5;

   /* Define third column of rotation matrix */
   hom.rot.z.x = -c1 * t1 - s1 * s4 * s5;
   hom.rot.z.y = -s1 * t1 + c1 * s4 * s5;
   hom.rot.z.z = s23 * c4 * s5 - c23 * c5;

   /* Calculate term to be used in definition of...  */
   /* position vector.                               */
   t1 = PUMA_A2 * c2 + PUMA_A3 * c23 - PUMA_D4 * s23;

   /* Define position vector */
   hom.tran.x = c1 * t1 - PUMA_D3 * s1;
   hom.tran.y = s1 * t1 + PUMA_D3 * c1;
   hom.tran.z = -PUMA_A3 * s23 - PUMA_A2 * s2 - PUMA_D4 * c23;

   /* Calculate terms to be used to...   */
   /* determine flags.                   */
   sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y -
           PUMA_D3 * PUMA_D3;
   k = (sumSq + hom.tran.z * hom.tran.z - PUMA_A2 * PUMA_A2 -
       PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) /
       (2.0 * PUMA_A2);

   /* reset flags */
   *iflags = 0;

   /* Set shoulder-up flag if necessary */
   if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) +
       atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ)
   {
     *iflags |= PUMA_SHOULDER_RIGHT;
   }

   /* Set elbow down flag if necessary */
   if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) +
       atan2(k, -sqrt(PUMA_A3 * PUMA_A3 +
       PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ)
   {
      *iflags |= PUMA_ELBOW_DOWN;
   }

   /* set singular flag if necessary */
   t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1;
   t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 +
         hom.rot.z.z * s23;
   if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ)
   {
      *iflags |= PUMA_SINGULAR;
   }

   /* if not singular set wrist flip flag if necessary */
   else{
     if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ))
     {
       *iflags |= PUMA_WRIST_FLIP;
     }
   }

   /* convert hom.rot to world->quat */
   pmHomPoseConvert(hom, &worldPose);
   pmQuatRpyConvert(worldPose.rot,&rpy);
   world->tran = worldPose.tran;
   world->a = rpy.r * 180.0/PM_PI;
   world->b = rpy.p * 180.0/PM_PI;
   world->c = rpy.y * 180.0/PM_PI;

   
   /* return 0 and exit */
   return 0;
}

int kinematicsInverse(const EmcPose * world,
                      double * joint,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
   PmHomogeneous hom;
   PmPose worldPose;
   PmRpy rpy;
   int singular;

   double t1, t2, t3;
   double k;
   double sumSq;

   double th1;
   double th3;
   double th23;
   double th2;
   double th4;
   double th5;
   double th6;

   double s1, c1;
   double s3, c3;
   double s23, c23;
   double s4, c4;
   double s5, c5;
   double s6, c6;

   /* reset flags */
   *fflags = 0;

   /* convert pose to hom */
   worldPose.tran = world->tran;
   rpy.r = world->a*PM_PI/180.0;
   rpy.p = world->b*PM_PI/180.0;
   rpy.y = world->c*PM_PI/180.0;
   pmRpyQuatConvert(rpy,&worldPose.rot);
   pmPoseHomConvert(worldPose, &hom);

   /* Joint 1 (2 independent solutions) */

   /* save sum of squares for this and subsequent calcs */
   sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y -
           PUMA_D3 * PUMA_D3;

   /* FIXME-- is use of + sqrt shoulder right or left? */
   if (*iflags & PUMA_SHOULDER_RIGHT){
     th1 = atan2(hom.tran.y, hom.tran.x) - atan2(PUMA_D3, -sqrt(sumSq));
   }
   else{
     th1 = atan2(hom.tran.y, hom.tran.x) - atan2(PUMA_D3, sqrt(sumSq));
   }

   /* save sin, cos for later calcs */
   s1 = sin(th1);
   c1 = cos(th1);

   /* Joint 3 (2 independent solutions) */

   k = (sumSq + hom.tran.z * hom.tran.z - PUMA_A2 * PUMA_A2 -
       PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) / (2.0 * PUMA_A2);

   /* FIXME-- is use of + sqrt elbow up or down? */
   if (*iflags & PUMA_ELBOW_DOWN){
     th3 = atan2(PUMA_A3, PUMA_D4) - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k));
   }
   else{
     th3 = atan2(PUMA_A3, PUMA_D4) -
           atan2(k, sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k));
   }

   /* compute sin, cos for later calcs */
   s3 = sin(th3);
   c3 = cos(th3);

   /* Joint 2 */

   t1 = (-PUMA_A3 - PUMA_A2 * c3) * hom.tran.z +
        (c1 * hom.tran.x + s1 * hom.tran.y) * (PUMA_A2 * s3 - PUMA_D4);
   t2 = (PUMA_A2 * s3 - PUMA_D4) * hom.tran.z +
        (PUMA_A3 + PUMA_A2 * c3) * (c1 * hom.tran.x + s1 * hom.tran.y);
   t3 = hom.tran.z * hom.tran.z + (c1 * hom.tran.x + s1 * hom.tran.y) *
        (c1 * hom.tran.x + s1 * hom.tran.y);

   th23 = atan2(t1, t2);
   th2 = th23 - th3;

   /* compute sin, cos for later calcs */
   s23 = t1 / t3;
   c23 = t2 / t3;

   /* Joint 4 */

   t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1;
   t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + hom.rot.z.z * s23;
   if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ){
     singular = 1;
     *fflags |= PUMA_REACH;
     th4 = joint[3]*PM_PI/180;            /* use current value */
   }
   else{
     singular = 0;
     th4 = atan2(t1, t2);
   }

   /* compute sin, cos for later calcs */
   s4 = sin(th4);
   c4 = cos(th4);

   /* Joint 5 */

   s5 = hom.rot.z.z * (s23 * c4) -
        hom.rot.z.x * (c1 * c23 * c4 + s1 * s4) -
        hom.rot.z.y * (s1 * c23 * c4 - c1 * s4);
   c5 =-hom.rot.z.x * (c1 * s23) - hom.rot.z.y *
        (s1 * s23) - hom.rot.z.z * c23;
   th5 = atan2(s5, c5);

   /* Joint 6 */

   s6 = hom.rot.x.z * (s23 * s4) - hom.rot.x.x *
        (c1 * c23 * s4 - s1 * c4) - hom.rot.x.y *
        (s1 * c23 * s4 + c1 * c4);
   c6 = hom.rot.x.x * ((c1 * c23 * c4 + s1 * s4) *
        c5 - c1 * s23 * s5) + hom.rot.x.y *
        ((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) -
        hom.rot.x.z * (s23 * c4 * c5 + c23 * s5);
   th6 = atan2(s6, c6);

   /* FIXME-- is wrist flip the normal or offset results? */
   if (*iflags & PUMA_WRIST_FLIP){
     th4 = th4 + PM_PI;
     th5 = -th5;
     th6 = th6 + PM_PI;
   }

   /* copy out */
   joint[0] = th1*180/PM_PI;
   joint[1] = th2*180/PM_PI;
   joint[2] = th3*180/PM_PI;
   joint[3] = th4*180/PM_PI;
   joint[4] = th5*180/PM_PI;
   joint[5] = th6*180/PM_PI;

   return singular == 0 ? 0 : -1;
}

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_FORWARD_ONLY;
  return KINEMATICS_BOTH;
}


EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);

int comp_id;

int rtapi_app_main(void) {
    int res=0;
    
    comp_id = hal_init("pumakins");
    if (comp_id < 0) return comp_id;
    
    haldata = hal_malloc(sizeof(struct haldata));
    if (!haldata) goto error;

    if((res = hal_pin_float_new("pumakins.A2", HAL_IO, &(haldata->a2), comp_id)) != HAL_SUCCESS) goto error;
    if((res = hal_pin_float_new("pumakins.A3", HAL_IO, &(haldata->a3), comp_id)) != HAL_SUCCESS) goto error;
    if((res = hal_pin_float_new("pumakins.D3", HAL_IO, &(haldata->d3), comp_id)) != HAL_SUCCESS) goto error;
    if((res = hal_pin_float_new("pumakins.D4", HAL_IO, &(haldata->d4), comp_id)) != HAL_SUCCESS) goto error;
    
    PUMA_A2 = DEFAULT_PUMA560_A2;
    PUMA_A3 = DEFAULT_PUMA560_A3;
    PUMA_D3 = DEFAULT_PUMA560_D3;
    PUMA_D4 = DEFAULT_PUMA560_D4;
    hal_ready(comp_id);
    return 0;
    
error:
    hal_exit(comp_id);
    return res;
}

void rtapi_app_exit(void) { hal_exit(comp_id); }
#endif

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

More
12 Oct 2010 15:55 #4643 by andypugh
prabid wrote:

1.i can't make custom kinematics equation for my robot so can i use the ready made file in emc2 for puma 560 robot to work with my dimension of my robot and if yes how can i do it or how can i learn to make this changes


The pumakins kinematics module creates 4 "pins" in the HAL that can be set to suit the geometry of your robot.
You would use lines like:
setp pumakins.A2 200
setp pumakins.A3 300
setp pumakins.D3 100
setp pumakins.D4 500
to set the kinematics to suit your particular robot.

The A and D values are the Denavit-Hartenberg parameters of your robot.
en.wikipedia.org/wiki/Denavit-Hartenberg_Parameters

2. how can i know if my kinematics are right are there any simulator which i can simulate my robot with this kinematics i draw a cad files with solidworks 2010 to my robot


The puma sample config shipped with EMC2 includes a 3D preview using wiki.linuxcnc.org/emcinfo.pl?Vismach , this might do what you want.

3.how can i configure the emc2 to work with the touch probe i read the manual and many thread in other forums but i just can't get it

You will probably need to write a G-code program to probe a specific pattern. If you want to do more than single-point probing, then it is going to be rather tricky.

4. if i want to add automatic too change can that be done with emc2 or not and if yes how can i make it ??

It can be done, and there are at least two ways to do it. Many people use Classic Ladder for the purpose, or you could write your own toolchanger module (using "comp" makes this rather easier. linuxcnc.org/docs/html/hal_comp.html

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

More
15 Oct 2010 10:06 #4694 by prabid
can i make the arm robot and u make the configuration of the robot in emc2 i can send the cad files and soon i finish them because i really didn't get what u say where i can find the
setp pumakins.A2 200
setp pumakins.A3 300
setp pumakins.D3 100
setp pumakins.D4 500

i search for it in hal file and didn't found anything and didn't the code which i put in first post i will change it or not this the code of the original file of puma 560
and for the record i am not making puma 560 i will just use it's kinematics in my own design arm because u can't image it is very different for me to do and configure properly the emc2

i will connect in the end effector laser scanner better than prope to make the 3d cad models and by using emc2 i will make certain positions to move the scanner to and by another software will the output come

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

More
15 Oct 2010 11:15 #4695 by andypugh
prabid wrote:

and for the record i am not making puma 560 i will just use it's kinematics in my own design arm


If it is not an actual Puma560 then you probably need to use the "genserkins" kinematics module.

In fact, the existing Puma_sim sample config (part of the liveCD install) uses genserkins.

The HAL file is here (and also on your hard drive)
git.linuxcnc.org/gitweb?p=emc2.git;a=blo...a8fe831906f3;hb=HEAD

You can see that it loads genserkins near the start, then defines the DH parameters of the joints later in the code.

You ought to be able to take that sample config and then edit it to suit your robot, once you have worked out the DH parameters that define your robot.

In response to the request that I should create the HAL file for you: I have no experience of calculating DH parameters, and no interest in doing so. It's your robot.

This WIkpedia page explains Denavit Hartenberg parameters very clearly with an animation:
en.wikipedia.org/wiki/Denavit-Hartenberg_Parameters

I am a little puzzled that the genserkins module only creates ALPHA pina and not THETA ones, but the genserkins manual page gives a reference to a book that probably explains it
www.linuxcnc.org/docview/html//man/man9/genhexkins.9.html

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

More
15 Oct 2010 23:42 #4708 by prabid
thanks for helping and i will begin to make the robot first then doing the configurations of emc2 and i will post as soon as possible all my result and my progress
sorry i have another question what do u think stepper or servo for the robotic arm ???
i think the servo is better

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

More
16 Oct 2010 00:05 #4709 by andypugh
Servo is better, but a lot more expensive, and a lot more difficult to get working well (loop tuning)
The following user(s) said Thank You: bodhi_94

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

More
16 Oct 2010 11:19 #4714 by BigJohnT
The servo/stepper choice depends on your requirements and budget. Servos are faster and stronger and and cost more and don't loose position information. Steppers are slower and less powerful and usually don't have position feedback. As Andy said servos can be more difficult to tune and a stepper setup is pretty simple to get going.

John

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

More
13 Jan 2011 19:42 - 13 Jan 2011 19:43 #6666 by billi83
I have a homemade 6dof manipulator like puma with steppers, but:

Theta5 = (Theta_motor_5+Theta_motor_6)/2.

Theta6= (Theta_motor_5-Theta_motor_6)/2.

It happens because I use a differential mechanism on the last 2 dof with 3 bevel gear.

How to implement that in EMC2?
Last edit: 13 Jan 2011 19:43 by billi83.

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

More
13 Jan 2011 22:24 #6668 by andypugh
Sounds interesting.
The equations look linear and invertible, so it should be just a case of creating a kinematics module based on the nearest conventional robot and editing the equations.

(You will then need to recompile EMC2)

git.linuxcnc.org/gitweb?p=emc2.git;a=blo...16873ab66a8e;hb=HEAD
Lines 56 to 69 and lines 312-317 need altering to take into account your odd mechanism. I think it should be a simple substitution in just those lines.

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

More
14 Jan 2011 10:15 #6678 by billi83
thanks for helping and I think it’ll works with pumakins.c, but I'm using genserkins.c (sorry, I forgot to say).

If I could create this signals Phi4 and Phi5 in hal:
Phi4 = J4pos + J5pos
Phi5 = J4pos - J5pos

And If I use Phi4 and Phi5 to generate the signal for the stepper, I think it will works.
(without recompile EMC2)

But I don’t know how to create Phi4 and Phi5 in hal.

Is it possible?

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

Time to create page: 0.248 seconds
Powered by Kunena Forum