Typical world coordinates are:

Pos X-axis right

Pos Y-axis back
Pos Z-axis up

ga

Elbow
ew
Wrist

se
sw
Shoulder

sw/gnd

bs

Target at xt/yt/zt
Move base to atan(xt/yt) => new yt'=(x2+y2), xt' = 0
Problem is 2D only now
Define grip angle ga (this is a boundary condition)
Compute wrist position yw/zw from yt',zt using ga and
length of gripper
Compute shoulder-wrist distance sw from wrist position
(shoulder has y=0 !)
Compute angle of sw relative to ground
Use cosine law (c2 = a2 + b2 - 2abcos ) to compute
angles sw/se and se/ew
Compute shoulder angle from sw/gnd and sw/se
Done in world coordinates!
Needs transformation to system (robot) coordinates and
Motor interface (e.g. servos pulse lengths)
Cosine law

Gripper
gr
Target
zt

Base
Xt,yt => yt'
A. Kugel
http://rg-asc.ziti.uni-heidelberg.de/esr

Code derived from

https://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino
bool set_arm( float x, float y, float z, float grip_angle_d )
{
// we have a couple of (pseudo) constants which could be pre-computed. Adapt to your specific robot
// my system has a wrist rotor (6DOF arm) which sits between wrist and gripper. If you don't have this
// remove the wr element
float bs = jointParms[0].height; float se = jointParms[1].height; float ew = jointParms[2].height;
float wg = jointParms[3].height; float wr = jointParms[4].height; float gr = jointParms[5].height;
This code assumes a given grip orientation. Of course, a
// effective gripper/wrist length is wg + wr + some fraction of gr to hold target tight
fixed angle will not work for all target positions. You will need
float grp = wg + wr + .75*gr;
to re-compute with a different angle if the value are out of
float se2 = se*se; // squared values
range. Steps of ~30 seem reasonable from my testing.
float ew2 = ew*ew;
// and a couple of local variables
// assume positive y direction is to front. Compensate at higher level if neccesaary
float ba = degrees(atan2( x, y )); // base angle to cancel x
float yt,zt, yw,zw; // transformed positions of target and wrist after x-cancelation
float sw2,sw; // computed length shoulder to wrist and squared
float gar, sa, ea, wa, sw_gnd, sw_se, se_ew, ew_gr; // computed angles in rad, world coordinates
// compute target position after x transform
yt = sqrt(( x * x ) + ( y * y ));
zt = z; // z unchanged
// compute wrist position. assume grip_angle_d = 0 is horizontal, positive value is upwards hence wrist for pos angle will be BELOW target
gar = radians(grip_angle_d); // we subtract the base height later( or here and then also from shoulder)
// final step is to transform into robot coordinates
zw = zt - grp*sin(gar);
// the following transformations depend on the actual robot
if (0.0 > zw) return false; // shouldn't be negative
// my system ....
yw = yt - grp*cos(gar);
// all joints (except base) rotate along X-axis
if (0.0 > yw) return false;
// 0 is upright (in Z) orientation
// compute sw, distance shoulder (at y=0) and wrist. compensate base height if not subtracted already
// increasing angles rotate towards front (follow "right hand rule",
sw2 = (zw-bs)*(zw-bs) + yw*yw; // squared
positive
X to right)
sw = sqrt(sw2);
sa
=
90
- sa; // 0 arm = 90 world, 0 world = 90 arm
// angle of sw relative to gnd
ea
=
180
- ea; // residual angle from triangle cosine law
sw_gnd = atan2((zw-bs), yw);
calculation
// angle sw to se via cosine law see drawing
wa = -wa;
// negated
sw_se = acos((sw2 + se2 - ew2) / (2*se*sw));
if
(0.0
<
ba)
ba
=
180.0
- ba;
// angle se to ew via cosine law
else
ba
=
-(180.0
+
ba);
se_ew = acos((ew2 + se2 - sw2) / (2*se*ew));
// shoulder angle relative to gnd: sw_gnd + sw_se. needs conversion as 0 angle is up in servo world
(!(isnan(ba) || isnan(sa) || isnan(ea) || isnan(wa))) {
sa = degrees(sw_gnd + sw_se);
if (!jointSetAngle( 0, ba)) return false ;
// elbow angle , converted
if (!jointSetAngle( 1, sa)) return false ;
ea = degrees(se_ew);
if (!jointSetAngle( 2, ea)) return false ;
// wrist angle: target grip angle minus elbow minis shoulder
if (!jointSetAngle( 3, wa)) return false ;
// initially, wrist is pointing to the direction given by shoulder and ellbow
}
else
{
wa = sa - (180 - ea);
return false;
// offset to point the gripper to the target direction
}
wa = grip_angle_d - wa;
return true;
// this is the end of world coordinate computation ...
// convert to robot =>
}
A. Kugel
http://rg-asc.ziti.uni-heidelberg.de/esr