# making 6 axis robotic arm with puma 560 kinematics

*making 6 axis robotic arm with puma 560 kinematics*was created by

*prabid*

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.

*andypugh*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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

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

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.html4. if i want to add automatic too change can that be done with emc2 or not and if yes how can i make it ??

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

*prabid*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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.

*andypugh*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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

*prabid*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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.

*andypugh*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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

*BigJohnT*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

John

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

*billi83*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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?

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

*andypugh*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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.

*billi83*on topic

*Re:making 6 axis robotic arm with puma 560 kinematics*

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.