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

- Committer:
- Bobty
- Date:
- 2015-05-15
- Revision:
**0:9a3fc5ff8de3**

### File content as of revision 0:9a3fc5ff8de3:

// SCARA_Arms library // Code to compute the shoulder angles required to move a Dual-Arm SCARA robot to required x,y position // More information at http://robdobson.com/scara-arm-calculations // Copyright (C) Rob Dobson 2013-2015, MIT License #include "mbed.h" #include "math.h" #include "ScaraArms.h" const double PI = 3.14159; // Description of a Dual-Arm SCARA robot // 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) // Constructor // 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) ScaraArms::ScaraArms(double betweenShouldersMM, double shoulderToElbowMM, double elbowToHandMM, double handToToolMM) { _betweenShouldersMM = betweenShouldersMM; _shoulderToElbowMM = shoulderToElbowMM; _elbowToHandMM = elbowToHandMM; _handToToolMM = handToToolMM; } // Compute the angles at the shoulders given the required XY position of the tool in radians // Note that the way the angles are measured is that theta1 (the left arm angle) is measured counter-clockwise from 3 O'Clock // theta2 (the right arm angle) is measured clockwise from 9 O'Clock // To calculate both clockwise from 9 O'Clock use PI-theta2 // To convert the angles to degrees multiply by 180/PI // The XY position has it's origin (0,0) at the left shoulder void ScaraArms::ConvertXYtoScara(double x, double y, double &theta1, double &theta2) { double L1 = _shoulderToElbowMM; double L2 = _elbowToHandMM; double L3 = _handToToolMM; double alpha = PI/4; double C = _betweenShouldersMM; double L4 = sqrt(L2*L2+L3*L3-2*L2*L3*cos(PI-alpha)); double eta = acos((L3*L3+L4*L4-L2*L2)/(2*L3*L4)); // 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); double d3 = sqrt((C-x)*(C-x)+y*y); double theta2c = atan2(y, (C-x)); double theta2d = acos((d3*d3+L1*L1-L4*L4) / (2*L1*d3)); theta2 = theta2c+theta2d; double x4 = C+L1*cos(PI-theta2); double y4 = L1*sin(PI-theta2); double delta = atan2(x4-x,y-y4); double x1 = x + L3*sin(delta - eta); double y1 = y - L3*cos(delta - eta); double d1 = sqrt(x1*x1+y1*y1); double theta1a = atan2(y1, x1); double theta1b = acos((d1*d1+L1*L1-L2*L2) / (2*L1*d1)); theta1 = theta1a+theta1b; } // Compute the XY position of the tool given the angles at the shoulders // See the comments above for the reverse calculation concerning the way the angles are measured and the origin // of the XY coordinates // These calculations use a different mathematical model as the conversion in this direction is simpler and // it provides a useful cross check void ScaraArms::ConvertScaratoXY(double theta1, double theta2, double &x, double &y) { double L1 = _shoulderToElbowMM; double L2 = _elbowToHandMM; double L3 = _handToToolMM; double alpha = PI/4; double C = _betweenShouldersMM; // Uses different maths as a cross check double theta1rev = theta1; double theta2rev = PI - theta2; double xh1 = L1 * cos(theta1rev); double yh1 = sin(theta1rev) * L1; double xh2 = C + L1 * cos(theta2rev); double yh2 = sin(theta2rev) * L1; double gamma = atan2((yh1-yh2), (xh1-xh2)); double l = sqrt(pow(xh1-xh2,2)+pow(yh1-yh2,2)); double h = sqrt(pow(L2,2)-pow(l/2,2)); double xi = (xh1+xh2)/2 + (h * sin(gamma)); double yi = (yh1+yh2)/2 + fabs(h * cos(gamma)); double psichk = atan2((yi-yh2),(xh2-xi)); x = xi - L3 * cos(psichk+alpha); y = yi + L3 * sin(psichk+alpha); }