It's not real mysterious at a second glance. The orange arm and the main black arm are the same length and counter-rotating, so all of the vertical motion cancels out. _+ Sin, -Sin and all that. Looks pretty cool though, probably easy to build with Lego parts.
It's not real mysterious at a second glance. The orange arm and the main black arm are the same length and counter-rotating, so all of the vertical motion cancels out. _+ Sin, -Sin and all that. Looks pretty cool though, probably easy to build with Lego parts.
Being a software guy I decided to understand this by modeling it in software. So I took my Processing Scara arm IK code and reproduced the behavior. I was able to do so if for each degree the humerus moved counterclockwise, the ulna moved two degrees clockwise. The full code is at the bottom, but the critical two lines are:
int originX = 320;
int originY = 200;
public class ScaraArm
{
// Angular values for common angles
public static final double ONE_DEGREE = Math.PI / 360;
public static final double RIGHT_ANGLE = Math.PI / 2;
public static final double STRAIGHT_ANGLE = Math.PI;
public static final double FULL_ROTATION = 2.0 * Math.PI;
private static final int SHOULDER = 0;
private static final int ELBOW = 1;
private static final int COUNT = 2;
// Size of robot bones in consistent units (mm recommended).
private int _humerus;
private int _ulna;
private int _humerusSq;
private int _ulnaSq;
// Coordinate of pen tip in Cartesian space.
private int _x, _y;
// Coordinates in arm scara polar space.
private double _shoulderAngleRads;
private double _elbowAngleRads;
/**
* Constructor used to initialize arm parameters.
* @param humerus - shoulder-to-elbow "bone" in mm
* @param ulna - elbow-to-wrist "bone" in mm
*/
public ScaraArm(int humerus, int ulna)
{
_humerus = humerus;
_ulna = ulna;
/* pre-calculations */
_humerusSq = _humerus * _humerus;
_ulnaSq = _ulna * _ulna;
}
/**
* Park - Move the arm servos to the parked position. Called during start and stop.
* By parking the arm the servos will start from a known position and minimize startup
* lurch.
*/
public void park()
{
setShoulder( RIGHT_ANGLE );
setElbow( RIGHT_ANGLE );
}
/**
* SetPosition : Arm positioning routine utilizing inverse kinematics. Since the arm
* is resting on a surface Z can only be positive. Servo movement constraints prevent
* y from being negative, But X can be a signed value.
* Note: This must be called before and of the move routines to initialize arm state.
* @param x - the side to side displacement.
* @param y - the distance out from the base center.
*/
public void setPosition( int x, int y )
{
// Save the Cartesian space coordinates.
_x = x;
_y = y;
// Use Pythagorean theorem to calculate shoulder to wrist distance.
int s_w = ( x * x ) + ( y * y );
double s_w_sqrt = Math.sqrt( s_w );
// s_w angle to centerline
double a1 = Math.atan2( y, x );
// s_w angle to humerus, note the need to clamp this value between -1 to 1!
double q = (double)(_humerusSq - _ulnaSq + s_w) / (2.0 * _humerus * s_w_sqrt);
if (q > 1)
q = 1;
else if (q < -1)
q = -1;
double a2 = Math.acos(q);
// shoulder angle
double shoulderRads = a1 + a2;
setShoulder(shoulderRads);
// elbow angle
double elb_angle_r = Math.acos((double)(_humerusSq + _ulnaSq - s_w) / ( 2.0 * _humerus * _ulna ));
// Set the joints
setElbow(elb_angle_r);
}
/**
* SetY : Arm positioning routine utilizing inverse kinematics. Moves the arm from
* the current Y coordinate to the newY passed in. It maintains all other position
* state.
* @param newY - the new forward/back displacement
*/
public void setY(int newY)
{
setPosition(_x, newY);
}
/**
* SetX : Arm positioning routine utilizing inverse kinematics. Moves the arm from
* the current X coordinate to the newX passed in. It maintains all other position
* state.
* @param newX - the new height above the table
*/
public void setX(int newX)
{
setPosition(newX, _y);
}
/**
* getShoulder returns a 32-bit floating point radian value
*/
public double getShoulder()
{
return _shoulderAngleRads;
}
/**
* setShoulder: Sets the should angle member, computes the servo pulse width for that
* angle, and sets the associated servo.
* @param shoulderAngleRads - 32-bit floating point radian value
*/
public void setShoulder(double shoulderAngleRads)
{
// Rotate the coordinate system by a right angle so that a straight angle is
// a full extension of the joint.
_shoulderAngleRads = shoulderAngleRads;
}
/**
* SetElbow: Sets elbow angle data member, computes the servo pulse width for that
* angle, and sets the associated servo.
* @param elbowAngle - floating point radian value
*/
public double getElbow()
{
return _elbowAngleRads;
}
/**
* SetElbow: Sets elbow angle data member, computes the servo pulse width for that
* angle, and sets the associated servo.
* @param elbowAngle - floating point radian value
*/
public void setElbow(double elbowAngle)
{
// Rotate the coordinate system by a right angle so that a straight angle is
// a full extension of the joint.
_elbowAngleRads = elbowAngle;
}
/**
*/
public void drawArm()
{
double hx = _humerus * Math.cos(_shoulderAngleRads);
double hy = _humerus * Math.sin(_shoulderAngleRads);
int screenX = (int)(originX + hx);
int screenY = (int)(originY - hy);
line(originX, originY, screenX, screenY);
// Convert elbow angle so that it is relative to shoulder angle
double elbow = _elbowAngleRads - STRAIGHT_ANGLE + _shoulderAngleRads;
int wx = (int)(_ulna * Math.cos(elbow)) + screenX;
int wy = screenY - (int)(_ulna * Math.sin(elbow));
line( screenX, screenY, wx, wy);
text("(" + _x + ", " + _y + ") a1 = " + (int)Math.toDegrees(_shoulderAngleRads) + ", a2 = " + (int)Math.toDegrees(_elbowAngleRads), wx, wy);
}
};
int HUMERUS = 140;
int ULNA = 140;
ScaraArm robotArm = new ScaraArm(HUMERUS, ULNA);
PFont f;
void setup()
{
size(2 * originX, 2 * originY);
f = createFont("Arial", 16, true);
robotArm.setShoulder( ScaraArm.RIGHT_ANGLE );
robotArm.setElbow( 0 );
}
void draw() {
background(255);
textFont(f, 16);
fill(0);
line(originX, 0, originX, 2 * originY);
line(0, originY, 2 * originX, originY);
int localX = mouseX - originX;
int localY = originY - mouseY;
robotArm.drawArm();
robotArm.setShoulder(robotArm.getShoulder() + ScaraArm.ONE_DEGREE);
robotArm.setElbow(robotArm.getElbow() - 2 * ScaraArm.ONE_DEGREE);
}
Comments
Being a software guy I decided to understand this by modeling it in software. So I took my Processing Scara arm IK code and reproduced the behavior. I was able to do so if for each degree the humerus moved counterclockwise, the ulna moved two degrees clockwise. The full code is at the bottom, but the critical two lines are:
robotArm.setShoulder(robotArm.getShoulder() + ScaraArm.ONE_DEGREE);
robotArm.setElbow(robotArm.getElbow() - 2 * ScaraArm.ONE_DEGREE);
Full code below:
int originX = 320; int originY = 200; public class ScaraArm { // Angular values for common angles public static final double ONE_DEGREE = Math.PI / 360; public static final double RIGHT_ANGLE = Math.PI / 2; public static final double STRAIGHT_ANGLE = Math.PI; public static final double FULL_ROTATION = 2.0 * Math.PI; private static final int SHOULDER = 0; private static final int ELBOW = 1; private static final int COUNT = 2; // Size of robot bones in consistent units (mm recommended). private int _humerus; private int _ulna; private int _humerusSq; private int _ulnaSq; // Coordinate of pen tip in Cartesian space. private int _x, _y; // Coordinates in arm scara polar space. private double _shoulderAngleRads; private double _elbowAngleRads; /** * Constructor used to initialize arm parameters. * @param humerus - shoulder-to-elbow "bone" in mm * @param ulna - elbow-to-wrist "bone" in mm */ public ScaraArm(int humerus, int ulna) { _humerus = humerus; _ulna = ulna; /* pre-calculations */ _humerusSq = _humerus * _humerus; _ulnaSq = _ulna * _ulna; } /** * Park - Move the arm servos to the parked position. Called during start and stop. * By parking the arm the servos will start from a known position and minimize startup * lurch. */ public void park() { setShoulder( RIGHT_ANGLE ); setElbow( RIGHT_ANGLE ); } /** * SetPosition : Arm positioning routine utilizing inverse kinematics. Since the arm * is resting on a surface Z can only be positive. Servo movement constraints prevent * y from being negative, But X can be a signed value. * Note: This must be called before and of the move routines to initialize arm state. * @param x - the side to side displacement. * @param y - the distance out from the base center. */ public void setPosition( int x, int y ) { // Save the Cartesian space coordinates. _x = x; _y = y; // Use Pythagorean theorem to calculate shoulder to wrist distance. int s_w = ( x * x ) + ( y * y ); double s_w_sqrt = Math.sqrt( s_w ); // s_w angle to centerline double a1 = Math.atan2( y, x ); // s_w angle to humerus, note the need to clamp this value between -1 to 1! double q = (double)(_humerusSq - _ulnaSq + s_w) / (2.0 * _humerus * s_w_sqrt); if (q > 1) q = 1; else if (q < -1) q = -1; double a2 = Math.acos(q); // shoulder angle double shoulderRads = a1 + a2; setShoulder(shoulderRads); // elbow angle double elb_angle_r = Math.acos((double)(_humerusSq + _ulnaSq - s_w) / ( 2.0 * _humerus * _ulna )); // Set the joints setElbow(elb_angle_r); } /** * SetY : Arm positioning routine utilizing inverse kinematics. Moves the arm from * the current Y coordinate to the newY passed in. It maintains all other position * state. * @param newY - the new forward/back displacement */ public void setY(int newY) { setPosition(_x, newY); } /** * SetX : Arm positioning routine utilizing inverse kinematics. Moves the arm from * the current X coordinate to the newX passed in. It maintains all other position * state. * @param newX - the new height above the table */ public void setX(int newX) { setPosition(newX, _y); } /** * getShoulder returns a 32-bit floating point radian value */ public double getShoulder() { return _shoulderAngleRads; } /** * setShoulder: Sets the should angle member, computes the servo pulse width for that * angle, and sets the associated servo. * @param shoulderAngleRads - 32-bit floating point radian value */ public void setShoulder(double shoulderAngleRads) { // Rotate the coordinate system by a right angle so that a straight angle is // a full extension of the joint. _shoulderAngleRads = shoulderAngleRads; } /** * SetElbow: Sets elbow angle data member, computes the servo pulse width for that * angle, and sets the associated servo. * @param elbowAngle - floating point radian value */ public double getElbow() { return _elbowAngleRads; } /** * SetElbow: Sets elbow angle data member, computes the servo pulse width for that * angle, and sets the associated servo. * @param elbowAngle - floating point radian value */ public void setElbow(double elbowAngle) { // Rotate the coordinate system by a right angle so that a straight angle is // a full extension of the joint. _elbowAngleRads = elbowAngle; } /** */ public void drawArm() { double hx = _humerus * Math.cos(_shoulderAngleRads); double hy = _humerus * Math.sin(_shoulderAngleRads); int screenX = (int)(originX + hx); int screenY = (int)(originY - hy); line(originX, originY, screenX, screenY); // Convert elbow angle so that it is relative to shoulder angle double elbow = _elbowAngleRads - STRAIGHT_ANGLE + _shoulderAngleRads; int wx = (int)(_ulna * Math.cos(elbow)) + screenX; int wy = screenY - (int)(_ulna * Math.sin(elbow)); line( screenX, screenY, wx, wy); text("(" + _x + ", " + _y + ") a1 = " + (int)Math.toDegrees(_shoulderAngleRads) + ", a2 = " + (int)Math.toDegrees(_elbowAngleRads), wx, wy); } }; int HUMERUS = 140; int ULNA = 140; ScaraArm robotArm = new ScaraArm(HUMERUS, ULNA); PFont f; void setup() { size(2 * originX, 2 * originY); f = createFont("Arial", 16, true); robotArm.setShoulder( ScaraArm.RIGHT_ANGLE ); robotArm.setElbow( 0 ); } void draw() { background(255); textFont(f, 16); fill(0); line(originX, 0, originX, 2 * originY); line(0, originY, 2 * originX, originY); int localX = mouseX - originX; int localY = originY - mouseY; robotArm.drawArm(); robotArm.setShoulder(robotArm.getShoulder() + ScaraArm.ONE_DEGREE); robotArm.setElbow(robotArm.getElbow() - 2 * ScaraArm.ONE_DEGREE); }That's right. Don't forget about sidereal rotation, which requires the 2:1 gear stepup.
PS: Can you say "ulna" on TV? It's not real humerus with the moral majority.