Rob Dobson / SCARA_Arms

Calculations for a Dual-Arm SCARA robot Computes the shoulder angles to make the robot move to an XY position Also the reverse calculation - mainly useful as a cross-check

Description of a Dual-Arm SCARA robot (more info at http://robdobson.com/scara-arm-calculations)

The two arms of the robot are akin to two human arms with the hands permanently clasped together The arms are always in the same plane (generally horizontal) and the hands hold a stick (also in the same plane) On the end of the stick is the tool (pen/actuayor/nozzle/laser etc)

The measurements required to describe a Dual-Arm SCARA robot of the kind described above are: 1) Distance between shoulders 2) Distance from shoulder to elbow 3) Distance from elbow to hand 4) Distance from hand to tool (length of stick holding the tool)

Example Usage

Setup calculation double betweenShouldersMM = 320; double shoulderToElbowMM = 138; double elbowToHandMM = 307; double handToToolMM = 22.5; ScaraArms scaraArms(betweenShouldersMM, shoulderToElbowMM, elbowToHandMM, handToToolMM);

Compute the angles at the shoulders given the required XY position of the tool in radians double requiredX = 200; double requiredY = 180; double theta1, theta2; scaraArms.ConvertXYtoScara(requiredX, requiredY, theta1, theta2);

Show result printf("Angles required: left (from 3 O'Clock counter-clockwise) %f and right (from 9 O'Clock clockwise) %f", theta1, theta2);

ScaraArms.cpp@0:9a3fc5ff8de3, 2015-05-15 (annotated)

Committer:
Bobty
Date:
Fri May 15 08:50:58 2015 +0000
Revision:
0:9a3fc5ff8de3
Initial revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bobty 0:9a3fc5ff8de3 1 // SCARA_Arms library
Bobty 0:9a3fc5ff8de3 2 // Code to compute the shoulder angles required to move a Dual-Arm SCARA robot to required x,y position
Bobty 0:9a3fc5ff8de3 3 // More information at http://robdobson.com/scara-arm-calculations
Bobty 0:9a3fc5ff8de3 4 // Copyright (C) Rob Dobson 2013-2015, MIT License
Bobty 0:9a3fc5ff8de3 5
Bobty 0:9a3fc5ff8de3 6 #include "mbed.h"
Bobty 0:9a3fc5ff8de3 7 #include "math.h"
Bobty 0:9a3fc5ff8de3 8 #include "ScaraArms.h"
Bobty 0:9a3fc5ff8de3 9
Bobty 0:9a3fc5ff8de3 10 const double PI = 3.14159;
Bobty 0:9a3fc5ff8de3 11
Bobty 0:9a3fc5ff8de3 12 // Description of a Dual-Arm SCARA robot
Bobty 0:9a3fc5ff8de3 13 // The two arms of the robot are akin to two human arms with the hands permanently clasped together
Bobty 0:9a3fc5ff8de3 14 // The arms are always in the same plane (generally horizontal) and the hands hold a stick (also in the same plane)
Bobty 0:9a3fc5ff8de3 15 // On the end of the stick is the tool (pen/actuayor/nozzle/laser etc)
Bobty 0:9a3fc5ff8de3 16
Bobty 0:9a3fc5ff8de3 17 // Constructor
Bobty 0:9a3fc5ff8de3 18 // The measurements required to describe a Dual-Arm SCARA robot of the kind described above are:
Bobty 0:9a3fc5ff8de3 19 // 1) Distance between shoulders
Bobty 0:9a3fc5ff8de3 20 // 2) Distance from shoulder to elbow
Bobty 0:9a3fc5ff8de3 21 // 3) Distance from elbow to hand
Bobty 0:9a3fc5ff8de3 22 // 4) Distance from hand to tool (length of stick holding the tool)
Bobty 0:9a3fc5ff8de3 23
Bobty 0:9a3fc5ff8de3 24 ScaraArms::ScaraArms(double betweenShouldersMM, double shoulderToElbowMM, double elbowToHandMM, double handToToolMM)
Bobty 0:9a3fc5ff8de3 25 {
Bobty 0:9a3fc5ff8de3 26 _betweenShouldersMM = betweenShouldersMM;
Bobty 0:9a3fc5ff8de3 27 _shoulderToElbowMM = shoulderToElbowMM;
Bobty 0:9a3fc5ff8de3 28 _elbowToHandMM = elbowToHandMM;
Bobty 0:9a3fc5ff8de3 29 _handToToolMM = handToToolMM;
Bobty 0:9a3fc5ff8de3 30 }
Bobty 0:9a3fc5ff8de3 31
Bobty 0:9a3fc5ff8de3 32 // Compute the angles at the shoulders given the required XY position of the tool in radians
Bobty 0:9a3fc5ff8de3 33 // Note that the way the angles are measured is that theta1 (the left arm angle) is measured counter-clockwise from 3 O'Clock
Bobty 0:9a3fc5ff8de3 34 // theta2 (the right arm angle) is measured clockwise from 9 O'Clock
Bobty 0:9a3fc5ff8de3 35 // To calculate both clockwise from 9 O'Clock use PI-theta2
Bobty 0:9a3fc5ff8de3 36 // To convert the angles to degrees multiply by 180/PI
Bobty 0:9a3fc5ff8de3 37 // The XY position has it's origin (0,0) at the left shoulder
Bobty 0:9a3fc5ff8de3 38 void ScaraArms::ConvertXYtoScara(double x, double y, double &theta1, double &theta2)
Bobty 0:9a3fc5ff8de3 39 {
Bobty 0:9a3fc5ff8de3 40 double L1 = _shoulderToElbowMM;
Bobty 0:9a3fc5ff8de3 41 double L2 = _elbowToHandMM;
Bobty 0:9a3fc5ff8de3 42 double L3 = _handToToolMM;
Bobty 0:9a3fc5ff8de3 43 double alpha = PI/4;
Bobty 0:9a3fc5ff8de3 44 double C = _betweenShouldersMM;
Bobty 0:9a3fc5ff8de3 45 double L4 = sqrt(L2*L2+L3*L3-2*L2*L3*cos(PI-alpha));
Bobty 0:9a3fc5ff8de3 46 double eta = acos((L3*L3+L4*L4-L2*L2)/(2*L3*L4));
Bobty 0:9a3fc5ff8de3 47
Bobty 0:9a3fc5ff8de3 48 // printf("L1 %f, L2 %f, L3 %f, L4 %f, eta %f, alpha %f\r\n", L1, L2, L3, L4, eta*180/PI, alpha*180/PI);
Bobty 0:9a3fc5ff8de3 49
Bobty 0:9a3fc5ff8de3 50 double d3 = sqrt((C-x)*(C-x)+y*y);
Bobty 0:9a3fc5ff8de3 51 double theta2c = atan2(y, (C-x));
Bobty 0:9a3fc5ff8de3 52 double theta2d = acos((d3*d3+L1*L1-L4*L4) / (2*L1*d3));
Bobty 0:9a3fc5ff8de3 53 theta2 = theta2c+theta2d;
Bobty 0:9a3fc5ff8de3 54
Bobty 0:9a3fc5ff8de3 55 double x4 = C+L1*cos(PI-theta2);
Bobty 0:9a3fc5ff8de3 56 double y4 = L1*sin(PI-theta2);
Bobty 0:9a3fc5ff8de3 57
Bobty 0:9a3fc5ff8de3 58 double delta = atan2(x4-x,y-y4);
Bobty 0:9a3fc5ff8de3 59 double x1 = x + L3*sin(delta - eta);
Bobty 0:9a3fc5ff8de3 60 double y1 = y - L3*cos(delta - eta);
Bobty 0:9a3fc5ff8de3 61
Bobty 0:9a3fc5ff8de3 62 double d1 = sqrt(x1*x1+y1*y1);
Bobty 0:9a3fc5ff8de3 63 double theta1a = atan2(y1, x1);
Bobty 0:9a3fc5ff8de3 64 double theta1b = acos((d1*d1+L1*L1-L2*L2) / (2*L1*d1));
Bobty 0:9a3fc5ff8de3 65
Bobty 0:9a3fc5ff8de3 66 theta1 = theta1a+theta1b;
Bobty 0:9a3fc5ff8de3 67 }
Bobty 0:9a3fc5ff8de3 68
Bobty 0:9a3fc5ff8de3 69 // Compute the XY position of the tool given the angles at the shoulders
Bobty 0:9a3fc5ff8de3 70 // See the comments above for the reverse calculation concerning the way the angles are measured and the origin
Bobty 0:9a3fc5ff8de3 71 // of the XY coordinates
Bobty 0:9a3fc5ff8de3 72 // These calculations use a different mathematical model as the conversion in this direction is simpler and
Bobty 0:9a3fc5ff8de3 73 // it provides a useful cross check
Bobty 0:9a3fc5ff8de3 74 void ScaraArms::ConvertScaratoXY(double theta1, double theta2, double &x, double &y)
Bobty 0:9a3fc5ff8de3 75 {
Bobty 0:9a3fc5ff8de3 76 double L1 = _shoulderToElbowMM;
Bobty 0:9a3fc5ff8de3 77 double L2 = _elbowToHandMM;
Bobty 0:9a3fc5ff8de3 78 double L3 = _handToToolMM;
Bobty 0:9a3fc5ff8de3 79 double alpha = PI/4;
Bobty 0:9a3fc5ff8de3 80 double C = _betweenShouldersMM;
Bobty 0:9a3fc5ff8de3 81
Bobty 0:9a3fc5ff8de3 82 // Uses different maths as a cross check
Bobty 0:9a3fc5ff8de3 83 double theta1rev = theta1;
Bobty 0:9a3fc5ff8de3 84 double theta2rev = PI - theta2;
Bobty 0:9a3fc5ff8de3 85 double xh1 = L1 * cos(theta1rev);
Bobty 0:9a3fc5ff8de3 86 double yh1 = sin(theta1rev) * L1;
Bobty 0:9a3fc5ff8de3 87 double xh2 = C + L1 * cos(theta2rev);
Bobty 0:9a3fc5ff8de3 88 double yh2 = sin(theta2rev) * L1;
Bobty 0:9a3fc5ff8de3 89 double gamma = atan2((yh1-yh2), (xh1-xh2));
Bobty 0:9a3fc5ff8de3 90 double l = sqrt(pow(xh1-xh2,2)+pow(yh1-yh2,2));
Bobty 0:9a3fc5ff8de3 91 double h = sqrt(pow(L2,2)-pow(l/2,2));
Bobty 0:9a3fc5ff8de3 92 double xi = (xh1+xh2)/2 + (h * sin(gamma));
Bobty 0:9a3fc5ff8de3 93 double yi = (yh1+yh2)/2 + fabs(h * cos(gamma));
Bobty 0:9a3fc5ff8de3 94
Bobty 0:9a3fc5ff8de3 95 double psichk = atan2((yi-yh2),(xh2-xi));
Bobty 0:9a3fc5ff8de3 96 x = xi - L3 * cos(psichk+alpha);
Bobty 0:9a3fc5ff8de3 97 y = yi + L3 * sin(psichk+alpha);
Bobty 0:9a3fc5ff8de3 98 }
Bobty 0:9a3fc5ff8de3 99