Inverse Kinematics in FORTH and Java
Martin_H
Posts: 4,051
I'm build a simple scara robot arm. Unlike a revolute coordinate arm it primarily operates in an XY plane. This greatly simplifies Inverse Kinematics as only two joints need to be considered (shoulder and elbow). So I took my C++ code, quickly ported it to Java, then simplified it to deal with a scara arm. What I would like to do it translate this to FORTH and I already have a good start. But the code has a number of mathematical functions that I'm unsure how they are handled in FORTH. This includes: square roots, arc tangents, arc cosine, and radian angles such as Pi.
Here's the scara arm code in Java:
Below is start of line by line translation of the IK code to FORTH. Any untranslated lines are left commented out:
tl;dr - How on Earth will I do this sort of mathematics in FORTH (any flavor) especially on the Propeller?
Here's the scara arm code in Java:
package ik;
public class ScaraArm
{
// Angular values for common angles
static final double STEP_ANGLE = Math.PI / 360;
static final double RIGHT_ANGLE = Math.PI / 2;
static final double STRAIGHT_ANGLE = Math.PI;
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;
private Joint[] joints = new Joint[COUNT];
/**
* 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;
}
/**
* Setter to inject the servo parameters for each joint.
* @param jointIdx - the enumerated name of the joint.
* @param pin - the numeric value of the pin.
* @param minWidth - the minimum pulse with for this joint.
* @param maxWidth - the maximum pulse with for this joint.
* @param center - pulse width that centers the joint
* @param widthPerRadian - the pulse width to deflect the servo one radian from center
*/
public void setJointParameters(int jointIdx, int pin, int minWidth, int maxWidth, int center, double widthPerRadian)
{
joints[jointIdx].setParameters( pin, minWidth, maxWidth, center, widthPerRadian);
}
/**
* 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( x, y );
// s_w angle to humerus, note the need to clamp this value between -1 to 1!
double q = ((_humerusSq - _ulnaSq) + s_w) / (2 * _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(( _humerusSq + _ulnaSq - s_w ) / ( 2 * _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);
}
/**
* 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 - RIGHT_ANGLE;
joints[SHOULDER].setPosition(_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 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 - STRAIGHT_ANGLE;
joints[ELBOW].setPosition(_elbowAngleRads);
}
static public void main(String args[])
{
// Arm dimensions( mm )
int HUMERUS = 94; // shoulder-to-elbow "bone" 3.7"
int ULNA = 78; // elbow-to-wrist "bone" 3.1"
ScaraArm robotArm = new ScaraArm(HUMERUS, ULNA);
robotArm.park();
}
};
Below is start of line by line translation of the IK code to FORTH. Any untranslated lines are left commented out:
\ Angular values for common angles
0 constant STEP_ANGLE \ Math.PI / 360
0 constant RIGHT_ANGLE \ Math.PI / 2
0 constant STRAIGHT_ANGLE \ Math.PI
0 constant FULL_ROTATION \ 2.0 * Math.PI
0 constant SHOULDER
1 constant ELBOW
2 constant COUNT
\ Size of robot bones in consistent units (mm recommended).
create humerus 1 cell allot ;
create ulna 1 cell allot ;
create humerus_sq 1 cell allot ;
create ulna_sq 1 cell allot ;
\ Coordinate of pen tip in Cartesian space.
create x 1 cell allot ;
create y 1 cell allot ;
\ Coordinates in arm scara polar space.
create shoulder_angle_rads 1 cells allot;
create elbow_angle_rads 1 cells allot;
\ working variables during IK calculations
create s_w 1 cell allot ;
create s_w_sqrt 1 cell allot ;
\ Used to allocate an array of cells of the desired size passed on stack
: array ( n -- addr )
create
cells \ converts count into size in bytes
here over erase \ clear the buffer and
allot \ allocate space
Does>
swap cells + ; \ run time gives address of cell at index
\ Create arrays for servo properties.
COUNT array servo_center
COUNT array servo_min
COUNT array servo_max
COUNT array servo_pin
COUNT array width_per_radian
\ servo_position: Computes the pulse width that matches the desired joint angle
\ using the constraints of known angle to pulse values. Servo center is zero,
\ counter clockwise a quarter circle is Pi/2, while clockwise is -Pi/2.
\ angle 32-bit doubleing point radian value
: servo_position ( index angle -- )
\ int pulseWidth = _center + (int)(_widthPerRadian * angle);
\ _servo.writeMicroseconds(pulseWidth)
;
\ Used to initialize arm parameters.
\ humerus - shoulder-to-elbow "bone" in mm
\ ulna - elbow-to-wrist "bone" in mm
: scara_arm \ (humerus ulna -- )
dup humerus !
dup * humerus_sq !
\ pre-calculations
dup ulna !
dup * ulna_sq ! ;
\ 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.
: park ( -- )
RIGHT_ANGLE set_ shoulder
RIGHT_ANGLE set_elbow ;
\ 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 any of the move routines to initialize arm state.
\ x - the side to side displacement.
\ y - the distance out from the base center.
: set_position ( x y -- )
\ Save the Cartesian space coordinates.
y !
x !
x @ dup *
y @ dup *
+ dup
s_w !
\ Use Pythagorean theorem to calculate shoulder to wrist distance.
\ double s_w_sqrt = Math.sqrt( s_w );
\ s_w angle to centerline
\ double a1 = Math.atan2( x, y );
\ s_w angle to humerus, note the need to clamp this value between -1 to 1!
\ double q = ((_humerusSq - _ulnaSq) + s_w) / (2 * _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(( _humerusSq + _ulnaSq - s_w ) / ( 2 * _humerus * _ulna ));
\ Set the joints
\ setElbow(elb_angle_r);
;
\ 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.
\ newY - the new forward/back displacement
: set_y ( newY -- )
x @ swap
set_position ;
\ 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.
\ newX - the new height above the table
: set_x ( newX -- )
y @
set_position ;
\ Sets the should angle member, computes the servo pulse width for that
\ angle, and sets the associated servo.
\ shoulder_angle_rads - 32-bit floating point radian value
: set_shoulder ( shoulderAngleRads -- )
\ Rotate the coordinate system by a right angle so that a straight angle is
\ a full extension of the joint.
RIGHT_ANGLE -
dup
shoulder_angle_rads !
SHOULDER swap
servo_position ;
\ SetElbow: Sets elbow angle data member, computes the servo pulse width for that
\ angle, and sets the associated servo.
\ elbowAngle - floating point radian value
: set_elbow ( elbowAngle -- )
\ Rotate the coordinate system by a right angle so that a straight angle is
\ a full extension of the joint.
STRAIGHT_ANGLE -
dup
elbow_angle_rads !
ELBOW swap
servo_position ;
\ Arm dimensions( mm )
94 \ shoulder-to-elbow "bone" 3.7"
78 \ elbow-to-wrist "bone" 3.1"
scara_arm
park
tl;dr - How on Earth will I do this sort of mathematics in FORTH (any flavor) especially on the Propeller?
Comments
\ Values for common angles in brad16 hex 002e constant STEP_ANGLE \ PI / 360 2000 constant RIGHT_ANGLE \ PI / 2 4000 constant STRAIGHT_ANGLE \ PI decimal 0 constant SHOULDER 1 constant ELBOW 2 constant COUNT \ Size of robot bones in consistent units (mm recommended). create humerus 1 cell allot create ulna 1 cell allot create humerus_sq 1 cell allot create ulna_sq 1 cell allot \ Coordinate of pen tip in Cartesian space. create x 1 cell allot create y 1 cell allot \ Coordinates in arm scara polar space. create shoulder_angle_brads 1 cells allot create elbow_angle_brads 1 cells allot \ working variables during IK calculations create s_w 1 cell allot create s_w_sqrt 1 cell allot \ Used to allocate an array of cells of the desired size passed on stack : array ( n -- addr ) create cells \ converts count into size in bytes here over erase \ clear the buffer and allot \ allocate space Does> swap cells + ; \ run time gives address of cell at index \ Create arrays for servo properties. COUNT array servo_center COUNT array servo_min COUNT array servo_max COUNT array servo_pin COUNT array width_per_radian \ Assume the existence of the following functions \ sin function using brads ( brad16 -- sin ) : sin ; \ cosin function using brads ( brad16 -- cos ) : cos ; \ arccosine function using brads ( cos -- brad16 ) : acos ; : atan2 ( tan -- brad16 ) ; \ Heater's Integer sqrt function : isqrtloop begin dup 0= if exit then swap over or swap rot rot 2dup swap <= if swap over - swap rot swap over + else rot swap over - then 1 rshift swap 2 rshift again ; \ Integer square root ( i -- s ) : isqrt 0 1 30 lshift isqrtloop drop swap drop ; \ squares the input ( i -- i*i ) : square dup * ; \ Computes the pulse width that matches the desired joint angle using \ the constraints of known angle to pulse values. Servo center is zero, \ counter clockwise a quarter circle is Pi/2, while clockwise is -Pi/2. \ angle - brad value \ index - servo index \ ( angle index -- ) : servo_position dup width_per_radian \ ( angle index width_per_radian ) swap servo_center \ ( angle width_per_radian center ) -rot \ ( center angle width_per_radian ) * + \ (pulse_width) \ _servo.writeMicroseconds(pulseWidth) ; \ Used to initialize arm parameters. \ humerus - shoulder-to-elbow "bone" in mm \ ulna - elbow-to-wrist "bone" in mm : scara_arm \ (humerus ulna -- ) dup humerus ! square humerus_sq ! \ pre-calculations dup ulna ! square ulna_sq ! ; \ Sets the should angle member, computes the servo pulse width for that \ angle, and sets the associated servo. \ shoulder_angle_brads - 32-bit floating point radian value : set_shoulder ( shoulderAngleRads -- ) \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. RIGHT_ANGLE - dup shoulder_angle_brads ! SHOULDER servo_position ; \ Sets elbow angle data member, computes the servo pulse width for that \ angle, and sets the associated servo. \ elbowAngle - floating point radian value : set_elbow ( elbowAngle -- ) \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. STRAIGHT_ANGLE - dup elbow_angle_brads ! ELBOW servo_position ; \ 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. : park ( -- ) RIGHT_ANGLE set_shoulder RIGHT_ANGLE set_elbow ; \ Clamps n between -32,768 to 32,767 to prevent inverse trig function overflow \ ( n -- clamped_value ) : clamp -32768 max 32767 min ; \ 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 any of the move routines to initialize arm state. \ x - the side to side displacement. \ y - the distance out from the base center. : set_position ( x y -- ) \ Save the Cartesian space coordinates. y ! x ! \ Use Pythagorean theorem to calculate shoulder to wrist distance. x @ square y @ square + dup s_w ! isqrt s_w_sqrt ! \ s_w angle to centerline x @ y @ atan2 \ a1 is on the stack. \ s_w angle to humerus, note the need to clamp this value to prevent overflow! \ compute (_humerus_sq - ulna_sq + s_w) / (2 * humerus * s_w_sqrt) humerus_sq @ ulna_sq @ - s_w @ + 2 humerus @ * s_w_sqrt @ * / clamp acos \ a2 is on the stack \ shoulder angle = a1 + a2 + set_shoulder \ elbow angle \ compute ( humerus_sq + ulna_sq - s_w ) / ( 2 * humerus * ulna ) humerus_sq @ ulna_sq @ + s_w @ - 2 humerus @ * ulna @ * / acos \ Set the joints set_elbow ; \ 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. \ newY - the new forward/back displacement : set_y ( newY -- ) x @ swap set_position ; \ 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. \ newX - the new height above the table : set_x ( newX -- ) y @ set_position ; \ Arm dimensions( mm ) 94 \ shoulder-to-elbow "bone" 3.7" 78 \ elbow-to-wrist "bone" 3.1" scara_arm parkWill you use fixed point / scaled interger math, or would you use software floating point? Or could you pre-crunch the numbers on a PC spread sheet, and just use a look up table? That might be less work that generically using the internal table(s).
Also, which forth do you plan to use?
Propforth has a floating point package,
-edit- PropForth5.5/CurrentRelease/Extensions / float.f
but pfth can use any existing forth floating point code that is ANSI standrad compatible.
I'm contemplating using scaled integer math as it should be faster, although I'm not adverse to using floating point if it performs well. I'm following my usual development methodology which is to use gForth on the PC to get most of the way there, then port to the Propeller. I'm not sure which FORTH I will use when I try it on the Prop. I used PropForth for my last project, so I might try pfth to see what it is like.
The reason I'm using FORTH is for the same reason I used it with the Drawbot. It allows for an interactive shell which should be handy for a drawing robot.
There are many samples in the forum. I used one from Beau (in pasm) and converted it to spin (is part of a integer navigation object I posted too). In case I can post it, but I'm sure anything around is better...
Massimo
http://forums.parallax.com/showthread.php/128884-integer-only-GPS-navigation-object
As I said atan2 is adapted from Beau's code, so the base is top notch. My adaptation, well... maybe not so much..
Massimo
IF you wnat ot try it Sal's way, section 7 of the Propforth Manual (PropForth5.5/doc/ PropForth.htm ) has some instuctions for PropForth Assembler.
\ Values for common angles in brad13 $0016 constant DEGREE_ANGLE $0400 constant EIGHTH_ANGLE $0800 constant RIGHT_ANGLE $1000 constant STRAIGHT_ANGLE $1400 constant FIVEEIGHTH_ANGLE \ arctangent table from Beau Schwabe's cordic create table_atan , $20000000 , \ atan(1) = 45 degrees iteration 0 $12E4051E , \ atan(1/2) = 26.56 degrees iteration 1 $09FB385B , \ atan(1/4) iteration 2 $051111D4 , \ atan(1/8) iteration 3 $028B0D43 , \ atan(1/16) iteration 4 $0145D7E1 , \ atan(1/32) iteration 5 $00A2F61E , \ atan(1/64) iteration 6 $00517C55 , \ atan(1/128) iteration 7 $0028BE53 , \ atan(1/256) iteration 8 $00145F2F , \ atan(1/512) iteration 9 $000A2F98 , \ atan(1/1024) iteration 10 $000517CC , \ atan(1/2048) iteration 11 $00028BE6 , \ atan(1/4096) iteration 12 Does> swap cells + ; 0 constant SHOULDER 1 constant ELBOW 2 constant COUNT \ Size of robot bones in consistent units (mm recommended). create humerus 1 cells allot create ulna 1 cells allot create humerus_sq 1 cells allot create ulna_sq 1 cells allot \ Coordinate of pen tip in Cartesian space. create x 1 cells allot create y 1 cells allot \ Coordinates in arm scara polar space. create shoulder_angle 1 cells allot create elbow_angle 1 cells allot \ working variables during IK calculations create s_w 1 cells allot create s_w_sqrt 1 cells allot \ Used to allocate an array of cells of the desired size passed on stack : array ( n -- addr ) create cells \ converts count into size in bytes here over erase \ clear the buffer and allot \ allocate space Does> swap cells + ; \ run time gives address of cell at index \ word to long sign extension function. \ n has junk in the top 16 bits, so sign extend \ ( n -- n ) : w>l dup $ffff and swap $8000 and if $ffff0000 + then ; \ Create arrays for servo properties. COUNT array servo_center COUNT array servo_min COUNT array servo_max COUNT array servo_pin COUNT array width_per_radian \ Integer trig functions. \ sin function using brads ( brad13 -- sin32 ) \ The sine table contains one quadrant, so we'll need to \ work around that when generating the index into the table. \ note use um* with the return value! : sin dup \ save a copy of input for later. dup RIGHT_ANGLE and if \ ranges from $800 to $001 negate $fff and else \ ranges from $000 to $7FF $7ff and then \ index into sin table and bit shift to 32 bits. $e000 + w@ 15 lshift \ use the sign from the preserved value. swap STRAIGHT_ANGLE and if negate then ; \ cos defined in terms of sin by coordinate rotation. : cos RIGHT_ANGLE + sin ; \ inverse cosine function using brads ( cos -- brad13 ) : acos ; create atan2_theta 1 cells allot \ 13-bit fixed point four-quadrant arctangent. Given Cartesian vector (x, y), \ finds the angle subtended by the vector and the positive x-axis. \ x a signed integer \ y a signed integer \ ( x y -- brad13 ) : atan2 0 atan2_theta ! over 0 < if negate swap \ y = -y negate swap \ x = -x STRAIGHT_ANGLE atan2_theta ! then 12 0 do dup 0 < if over i 2/ over + \ calculate y + (x ~> i) -rot \ push to bottom of stack i 2/ - \ calculate x - (y ~> i) swap \ update y atan2_theta @ \ get the approximation and i table_atan @ - \ substract the lookup value. atan2_theta ! \ assign it back. else over i 2/ over - \ calculate y - (x ~> i) -rot \ push to bottom of stack i 2/ + \ calculate x + (y ~> i) swap \ update y atan2_theta @ \ get the approximation and i table_atan @ + \ add the lookup value. atan2_theta ! \ assign it back. then loop atan2_theta @ 19 rshift ; \ Heater's Integer sqrt function : isqrtloop begin dup 0= if exit then swap over or swap rot rot 2dup swap <= if swap over - swap rot swap over + else rot swap over - then 1 rshift swap 2 rshift again ; \ Integer square root ( i -- s ) : isqrt 0 1 30 lshift isqrtloop drop swap drop ; \ squares the input ( i -- i*i ) : square dup * ; \ Computes the pulse width that matches the desired joint angle using \ the constraints of known angle to pulse values. Servo center is zero, \ counter clockwise a quarter circle is Pi/2, while clockwise is -Pi/2. \ angle - brad value \ index - servo index \ ( angle index -- ) : servo_position dup width_per_radian \ ( angle index width_per_radian ) swap servo_center \ ( angle width_per_radian center ) -rot \ ( center angle width_per_radian ) * + \ (pulse_width) \ _servo.writeMicroseconds(pulseWidth) ; \ Used to initialize arm parameters. \ humerus - shoulder-to-elbow "bone" in mm \ ulna - elbow-to-wrist "bone" in mm : scara_arm \ (humerus ulna -- ) dup humerus ! square humerus_sq ! \ pre-calculations dup ulna ! square ulna_sq ! ; \ Sets the should angle member, computes the servo pulse width for that \ angle, and sets the associated servo. \ shoulder_angle - 32-bit floating point radian value : set_shoulder ( shoulderAngleRads -- ) \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. RIGHT_ANGLE - dup shoulder_angle ! SHOULDER servo_position ; \ Sets elbow angle data member, computes the servo pulse width for that \ angle, and sets the associated servo. \ elbowAngle - floating point radian value : set_elbow ( elbowAngle -- ) \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. STRAIGHT_ANGLE - dup elbow_angle ! ELBOW servo_position ; \ 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. : park ( -- ) RIGHT_ANGLE set_shoulder RIGHT_ANGLE set_elbow ; \ Clamps n between -32,768 to 32,767 to prevent inverse trig function overflow \ ( n -- clamped_value ) : clamp -32768 max 32767 min ; \ 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 any of the move routines to initialize arm state. \ x - the side to side displacement. \ y - the distance out from the base center. : set_position ( x y -- ) \ Save the Cartesian space coordinates. y ! x ! \ Use Pythagorean theorem to calculate shoulder to wrist distance. x @ square y @ square + dup s_w ! isqrt s_w_sqrt ! \ s_w angle to centerline x @ y @ atan2 \ a1 is on the stack. \ s_w angle to humerus, note the need to clamp this value to prevent overflow! \ compute (_humerus_sq - ulna_sq + s_w) / (2 * humerus * s_w_sqrt) humerus_sq @ ulna_sq @ - s_w @ + 2 humerus @ * s_w_sqrt @ * / clamp acos \ a2 is on the stack \ shoulder angle = a1 + a2 + set_shoulder \ elbow angle \ compute ( humerus_sq + ulna_sq - s_w ) / ( 2 * humerus * ulna ) humerus_sq @ ulna_sq @ + s_w @ - 2 humerus @ * ulna @ * / clamp acos \ Set the elbow set_elbow ; \ 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. \ newY - the new forward/back displacement : set_y ( newY -- ) x @ swap set_position ; \ 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. \ newX - the new height above the table : set_x ( newX -- ) y @ set_position ; \ Arm dimensions( mm ) 94 \ shoulder-to-elbow "bone" 3.7" 78 \ elbow-to-wrist "bone" 3.1" scara_arm parkhttp://forums.parallax.com/showthread.php/132302-32-bit-CORDIC-algorithm-in-Spin
Massimo
\ Note: The goal is to write code that is as portable as possible. \ So I'll define gforth compatibility words to avoid using PropForth-isms : cells 4 * ; : -rot rot rot ; : ! L! ; : @ L@ ; 0 constant SHOULDER 1 constant ELBOW 2 constant WRIST 3 constant COUNT \ Size of robot bones in consistent units (mm recommended). variable humerus variable ulna variable humerus_sq variable ulna_sq \ Coordinate of pen tip in Cartesian space. variable x variable y \ Coordinates in arm scara polar space. variable shoulder_angle variable elbow_angle \ working variables during IK calculations variable s_w variable s_w_sqrt \ Heater's Integer sqrt function : isqrtloop begin dup 0= if exit then swap over or swap -rot 2dup swap <= if swap over - swap rot swap over + else rot swap over - then 1 rshift swap 2 rshift 0 until ; \ Integer square root ( i -- s ) : isqrt 0 1 30 lshift isqrtloop drop swap drop ; \ squares the input ( i -- i*i ) : square dup * ; hex \ Change radix in a portable manner. \ word to long sign extension function. \ n has junk in the top 16 bits, so sign extend \ ( n -- n ) : w>l dup FFFF and swap 8000 and if FFFF0000 + then ; \ Values for common angles in brad13 0016 wconstant DEGREE_ANGLE 0400 wconstant EIGHTH_ANGLE 0800 wconstant RIGHT_ANGLE 1000 wconstant STRAIGHT_ANGLE 1400 wconstant FIVEEIGHTH_ANGLE decimal \ Create a set of arrays using a workaround for the missing create does> variable table_atan 13 cells allot : atan() cells table_atan + ; hex \ Change radix in a portable manner. \ load array with arctangent table iteration from Beau Schwabe's cordic 20000000 0 atan() ! \ atan(1) = 45 degrees 12E4051E 1 atan() ! \ atan(1/2) = 26.56 degrees 09FB385B 2 atan() ! \ atan(1/4) 051111D4 3 atan() ! \ atan(1/8) 028B0D43 4 atan() ! \ atan(1/16) 0145D7E1 5 atan() ! \ atan(1/32) 00A2F61E 6 atan() ! \ atan(1/64) 00517C55 7 atan() ! \ atan(1/128) 0028BE53 8 atan() ! \ atan(1/256) 00145F2F 9 atan() ! \ atan(1/512) 000A2F98 A atan() ! \ atan(1/1024) 000517CC B atan() ! \ atan(1/2048) 00028BE6 C atan() ! \ atan(1/4096) decimal variable servo_center COUNT cells allot : servo_center() cells servo_center + ; variable servo_min COUNT cells allot : servo_min() cells servo_min + ; variable servo_min COUNT cells allot : servo_min() cells servo_min + ; variable servo_max COUNT cells allot : servo_max() cells servo_max + ; variable servo_pin COUNT cells allot : servo_pin() cells servo_pin + ; variable width_per_radian COUNT cells allot : width_per_radian() cells width_per_radian + ; hex \ Change radix in a portable manner. \ sin function using brads ( brad13 -- sin32 ) \ The sine table contains one quadrant, so we'll need to \ work around that when generating the index into the table. \ note use um* with the return value! : sin dup \ save a copy of input for later. dup RIGHT_ANGLE and if \ ranges from 800 to 001 negate FFF and else \ ranges from 000 to 7FF 7FF and then \ convert to word count and index into sin table 2* E000 + W@ \ bit shift to 32 bits for future um* F lshift \ use the sign from the preserved value. swap STRAIGHT_ANGLE and if negate then ; decimal \ cos defined in terms of sin by coordinate rotation. : cos RIGHT_ANGLE + sin ; \ stub to allow compilation : acos ; variable atan2_theta \ 13-bit fixed point four-quadrant arctangent. Given Cartesian vector (x, y), \ finds the angle subtended by the vector and the positive x-axis. \ x a signed integer \ y a signed integer \ ( x y -- brad13 ) : atan2 0 atan2_theta ! over 0 < if negate swap \ y = -y negate swap \ x = -x STRAIGHT_ANGLE atan2_theta ! then 12 0 do dup 0 < if over i 2/ over + \ calculate y + (x ~> i) -rot \ push to bottom of stack i 2/ - \ calculate x - (y ~> i) swap \ update y atan2_theta @ \ get the approximation and i atan() @ - \ substract the lookup value. atan2_theta ! \ assign it back. else over i 2/ over - \ calculate y - (x ~> i) -rot \ push to bottom of stack i 2/ + \ calculate x + (y ~> i) swap \ update y atan2_theta @ \ get the approximation and i atan() @ + \ add the lookup value. atan2_theta ! \ assign it back. then loop atan2_theta @ 19 rshift ; \ Computes the pulse width that matches the desired joint angle using \ the constraints of known angle to pulse values. Servo center is zero, \ counter clockwise a quarter circle is Pi/2, while clockwise is -Pi/2. \ angle - brad value \ index - servo index \ ( angle index -- ) : servo_position dup width_per_radian \ ( angle index width_per_radian ) swap servo_center \ ( angle width_per_radian center ) -rot \ ( center angle width_per_radian ) * + \ (pulse_width) \ _servo.writeMicroseconds(pulseWidth) ; \ Used to initialize arm parameters. \ humerus - shoulder-to-elbow "bone" in mm \ ulna - elbow-to-wrist "bone" in mm : scara_arm \ (humerus ulna -- ) dup humerus ! square humerus_sq ! \ pre-calculations dup ulna ! square ulna_sq ! ; \ Sets the should angle member, computes the servo pulse width for that \ angle, and sets the associated servo. \ shoulder_angle - 32-bit floating point radian value \ ( shoulderAngleRads -- ) : set_shoulder \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. RIGHT_ANGLE - dup shoulder_angle ! SHOULDER servo_position ; \ Sets elbow angle data member, computes the servo pulse width for that \ angle, and sets the associated servo. \ elbowAngle - floating point radian value \ ( elbowAngle -- ) : set_elbow \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. STRAIGHT_ANGLE - dup elbow_angle ! ELBOW servo_position ; \ 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. \ ( -- ) : park RIGHT_ANGLE set_shoulder RIGHT_ANGLE set_elbow ; \ Clamps n between -32,768 to 32,767 to prevent inverse trig function overflow \ ( n -- clamped_value ) : clamp -32768 max 32767 min ; \ 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 any of the move routines to initialize \ arm state! \ x - the side to side displacement. \ y - the distance out from the base center. \ ( x y -- ) : set_position \ Save the Cartesian space coordinates. y ! x ! \ Use Pythagorean theorem to calculate shoulder to wrist distance. x @ square y @ square + dup s_w ! isqrt s_w_sqrt ! \ s_w angle to centerline x @ y @ atan2 \ a1 is on the stack. \ s_w angle to humerus, note the need to prevent overflow! \ compute (_humerus_sq - ulna_sq + s_w) / (2 * humerus * s_w_sqrt) humerus_sq @ ulna_sq @ - s_w @ + 2 humerus @ * s_w_sqrt @ * / clamp acos \ a2 is on the stack \ shoulder angle = a1 + a2 + set_shoulder \ elbow angle \ compute ( humerus_sq + ulna_sq - s_w ) / ( 2 * humerus * ulna ) humerus_sq @ ulna_sq @ + s_w @ - 2 humerus @ * ulna @ * / clamp acos \ Set the elbow set_elbow ; \ 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. \ newY - the new forward/back displacement \ ( newY -- ) : set_y x @ swap set_position ; \ 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. \ newX - the new height above the table \ ( newX -- ) : set_x y @ set_position ; \ Arm dimensions( mm ) 94 \ shoulder-to-elbow "bone" 3.7" 78 \ elbow-to-wrist "bone" 3.1" scara_arm parkOccasionally I convert some other dialect of Forth into Tachyon just to see if there is something I can do to improve and yet not compromise Tachyon. So just as an exercise I did this with your code and tried to keep it the same more or less. Here it is:
[FONT=courier new][COLOR=#000000]TACHYON[/COLOR] [/FONT] [FONT=courier new][COLOR=#000000]{ Martin_H's Inverse Kinematics converted from PropForth to Tachyon[/COLOR][/FONT] [FONT=courier new] [COLOR=#000000][/COLOR][/FONT][FONT=courier new][COLOR=#000000]LOAD STATS:[/COLOR][/FONT] [FONT=courier new][COLOR=#000000]NAMES: 0647 bytes added[/COLOR][/FONT] [FONT=courier new][COLOR=#000000]CODE: 0790 bytes added[/COLOR][/FONT] [FONT=courier new] [COLOR=#000000][/COLOR][/FONT][FONT=courier new][COLOR=#000000]Execution time:[/COLOR][/FONT] [FONT=courier new][COLOR=#000000]#94 #78 NEWCNT scara_arm park .LAP 104us[/COLOR][/FONT] [FONT=courier new] [COLOR=#000000][/COLOR][/FONT][FONT=courier new][COLOR=#000000]}[/COLOR][/FONT] [FONT=courier new] [COLOR=#000000][/COLOR][/FONT][FONT=courier new][COLOR=#000000]pub cells 2* 2* ; 0 == SHOULDER 1 == ELBOW 2 == WRIST 3 == COUNT \ Size of robot bones in consistent units (mm recommended). LONG humerus,ulna,,humerus_sq,ulna_sq \ Coordinate of pen tip in Cartesian space. LONG x,y[/COLOR][/FONT] [FONT=courier new][COLOR=#000000] \ Coordinates in arm scara polar space. LONG shoulder_angle,elbow_angle \ working variables during IK calculations LONG s_w,s_w_sqrt \ Heater's Integer sqrt function pub isqrtloop BEGIN DUP 0EXIT[/COLOR][/FONT] [FONT=courier new][COLOR=#000000] SWAP OVER + OR[/COLOR][/FONT] [FONT=courier new][COLOR=#000000] SWAP - ROT 2DUP SWAP <= IF SWAP OVER - SWAP ROT SWAP OVER + ELSE ROT SWAP OVER - THEN 2/ SWAP 2/ 2/ AGAIN ; \ Integer square root ( i -- s ) pub isqrt 0 1 #30 SHL isqrtloop DROP NIP ; \ squares the input ( i -- i*i ) pub square DUP * ; \ word to long sign extension function. \ n has junk in the top 16 bits, so sign extend \ ( n -- n ) : w>l DUP $FFFF AND SWAP $8000 AND IF $FFFF0000 + THEN ; \ Values for common angles in brad13 $0016 == DEGREE_ANGLE $0400 == EIGHTH_ANGLE $0800 == RIGHT_ANGLE $1000 == STRAIGHT_ANGLE $1400 == FIVEEIGHTH_ANGLE \ Create a set of arrays using a workaround for the missing create does> TABLE table_atan[/COLOR][/FONT] [FONT=courier new][COLOR=#000000]\ load array with arctangent table iteration from Beau Schwabe's cordic $20000000 , \ atan(1) = 45 degrees $12E4051E , \ atan(1/2) = 26.56 degrees $09FB385B , \ atan(1/4) $051111D4 , \ atan(1/8) $028B0D43 , \ atan(1/16) $0145D7E1 , \ atan(1/32) $00A2F61E , \ atan(1/64) $00517C55 , \ atan(1/128) $0028BE53 , \ atan(1/256) $00145F2F , \ atan(1/512) $000A2F98 , \ atan(1/1024) $000517CC , \ atan(1/2048) $00028BE6 , \ atan(1/4096) [/COLOR][/FONT] [FONT=courier new][COLOR=#000000]pub atan() cells table_atan + ; [/COLOR][/FONT] [FONT=courier new][COLOR=#000000] LONG servo_center COUNT cells ALLOT pub servo_center() cells servo_center + ; LONG servo_min COUNT cells ALLOT pub servo_min() cells servo_min + ; LONG servo_min COUNT cells ALLOT pub servo_min() cells servo_min + ; [/COLOR][/FONT] [FONT=courier new][COLOR=#000000]LONG servo_max COUNT cells ALLOT pub servo_max() cells servo_max + ; LONG servo_pin COUNT cells ALLOT pub servo_pin() cells servo_pin + ; LONG width_per_radian COUNT cells ALLOT pub width_per_radian() cells width_per_radian + ; \ sin function using brads ( brad13 -- sin32 ) \ The sine table contains one quadrant, so we'll need to \ work around that when generating the index into the table. \ note use um* with the return value! pub sin DUP \ save a copy of input for later. DUP RIGHT_ANGLE AND IF \ ranges from 800 to 001 NEGATE $FFF AND ELSE \ ranges from 000 to 7FF $7FF AND THEN \ convert to word count and index into sin table 2* $E000 + W@ \ bit shift to 32 bits for future um* $0F SHL \ use the sign from the preserved value. SWAP STRAIGHT_ANGLE AND ?NEGATE ; \ cos defined in terms of sin by coordinate rotation. pub cos RIGHT_ANGLE + sin ; \ stub to allow compilation pub acos ; LONG atan2_theta \ 13-bit fixed point four-quadrant arctangent. Given Cartesian vector (x, y), \ finds the angle subtended by the vector and the positive x-axis. \ x a signed integer \ y a signed integer \ ( x y -- brad13 ) pub atan2 atan2_theta ~ OVER 0< IF NEGATE SWAP \ y = -y NEGATE SWAP \ x = -x STRAIGHT_ANGLE atan2_theta ! THEN #12 0 DO DUP 0< IF OVER I 2/ OVER + \ calculate y + (x ~> i) ROT ROT \ push to bottom of stack I 2/ - \ calculate x - (y ~> i) SWAP \ update y atan2_theta @ \ get the approximation and I atan() @ - \ substract the lookup value. atan2_theta ! \ assign it back. ELSE OVER I 2/ OVER - \ calculate y - (x ~> i) ROT ROT \ push to bottom of stack I 2/ + \ calculate x + (y ~> i) SWAP \ update y atan2_theta @ \ get the approximation and I atan() @ + \ add the lookup value. atan2_theta ! \ assign it back. THEN LOOP atan2_theta @ #19 SHR ; \ Computes the pulse width that matches the desired joint angle using \ the constraints of known angle to pulse values. Servo center is zero, \ counter clockwise a quarter circle is Pi/2, while clockwise is -Pi/2. \ angle - brad value \ index - servo index \ ( angle index -- ) pub servo_position DUP width_per_radian \ ( angle index width_per_radian ) SWAP servo_center \ ( angle width_per_radian center ) ROT ROT \ ( center angle width_per_radian ) * + \ (pulse_width) \ _servo.writeMicroseconds(pulseWidth) ; \ Used to initialize arm parameters. \ humerus - shoulder-to-elbow "bone" in mm \ ulna - elbow-to-wrist "bone" in mm pub scara_arm \ (humerus ulna -- ) DUP humerus ! square humerus_sq ! \ pre-calculations DUP ulna ! square ulna_sq ! ; \ Sets the should angle member, computes the servo pulse width for that \ angle, and sets the associated servo. \ shoulder_angle - 32-bit floating point radian value \ ( shoulderAngleRads -- ) pub set_shoulder \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. RIGHT_ANGLE - DUP shoulder_angle ! SHOULDER servo_position ; \ Sets elbow angle data member, computes the servo pulse width for that \ angle, and sets the associated servo. \ elbowAngle - floating point radian value \ ( elbowAngle -- ) pub set_elbow \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. STRAIGHT_ANGLE - DUP elbow_angle ! ELBOW servo_position ; \ 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. \ ( -- ) pub park RIGHT_ANGLE set_shoulder RIGHT_ANGLE set_elbow ; \ Clamps n between -32,768 to 32,767 to prevent inverse trig function overflow \ ( n -- clamped_value ) pub clamp -#32768 MAX #32767 MIN ; \ 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 any of the move routines to initialize \ arm state! \ x - the side to side displacement. \ y - the distance out from the base center. \ ( x y -- ) pub set_position \ Save the Cartesian space coordinates. y ! x ! \ Use Pythagorean theorem to calculate shoulder to wrist distance. x @ square y @ square + DUP s_w ! isqrt s_w_sqrt ! \ s_w angle to centerline x @ y @ atan2 \ a1 is on the stack. \ s_w angle to humerus, note the need to prevent overflow! \ compute (_humerus_sq - ulna_sq + s_w) / (2 * humerus * s_w_sqrt) humerus_sq @ ulna_sq @ - s_w @ + 2 humerus @ * s_w_sqrt @ * / clamp acos \ a2 is on the stack \ shoulder angle = a1 + a2 + set_shoulder \ elbow angle \ compute ( humerus_sq + ulna_sq - s_w ) / ( 2 * humerus * ulna ) humerus_sq @ ulna_sq @ + s_w @ - 2 humerus @ * ulna @ * / clamp acos \ Set the elbow set_elbow ; \ 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. \ newY - the new forward/back displacement \ ( newY -- ) pub set_y x @ SWAP set_position ; \ 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. \ newX - the new height above the table \ ( newX -- ) pub set_x y @ set_position ; DECIMAL \ Arm dimensions( mm ) 94 \ shoulder-to-elbow "bone" 3.7" 78 \ elbow-to-wrist "bone" 3.1" scara_arm park[/COLOR][/FONT] [FONT=courier new] [COLOR=#000000][/COLOR][/FONT][FONT=courier new][COLOR=#000000]END[/COLOR][/FONT] [FONT=courier new] [COLOR=#000000][/COLOR][/FONT]No worries Martin, I didn't know that Tachyon was so intimidating, that is, if someone is not intimidated by the Propeller chip itself then why should a Propeller specific dialect of Forth be intimidating? However I think that even as a noob you are doing well and certainly I see that you do not hold back by procrastinating and asking continual questions while waiting for someone to solve it for you. In fact I like that you give it a go then ask the question, well done.
I can't say if this Forth code is a good translation of the Java but this Tachyon version is just the equivalent of the PropForth version.I may take a look at the Java when I get some time and offer any suggestions for improvements that I find. Is there some area that still needs work?
I found Tachyon a bit intimidating because of its unchecked stack. I'm always running into trouble with stack underflows, so the interpreter catching that helps. Plus any deviations from ANS Forth tend to throw me off. Any comments on Forth style would be welcome, as I tend to do brute force translations which don't always produce the best Forth.
Right now I'm working on debugging the fixed point atan2 by comparing its output to the Spin version and common sense. It produces correct output in most cases, but is maddeningly off in a few. It's driving me bonkers.
10 10 atan2 22 / . \ correct
-10 10 atan2 22 / . \ correct
-10 -10 atan2 22 / . \ correct
0 -10 atan2 22 / . \ correct
10 -10 atan2 22 / . \ correct
10 0 atan2 22 / . \ wrong ~360 expected
0 10 atan2 22 / . \ wrong ~90 expected
-10 0 atan2 22 / . \ wrong ~180 expected
The current code is a bit different from your Tachyon port. I caught a couple of bugs. I had to create a new d2/ function (arithmetic shift right) and create an MIN_INT constant which is the smallest base 32 negative number as I misunderstood the Spin Negx constant at first. Here's the current updated code:
\ Note: The goal is to write code that is as portable as possible. \ So I'll define gforth compatibility words to avoid using PropForth-isms : cells 4 * ; : -rot rot rot ; : ! L! ; : @ L@ ; \ arithmetic shift right n i bits \ ( n i -- n ) : d2/ \ while i > 0 shift right. 0 do 2/ loop ; 0 constant SHOULDER 1 constant ELBOW 2 constant WRIST 3 constant COUNT \ Size of robot bones in consistent units (mm recommended). variable humerus variable ulna variable humerus_sq variable ulna_sq \ Coordinate of pen tip in Cartesian space. variable x variable y \ Coordinates in arm scara polar space. variable shoulder_angle variable elbow_angle \ working variables during IK calculations variable s_w variable s_w_sqrt \ Heater's Integer sqrt function : isqrtloop begin dup 0= if exit then swap over or swap -rot 2dup swap <= if swap over - swap rot swap over + else rot swap over - then 1 rshift swap 2 rshift 0 until ; \ Integer square root ( i -- s ) : isqrt 0 1 30 lshift isqrtloop drop swap drop ; \ squares the input ( i -- i*i ) : square dup * ; hex \ Change radix in a portable manner. \ word to long sign extension function. \ n has junk in the top 16 bits, so sign extend \ ( n -- n ) : w>l dup FFFF and swap 8000 and if FFFF0000 + then ; \ Values for common angles in brad13 0016 wconstant DEGREE_ANGLE 0400 wconstant EIGHTH_ANGLE 0800 wconstant RIGHT_ANGLE 1000 wconstant STRAIGHT_ANGLE 1400 wconstant FIVEEIGHTH_ANGLE decimal \ Create a set of arrays using a workaround for the missing create does> variable table_atan 17 cells allot : atan() cells table_atan + ; hex \ Change radix in a portable manner. 80000000 constant MIN_INT \ load array with arctangent table iteration from Beau Schwabe's cordic 20000000 0 atan() ! \ atan(1) = 45 degrees 12E4051E 1 atan() ! \ atan(1/2) = 26.56 degrees 09FB385B 2 atan() ! \ atan(1/4) 051111D4 3 atan() ! \ atan(1/8) 028B0D43 4 atan() ! \ atan(1/16) 0145D7E1 5 atan() ! \ atan(1/32) 00A2F61E 6 atan() ! \ atan(1/64) 00517C55 7 atan() ! \ atan(1/128) 0028BE53 8 atan() ! \ atan(1/256) 00145F2F 9 atan() ! \ atan(1/512) 000A2F98 A atan() ! \ atan(1/1024) 000517CC B atan() ! \ atan(1/2048) 00028BE6 C atan() ! \ atan(1/4096) 000145F3 D atan() ! \ atan(1/8192) 0000A2FA E atan() ! \ atan(1/16384) 0000517D F atan() ! \ atan(1/32768) 000028BE 10 atan() ! \ atan(1/65536) decimal variable servo_center COUNT cells allot : servo_center() cells servo_center + ; variable servo_min COUNT cells allot : servo_min() cells servo_min + ; variable servo_min COUNT cells allot : servo_min() cells servo_min + ; variable servo_max COUNT cells allot : servo_max() cells servo_max + ; variable servo_pin COUNT cells allot : servo_pin() cells servo_pin + ; variable width_per_radian COUNT cells allot : width_per_radian() cells width_per_radian + ; hex \ Change radix in a portable manner. \ sin function using brads ( brad13 -- sin32 ) \ The sine table contains one quadrant, so we'll need to \ work around that when generating the index into the table. \ note use um* with the return value! : sin dup \ save a copy of input for later. dup RIGHT_ANGLE and if \ ranges from 800 to 001 negate FFF and else \ ranges from 000 to 7FF 7FF and then \ convert to word count and index into sin table 2* E000 + W@ \ bit shift to 32 bits for future um* F lshift \ use the sign from the preserved value. swap STRAIGHT_ANGLE and if negate then ; decimal \ cos defined in terms of sin by coordinate rotation. : cos RIGHT_ANGLE + sin ; \ Create a set of arrays using a workaround for the missing create does> variable table_asin 13 cells allot : asin() cells table_asin + ; \ load array with place shifted angles for asin binary search 4096 0 asin() ! \ 45 degrees 2048 1 asin() ! \ 22.5 degrees 1024 2 asin() ! \ 11.25 degrees 512 3 asin() ! \ 5.62 degrees 256 4 asin() ! \ 2.81 degrees 128 5 asin() ! \ 1.40 degrees 64 6 asin() ! \ 0.70 degrees 32 7 asin() ! \ 0.35 degrees 16 8 asin() ! \ 0.17 degrees 8 9 asin() ! \ 0.08 degrees 4 10 asin() ! \ 0.04 degrees 2 11 asin() ! \ 0.02 degrees 1 12 asin() ! \ 0.01 degrees \ compute asin via a binary search \ ( sin - brad13 ) : asin \ push the first approximation 0 \ refine approximation iteratively 13 0 do \ compare the sine of the approximation to the value. 2dup 4/ sin > if \ The approximation is too small, so increase it i asin() @ + else \ The approximation is too large, so decrease it. i asin() @ - then loop \ return only the approximation. swap drop 4/ ; \ acos is defined in terms of asin : acos asin RIGHT_ANGLE - negate ; \ holds the atan approximation. variable atan2_theta \ init the approximation based on which side of the X axis. \ ( x y -- x y ) : atan2_init over 0< if negate swap \ y = -y negate swap \ x = -x MIN_INT atan2_theta ! else 0 atan2_theta ! then ; \ 13-bit fixed point four-quadrant arctangent. Given Cartesian vector (x, y), \ finds the angle subtended by the vector and the positive x-axis. \ x a signed integer \ y a signed integer \ ( x y -- brad13 ) : atan2 atan2_init 17 0 do dup 0< if over i d2/ over + \ calculate y + (x ~> i) -rot \ push to bottom of stack i d2/ - \ calculate x - (y ~> i) swap \ update y atan2_theta @ \ get the approximation and i atan() @ - \ substract the lookup value. atan2_theta ! \ assign it back. else over i d2/ over - \ calculate y - (x ~> i) -rot \ push to bottom of stack i d2/ + \ calculate x + (y ~> i) swap \ update y atan2_theta @ \ get the approximation and i atan() @ + \ add the lookup value. atan2_theta ! \ assign it back. then loop drop drop \ x and y are no longer needed atan2_theta @ 19 rshift ; 10 0 atan2 22 / . \ wrong ~360 expected 0 10 atan2 22 / . \ wrong ~90 expected -10 0 atan2 22 / . \ wrong ~180 expected 10 10 atan2 22 / . \ correct -10 10 atan2 22 / . \ correct -10 -10 atan2 22 / . \ correct 0 -10 atan2 22 / . \ correct 10 -10 atan2 22 / . \ correct \ Computes the pulse width that matches the desired joint angle using \ the constraints of known angle to pulse values. Servo center is zero, \ counter clockwise a quarter circle is Pi/2, while clockwise is -Pi/2. \ angle - brad value \ index - servo index \ ( angle index -- ) : servo_position dup width_per_radian \ ( angle index width_per_radian ) swap servo_center \ ( angle width_per_radian center ) -rot \ ( center angle width_per_radian ) * + \ (pulse_width) \ _servo.writeMicroseconds(pulseWidth) ; \ Used to initialize arm parameters. \ humerus - shoulder-to-elbow "bone" in mm \ ulna - elbow-to-wrist "bone" in mm : scara_arm \ (humerus ulna -- ) dup humerus ! square humerus_sq ! \ pre-calculations dup ulna ! square ulna_sq ! ; \ Sets the should angle member, computes the servo pulse width for that \ angle, and sets the associated servo. \ shoulder_angle - 32-bit floating point radian value \ ( shoulderAngleRads -- ) : set_shoulder \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. RIGHT_ANGLE - dup shoulder_angle ! SHOULDER servo_position ; \ Sets elbow angle data member, computes the servo pulse width for that \ angle, and sets the associated servo. \ elbowAngle - floating point radian value \ ( elbowAngle -- ) : set_elbow \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. STRAIGHT_ANGLE - dup elbow_angle ! ELBOW servo_position ; \ 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. \ ( -- ) : park RIGHT_ANGLE set_shoulder RIGHT_ANGLE set_elbow ; \ Clamps n between -32,768 to 32,767 to prevent inverse trig function overflow \ ( n -- clamped_value ) : clamp -32768 max 32767 min ; \ 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 any of the move routines to initialize \ arm state! \ x - the side to side displacement. \ y - the distance out from the base center. \ ( x y -- ) : set_position \ Save the Cartesian space coordinates. y ! x ! \ Use Pythagorean theorem to calculate shoulder to wrist distance. x @ square y @ square + dup s_w ! isqrt s_w_sqrt ! \ s_w angle to centerline x @ y @ atan2 \ a1 is on the stack. \ s_w angle to humerus, note the need to prevent overflow! \ compute (_humerus_sq - ulna_sq + s_w) / (2 * humerus * s_w_sqrt) humerus_sq @ ulna_sq @ - s_w @ + 2 humerus @ * s_w_sqrt @ * / clamp acos \ a2 is on the stack \ shoulder angle = a1 + a2 + set_shoulder \ elbow angle \ compute ( humerus_sq + ulna_sq - s_w ) / ( 2 * humerus * ulna ) humerus_sq @ ulna_sq @ + s_w @ - 2 humerus @ * ulna @ * / clamp acos \ Set the elbow set_elbow ; \ 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. \ newY - the new forward/back displacement \ ( newY -- ) : set_y x @ swap set_position ; \ 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. \ newX - the new height above the table \ ( newX -- ) : set_x y @ set_position ; \ Arm dimensions( mm ) 94 \ shoulder-to-elbow "bone" 3.7" 78 \ elbow-to-wrist "bone" 3.1" scara_arm parkUPDATE: I added a working asin and acos functions.
Here's the latest:
\ Note: The goal is to write code that is as portable as possible. \ So I'll define gforth compatibility words to avoid using PropForth-isms : cells 4 * ; : -rot rot rot ; : ! L! ; : @ L@ ; \ arithmetic shift right n i bits \ ( n i -- n ) : asr/ dup 0> if \ while i > 0 shift right. 0 do 2/ loop else drop then ; hex \ Change radix in a portable manner. \ begin of fixed point trig functions. \ Values for common angles in brad13 0016 constant DEGREE_ANGLE 0400 constant EIGHTH_ANGLE 0800 constant RIGHT_ANGLE 1000 constant STRAIGHT_ANGLE \ sin function using brads ( brad13 -- sin32 ) \ The sine table contains one quadrant, so we'll need to \ work around that when generating the index into the table. \ note use um* with the return value! : sin dup \ save a copy of input for later. dup RIGHT_ANGLE and if \ ranges from 800 to 001 negate FFF and else \ ranges from 000 to 7FF 7FF and then \ convert to word count and index into sin table 2* E000 + W@ \ bit shift to 32 bits for future um* F lshift \ use the sign from the preserved value. swap STRAIGHT_ANGLE and if negate then ; decimal \ cos defined in terms of sin by coordinate rotation. : cos RIGHT_ANGLE + sin ; \ tangent can be derived from sine / cos. Both sine and cos are \ fast table lookups, so this is only a division more expensive \ than sine. \ ( brad13 -- tan ) : tan dup sin swap cos \ Note that we shift the cos down because we only want the \ numerator shifted left! 15 asr/ / ; \ compute asin via a binary search. \ Note: we're shifting angles up two bits to allow for greater \ precision during the search. It will be shifted back before return. \ ( sin - brad13 ) : asin \ push the first approximation and correction 0 EIGHTH_ANGLE 4* \ refine approximation and correction iteratively 13 0 do \ move the correction to the bottom of stack. -rot \ compare the sine of the approximation to the value. 2dup 2/ 2/ sin > if \ The approximation is too small, add the correction. rot dup -rot + else \ The approximation is too large, so decrease it. rot dup -rot - then \ half the correction factor for next iterration. swap 2/ loop \ return only the approximation. drop swap drop 2/ 2/ ; \ acos is defined in terms of asin \ ( cos -- brad13 ) : acos asin RIGHT_ANGLE - negate ; \ ( y x -- brad13 ) : atan2 \ save x to use its sign to adjust results dup -rot \ form a fixed point y/x ratio from the inputs swap 15 lshift swap / \ push the first approximation and correction 0 EIGHTH_ANGLE 4* \ refine approximation and correction iteratively 13 0 do \ move the correction to the bottom of stack. -rot \ compare the sine of the approximation to the value. 2dup 2/ 2/ tan > if \ The approximation is too small, add the correction. rot dup -rot + else \ The approximation is too large, so decrease it. rot dup -rot - then \ half the correction factor for next iterration. swap 2/ loop \ return only the approximation. drop swap drop 2/ 2/ \ if x's sign was negative, then move results into 3 & 4 quadrants. swap 0< if STRAIGHT_ANGLE + then ; \ end of fixed point trig functions. \ begin of utility functions. \ Clamps n between max and min to prevent inverse trig function overflow \ ( n -- clamped_value ) : clamp RIGHT_ANGLE negate sin max RIGHT_ANGLE sin min ; \ Heater's Integer sqrt function : isqrtloop begin dup 0= if exit then swap over or swap -rot 2dup swap <= if swap over - swap rot swap over + else rot swap over - then 1 rshift swap 2 rshift 0 until ; \ Integer square root ( i -- s ) : isqrt 0 1 30 lshift isqrtloop drop swap drop ; \ squares the input ( i -- i*i ) : square dup * ; \ end utility functions \ begin inverse kinematics code 0 constant SHOULDER 1 constant ELBOW 2 constant WRIST 3 constant COUNT \ Size of robot bones in consistent units (mm recommended). variable humerus variable ulna variable humerus_sq variable ulna_sq \ Coordinate of pen tip in Cartesian space. variable x variable y \ Coordinates in arm scara polar space. variable shoulder_angle variable elbow_angle \ working variables during IK calculations variable s_w variable s_w_sqrt \ arrays to hold servo properties variable servo_center COUNT cells allot : servo_center() cells servo_center + ; variable servo_min COUNT cells allot : servo_min() cells servo_min + ; variable servo_min COUNT cells allot : servo_min() cells servo_min + ; variable servo_max COUNT cells allot : servo_max() cells servo_max + ; variable servo_pin COUNT cells allot : servo_pin() cells servo_pin + ; variable width_per_radian COUNT cells allot : width_per_radian() cells width_per_radian + ; \ Computes the pulse width that matches the desired joint angle using \ the constraints of known angle to pulse values. Servo center is zero, \ counter clockwise a quarter circle is Pi/2, while clockwise is -Pi/2. \ angle - brad value \ index - servo index \ ( angle index -- ) : servo_position dup width_per_radian \ ( angle index width_per_radian ) swap servo_center \ ( angle width_per_radian center ) -rot \ ( center angle width_per_radian ) * + \ (pulse_width) \ _servo.writeMicroseconds(pulseWidth) ; \ Used to initialize arm parameters. \ humerus - shoulder-to-elbow "bone" in mm \ ulna - elbow-to-wrist "bone" in mm : scara_arm \ (humerus ulna -- ) dup humerus ! square humerus_sq ! \ pre-calculations dup ulna ! square ulna_sq ! ; \ Sets the should angle member, computes the servo pulse width for that \ angle, and sets the associated servo. \ shoulder_angle - 32-bit floating point radian value \ ( shoulderAngleRads -- ) : set_shoulder \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. RIGHT_ANGLE - dup shoulder_angle ! SHOULDER servo_position ; \ Sets elbow angle data member, computes the servo pulse width for that \ angle, and sets the associated servo. \ elbowAngle - floating point radian value \ ( elbowAngle -- ) : set_elbow \ Rotate the coordinate system by a right angle so that a straight angle \ is a full extension of the joint. STRAIGHT_ANGLE - dup elbow_angle ! ELBOW servo_position ; \ 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. \ ( -- ) : park RIGHT_ANGLE set_shoulder RIGHT_ANGLE set_elbow ; \ 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 any of the move routines to initialize \ arm state! \ x - the side to side displacement. \ y - the distance out from the base center. \ ( x y -- ) : set_position \ Save the Cartesian space coordinates. y ! x ! \ Use Pythagorean theorem to calculate shoulder to wrist distance. x @ square y @ square + dup s_w ! isqrt s_w_sqrt ! \ s_w angle to centerline x @ y @ atan2 \ a1 is on the stack. \ s_w angle to humerus, note the need to prevent overflow! \ compute (_humerus_sq - ulna_sq + s_w) / (2 * humerus * s_w_sqrt) humerus_sq @ ulna_sq @ - s_w @ + 2 humerus @ * s_w_sqrt @ * / clamp acos \ a2 is on the stack \ shoulder angle = a1 + a2 + set_shoulder \ elbow angle \ compute ( humerus_sq + ulna_sq - s_w ) / ( 2 * humerus * ulna ) humerus_sq @ ulna_sq @ + s_w @ - 2 humerus @ * ulna @ * / clamp acos \ Set the elbow set_elbow ; \ 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. \ newY - the new forward/back displacement \ ( newY -- ) : set_y x @ swap set_position ; \ 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. \ newX - the new height above the table \ ( newX -- ) : set_x y @ set_position ; \ Arm dimensions( mm ) 94 \ shoulder-to-elbow "bone" 3.7" 78 \ elbow-to-wrist "bone" 3.1" scara_arm parkNow I can finally get to the meat of the problem which is IK in FORTH.
you test values look very small, there could be an issue with that. Could you check scaling up?
During cordic you shift right at each iteration. If you start with a 5 bit number there could be a problem here.
Your atan approach looks great.. have to find time to play with it!
Thanks,
Massimo
Ah, that makes sense and explains why increasing the iteration count didn't improve accuracy. For IK distances are usually in mm, and values between 35 to 170 are typical for the kinds of robots I build. I retested with input values of 70 and resolution improved, but the binary search technique is still better for numbers that size. Thanks for suggesting CORDIC as I learned a bunch along the way, without that knowledge I wouldn't have been able to pull off the binary search technique.
Video please?
Solved in that I now have all the mathematics working and producing good looking numbers. I still have arm calibration and the end effector to build. But look for a video when I write up of my scara arm project. My plan is to use FORTH to create a command language so you can send commands to the arm from a PC.
If it goes anything like my Drawbot project the hardest part is yet to come, but please don't burst my denial.
Here's the updated Java code:
int originX = 320; int originY = 200; public class ScaraArm { // Angular values for common angles static final double STEP_ANGLE = Math.PI / 360; static final double RIGHT_ANGLE = Math.PI / 2; static final double STRAIGHT_ANGLE = Math.PI; 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); } /** * 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 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 = 132; int ULNA = 142; ScaraArm robotArm = new ScaraArm(HUMERUS, ULNA); PFont f; void setup() { size(2 * originX, 2 * originY); f = createFont("Arial", 16, true); robotArm.park(); } 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.setPosition(localX, localY); robotArm.drawArm(); }