Base

Shoulder

Elbow

Wrist

Target

Gripper

bs

seew

gr

sw

Target at xt/yt/ztMove base to atan(xt/yt) => new yt'=(x2+y2), xt' = 0Problem 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 gripperCompute 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/ewCompute shoulder angle from sw/gnd and sw/seDone in world coordinates!Needs transformation to system (robot) coordinates and Motor interface (e.g. servos pulse lengths)

sw/gnd

ga

zt

Xt,yt => yt'

Cosine law

Typical world coordinates are:Pos X-axis rightPos Y-axis backPos Z-axis up

A. Kugelhttp://rg-asc.ziti.uni-heidelberg.de/esr

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; // effective gripper/wrist length is wg + wr + some fraction of gr to hold target tight float grp = wg + wr + .75*gr; float se2 = se*se; // squared values 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) zw = zt - grp*sin(gar); if (0.0 > zw) return false; // shouldn't be negative yw = yt - grp*cos(gar); if (0.0 > yw) return false; // compute sw, distance shoulder (at y=0) and wrist. compensate base height if not subtracted already sw2 = (zw-bs)*(zw-bs) + yw*yw; // squared sw = sqrt(sw2); // angle of sw relative to gnd sw_gnd = atan2((zw-bs), yw); // angle sw to se via cosine law see drawing sw_se = acos((sw2 + se2 - ew2) / (2*se*sw)); // angle se to ew via cosine law 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 sa = degrees(sw_gnd + sw_se); // elbow angle , converted ea = degrees(se_ew); // wrist angle: target grip angle minus elbow minis shoulder // initially, wrist is pointing to the direction given by shoulder and ellbow wa = sa - (180 - ea); // offset to point the gripper to the target direction wa = grip_angle_d - wa; // this is the end of world coordinate computation ... // convert to robot =>

// final step is to transform into robot coordinates // the following transformations depend on the actual robot // my system .... // all joints (except base) rotate along X-axis // 0 is upright (in Z) orientation // increasing angles rotate towards front (follow "right hand rule", positive X to right) sa = 90 - sa; // 0 arm = 90 world, 0 world = 90 arm ea = 180 - ea; // residual angle from triangle cosine law calculation wa = -wa; // negated if (0.0 < ba) ba = 180.0 - ba; else ba = -(180.0 + ba); (!(isnan(ba) || isnan(sa) || isnan(ea) || isnan(wa))) {

if (!jointSetAngle( 0, ba)) return false ;if (!jointSetAngle( 1, sa)) return false ;if (!jointSetAngle( 2, ea)) return false ;if (!jointSetAngle( 3, wa)) return false ;

} else { return false;

} return true; }A. Kugel

http://rg-asc.ziti.uni-heidelberg.de/esr

Code derived from https://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino

This code assumes a given grip orientation. Of course, a fixed angle will not work for all target positions. You will need to re-compute with a different angle if the value are out of range. Steps of ~30 seem reasonable from my testing.

Folie 1Folie 2