 
{ "r0in4" computes the orientation of frame 0 rel frame 4 with theta4 = 0 }
 
function nj4_175_22_r0in4(c1,c2,c3,s1,s2,s3:real):rotmat;
begin
   nj4_175_22_r0in4:= [
   c1*c2*c3 + c1*s2*s3,
   s1*c2*c3 + s1*s2*s3,
   s2*c3 - c2*s3,
    + s1,
    - c1,
   0.0,
   -c1*c2*s3 + c1*s2*c3,
   -s1*c2*s3 + s1*s2*c3,
   -s2*s3 - c2*c3] as rotmat;
end;
 
nj4_175_22_wrist_limits == [-0.5000,1.0000] as rbnds;
nj4_175_22_cone_angle_eps == 1.0e-4;
nj4_175_22_use_adjusted_goal == false;
 
function nj4_175_22_near_limit(z : real) : real;
 
begin
    if (abs(z-nj4_175_22_wrist_limits.lb)
       <= abs(z-nj4_175_22_wrist_limits.ub)) then
    begin
        nj4_175_22_near_limit := nj4_175_22_wrist_limits.lb;
    end else
    begin
        nj4_175_22_near_limit := nj4_175_22_wrist_limits.ub;
    end;
end;
 
function nj4_175_22_adjusted_goal(goal : rotmat) : rotmat;
 
var cp,sp,vp,k1,k2,mag_k,z_near : real;
    phi,psi : rangle;
    rot_k_psi : rotmat;
 
begin
    phi := atan2(sqrt(goal.r02**2 + goal.r12**2),abs(goal.r22));
    if azero(phi as_type real, nj4_175_22_cone_angle_eps) then
    begin
        k1 := 1.0;
        k2 := 0.0;
    end else
    begin
        if (goal.r22 > 1.0) then goal.r22 := 1.0;
        if (goal.r22 < -1.0) then goal.r22 := -1.0;
        mag_k := sqrt(1.0 - goal.r22**2);
        k1 := -goal.r12/mag_k;
        k2 := goal.r02/mag_k;
    end;
    z_near := nj4_175_22_near_limit(goal.r22);
    psi := atan(sqrt(1.0-z_near**2)/z_near) - phi
           + (nj4_175_22_cone_angle_eps as_type rangle);
    cp := cos(psi);
    sp := sin(psi);
    vp := 1.0 - cp;
    rot_k_psi := [k1*k1*vp + cp, k1*k2*vp,       k2*sp,
                  k1*k2*vp,      k2*k2*vp + cp, -k1*sp,
                    -k2*sp,      k1*sp,             cp] as rotmat;
    nj4_175_22_adjusted_goal := rot_k_psi*goal;
end;
 
import("inf,real);
 
function nj4_175_22_solve456_no(r6in0: rotmat; nearj: jvr; sol: integer): boolean;
var r: rotmat;
    th4,th6: rangle;
    c4,s4,c5,s5 : real;
    th5 : quad_kin_sol;
    theta5 : darray of rangle;
    other: integer;
begin
  r:= all_r0in4[sol] * r6in0;   { this could be more efficient }
 
  dset_all_sols(sol+all_sols.nsols,all_sols.solutions[sol-1]);
  if (not(in_bnds(r.r22,nj4_175_22_wrist_limits))
      and nj4_175_22_use_adjusted_goal) then
  begin
    r := nj4_175_22_adjusted_goal(r);
  end;
  th5 := quad_kin(-r.r22 + 1.000000000000000e+000,0.0,-r.r22 -5.000000000000312e-001);
  if exists(th5) then
  begin
    theta5 := array_create(rangle,1);
    theta5[0] := sol1(th5);
    theta5[1] := sol2(th5);
    for i := 0 to 1 do
    begin
      c5 := cos(theta5[i]);
      s5 := sin(theta5[i]);
      if (azero(s5) and azero(-4.330127018922081e-001*c5 -4.330127018922125e-001)) then
      begin
        th4 := nearj.j4 as_type rangle;
        c4 := cos(th4);
        s4 := sin(th4);
        th6 := new_batan2(-sgn(c5)*(r.r01*c4 + r.r11*s4),
                           sgn(c5)*(r.r00*c4 + r.r10*s4));
      end else
      begin
        th4 := new_batan2(8.660254037844465e-001*r.r12*s5 + -4.330127018922081e-001*r.r02*c5 -4.330127018922125e-001*r.r02,
                          8.660254037844465e-001*r.r02*s5 + 4.330127018922081e-001*r.r12*c5 + 4.330127018922125e-001*r.r12);
        th6 := new_batan2(-4.330127018922125e-001*r.r20*c5 + -4.330127018922081e-001*r.r20 -8.660254037844487e-001*r.r21*s5,
                          -4.330127018922125e-001*r.r21*c5 + -4.330127018922081e-001*r.r21 + 8.660254037844487e-001*r.r20*s5);
      end;
 
      sol := sol + i*all_sols.nsols;
      all_sols.solutions[sol-1].valid := true;
      dset_all_sol_jvr(sol,4,th4 as_type real);
      dset_all_sol_jvr(sol,5,theta5[i] as_type real);
      dset_all_sol_jvr(sol,6,th6 as_type real);
    end;
  end else
  begin
    all_sols.solutions[sol-1].valid := false;
    dset_all_sol_jvr(sol,4,nearj.j4);
    dset_all_sol_jvr(sol,5,inf);
    dset_all_sol_jvr(sol,6,nearj.j6);
    sol := sol + all_sols.nsols;
    all_sols.solutions[sol-1].valid := false;
    dset_all_sol_jvr(sol,4,nearj.j4);
    dset_all_sol_jvr(sol,5,inf);
    dset_all_sol_jvr(sol,6,nearj.j6);
  end;
  nj4_175_22_solve456_no := exists(th5);
end;
 
 
{ This function solves for the joint values of joints  }
{ 2 and 3.                                             }
{ Function arguments:                                  }
{      P4 - position of LINK4 origin in LINK0 frame    }
{      TH1 - current solution for value of joint 1     }
{      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 nj4_175_22_solve23(p4 : point;
                           th1 : rangle;
                           nj2,nj3 : real;
                           ind : integer) : integer;
 
var r,a,b,c,d,c1,s1,c3,s3 : real;
    th2 : rangle;
    th3 : quad_kin_sol;
 
begin
  c1 := cos(th1);   s1 := sin(th1);
  r := -((p4.xc -3.500000000000000e+001*c1)**2) - ((p4.yc -3.500000000000000e+001*s1)**2) -p4.zc*p4.zc + 1.453408999999986e+004;
  th3 := quad_kin(r,32910.0000, r + 7.500000000000008e+003);
  if th3.exists then
  begin
    c := c1*p4.xc + s1*p4.yc -3.500000000000000e+001;
    d := p4.zc;
    s3 := sin(th3.sol1); c3 := cos(th3.sol1);
    a := 2.500000000000003e+001*c3 + 1.096999999999993e+002*s3 + 7.500000000000000e+001;
    b := -2.500000000000003e+001*s3 + 1.096999999999993e+002*c3;
    th2 := atan2(a*d-b*c,a*c+b*d);
    dset_all_sols(ind,[true,[th1 as_type real,
                             th2 as_type real,
                             th3.sol1 as_type real,
                             0.0,0.0,0.0] as jvr] as flagged_jvr);
    dset_all_r0in4(ind,nj4_175_22_r0in4(c1,cos(th2),c3,s1,sin(th2),s3));
    s3 := sin(th3.sol2);  c3 := cos(th3.sol2);
    a := 2.500000000000003e+001*c3 + 1.096999999999993e+002*s3 + 7.500000000000000e+001;
    b := -2.500000000000003e+001*s3 + 1.096999999999993e+002*c3;
    th2 := atan2(a*d-b*c,a*c+b*d);
    dset_all_sols(ind+1,[true,[th1 as_type real,
                               th2 as_type real,
                               th3.sol2 as_type real,
                               0.0,0.0,0.0] as jvr] as flagged_jvr);
    dset_all_r0in4(ind+1,nj4_175_22_r0in4(c1,cos(th2),c3,s1,sin(th2),s3));
    nj4_175_22_solve23 := 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);
    nj4_175_22_solve23 := 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 nj4_175_22_solve123(p4:point; nj1,nj2,nj3: real): integer;  { CASE:  RRR, ca1 = 0 }
var theta1_sols : quad_kin_sol;
    a,b,c : real;
    nsols : integer;
begin
  a := -p4.yc;
  b := -2.000000000000000e+000*p4.xc;
  c := p4.yc;
  theta1_sols := quad_kin(a,b,c);
  if exists(theta1_sols) then
  begin
    nsols := nj4_175_22_solve23(p4,sol1(theta1_sols),nj2,nj3,1);
    nj4_175_22_solve123 := nsols + nj4_175_22_solve23(p4,sol2(theta1_sols),nj2,nj3,3);
  end
  else nj4_175_22_solve123:= 0;
end;
 
{ This is the Inverse Kinematic Function for nj4_175_22}
{ 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_nj4_175_22(guy: object; wrelb: frame): boolean;
var i,nsols: integer;
    wrel0: frame;
    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 + 8.300000000000000e+001] as frame;
  nsols:= nj4_175_22_solve123(pos(wrel0), nearj.j1, nearj.j2, nearj.j3); 
  in_envelope := false;
  if nsols = 0 then 
  begin
    for i:=1 to 8 do dset_all_sols(i,[false,nearj] as flagged_jvr);
  end
  else 
  begin
    dset_nsols(all_sols,4);                          { temporarily for solve456 }
    for i:=1 to 4 do
    begin
      if valid(all_sols.solutions[i-1]) then
        in_envelope:=nj4_175_22_solve456_no(ornt(wrel0),nearj, i) or in_envelope
      else
        dset_all_sols(i+4,[false,null_jvr] as flagged_jvr);
    end;
  end;
  dset_nsols(all_sols,8);
  at_least_one_good:= put_in_varying_limits(mrobot);
  if at_least_one_good then
    at_least_one_good:= tune_all_solutions(guy,wrelb);
  dset_in_env(all_sols,in_envelope);
  dset_all_jvrs(mrobot,all_sols);
  dset_sols_computed(mrobot,true);
  invkin_nj4_175_22:= in_envelope and at_least_one_good;
end;
 
