 
{ File k3.sil -  ( Inverse kinematics ) }
 
 
{ "r0in4" computes the orientation of frame 0 rel frame 4 with theta4 = 0 }
function rebel_s6_075_r0in4(c1,c2,s1,s2:real):rotmat;
begin
   rebel_s6_075_r0in4:=[
   c1*c2 - s1*s2,
   s1*c2 + c1*s2,
   0.0,
    - c1*s2 - s1*c2,
    - s1*s2 + c1*c2,
   0.0,
   0.0,
   0.0,
    + 1.000000000000000e+000 ] as rotmat;
end;
 
rebel_s6_075_epsilon == 0.001;
 
{ This function solves for the joint values of joints  }
{ 1 and 2.                                             }
{ Function arguments:                                  }
{      P4 - position of LINK4 origin in LINK0 frame    }
{      D3 - current solution for value of joint 3      }
{      NJ1 - last joint 1 value, for use in sing. case }
{      NJ2 - last joint 2 value, for use in sing. case }
{      IND - the index of this solution                }
{ Function return value:                               }
{      type: INTEGER                                   }
{      the number of solutions found                   }
function rebel_s6_075_solve12(p4:point; d3:real; nj1,nj2: real; ind:integer):integer;
var a,b,c,r:real;
    theta2_sols: quad_kin_sol;
    s1,c1,s2,c2,f2,f3,g1,g2,mag: real;
    theta1: rangle;
begin
r:=p4.xc*p4.xc+p4.yc*p4.yc+p4.zc*p4.zc;
a:=d3*d3 - r + 4.000000000000000e+002;
b:=0.0;
c:=d3*d3 - r + 5.625000000000000e+003;
theta2_sols:= quad_kin(a,b,c);
if theta2_sols.exists then
begin
s2:=sin(theta2_sols.sol1); c2:=cos(theta2_sols.sol1);
f2:=0.0;
g1:= 2.750000000000000e+001*c2 - s2*f2 + 4.750000000000000e+001;
g2:= 2.750000000000000e+001*s2 + c2*f2;
    mag:= g1*g1 + g2*g2;
    if mag<rebel_s6_075_epsilon then
    begin
      theta1:= nj1 as_type rangle;
      s1:=sin(nj1);
      c1:=cos(nj1);
    end
    else
    begin
      s1:= (g1*yc(p4)-g2*xc(p4))/mag;
      c1:= (g2*yc(p4)+g1*xc(p4))/mag;
      theta1:= new_batan2(s1,c1);
    end;
dset_all_sols(ind,[true,[theta1 as_type real,theta2_sols.sol1 as_type real,
        d3 as_type real,0.0,0.0,0.0] as jvr] as flagged_jvr);
dset_all_r0in4(ind, rebel_s6_075_r0in4(c1,c2,s1,s2));
s2:=sin(theta2_sols.sol2);  c2:=cos(theta2_sols.sol2);
g1:= 2.750000000000000e+001*c2 - s2*f2 + 4.750000000000000e+001;
g2:= 2.750000000000000e+001*s2 + c2*f2;
    mag:= g1*g1 + g2*g2;
    if mag<rebel_s6_075_epsilon then
    begin
      theta1:= nj1 as_type rangle;
      s1:=sin(nj1);
      c1:=cos(nj1);
    end
    else
    begin
      s1:= (g1*yc(p4)-g2*xc(p4))/mag;
      c1:= (g2*yc(p4)+g1*xc(p4))/mag;
      theta1:= new_batan2(s1,c1);
    end;
dset_all_sols(ind+1,[true,[theta1 as_type real,theta2_sols.sol2 as_type real,
        d3 as_type real,0.0,0.0,0.0] as jvr] as flagged_jvr);
dset_all_r0in4(ind+1, rebel_s6_075_r0in4(c1,c2,s1,s2));
rebel_s6_075_solve12:=2;
end
else
begin
dset_all_sols(ind,[false,null_jvr] as flagged_jvr);
dset_all_sols(ind+1,[false,null_jvr] as flagged_jvr);
rebel_s6_075_solve12:=0;
end;
end;
 
{ This function solves for the joint values of joints  }
{ 1, 2 and 3.                                          }
{ Function arguments:                                  }
{      P4 - position of LINK4 origin in LINK0 frame    }
{      NJ1 - last joint 1 value, for use in sing. case }
{      NJ2 - last joint 2 value, for use in sing. case }
{      NJ3 - last joint 3 value, for use in sing. case }
{      IND - the index of this solution                }
{ Function return value:                               }
{      type: INTEGER                                   }
{      the number of solutions found                   }
function rebel_s6_075_solve123(p4:point; nj1,nj2,nj3: real):integer;
var d3: real;
begin
  d3:= p4.zc;
  rebel_s6_075_solve123:= rebel_s6_075_solve12(p4,d3,nj1,nj2,1);
end;
 
{ First arg, "sgnzax" is 1.0 if you want Z to point up, or -1.0 if down }
 
rebel_s6_075_last_projected_yax == [0,1,0] as point;
rebel_s6_075_epsilon == 0.001;
 
function rebel_s6_075_project_4dof(sgnzax: real; wrelb: frame; tip: point): frame;   { returns new wrelb }
var old_tip,newx,newy,newz: point;
    leny: real;
    newr: rotmat;
begin
  newz:= [0,0,sgnzax] as point;
  newy:= newz xprod (xax(wrelb) as_type point);
  leny:= length(newy);
  if leny<rebel_s6_075_epsilon then newy:= rebel_s6_075_last_projected_yax
                       else newy:= newy / leny;
  rebel_s6_075_last_projected_yax:=newy;
  newx:= newy xprod newz;
  newr:= [ newx as_type dir3dr, newy as_type dir3dr, newz as_type dir3dr ] as rotmat;
  old_tip:= wrelb * tip;
  rebel_s6_075_project_4dof:= [old_tip - (newr*tip),newr] as frame;
end;
 
 
{ This is the Inverse Kinematic Function for rebel_s6_075 }
{ The argument GUY is the LINKN of the robot and the }
{ argument WRELB is the desired pose of the wrist    }
{ relative to the base of the robot.                 }
 
function invkin_rebel_s6_075(guy : object; wrelb : frame): boolean;
var i,nsols : integer;
    wrel0 : frame;
    tip : point;
    ee : object;
    the_xax : dir3dr;
    theta4 : rangle;
    mrobot : object;
    nearj : jvr;
    in_envelope,at_least_one_good : boolean;
 
begin
  mrobot := parent(guy);
  nearj := jvr_to_get_close_to(mrobot);
  for i := 0 to 7 do all_sols.solutions[i].valid := false;
  wrel0 := [wrelb.r00, wrelb.r01, wrelb.r02, wrelb.xtr,
     + wrelb.r10,  + wrelb.r11,  + wrelb.r12,  + wrelb.ytr,
     + wrelb.r20,  + wrelb.r21,  + wrelb.r22,  + wrelb.ztr] as frame;
  ee := end_effector_of(guy);
  if (ee = null_object) then tip := pos(flange_rel_wrist(mrobot))
                        else tip := pos(pose(ee)) in_frame pose(guy);
  wrel0 := rebel_s6_075_project_4dof(1.0000,wrel0,tip);
  nsols := rebel_s6_075_solve123(pos(wrel0), nearj.j1, nearj.j2, nearj.j3); 
  if nsols = 0 then 
  begin
    in_envelope := false; 
    for i:=1 to 4 do dset_all_sols(i,[false,nearj] as flagged_jvr);
  end
  else
  begin
    in_envelope := true;
    for i := 1 to nsols do
    begin
      the_xax := all_r0in4[i] * wrel0.xax;
      theta4 := new_batan2(the_xax.yc,the_xax.xc);
      dset_all_sol_jvr(i,4,theta4 as_type real);
    end;
  end;
  dset_nsols(all_sols,2);
  at_least_one_good:= put_in_static_limits(mrobot);
  dset_in_env(all_sols,in_envelope);
  dset_all_jvrs(mrobot,all_sols);
  dset_sols_computed(mrobot,true);
  invkin_rebel_s6_075:= in_envelope and at_least_one_good;
end;
 
