 
{ "r0in4" computes the orientation of frame 0 rel frame 4 with theta4 = 0 }
 
function c5g_pal_470_31_r0in4(c1,c2,c3,s1,s2,s3:real):rotmat;
begin
   c5g_pal_470_31_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;
 
{ 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 c5g_pal_470_31_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 -4.600000000000000e+001*c1)**2) - ((p4.yc -4.600000000000000e+001*s1)**2) -p4.zc*p4.zc + 3.134656144000001e+003;
  th3 := quad_kin(r,-0.0000, r + 6.761496000000001e+004);
  if th3.exists then
  begin
    c := c1*p4.xc + s1*p4.yc -4.600000000000000e+001;
    d := p4.zc;
    s3 := sin(th3.sol1); c3 := cos(th3.sol1);
    a := 1.609880000000000e+002*c3 + 1.050000000000000e+002;
    b := -1.609880000000000e+002*s3;
    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,c5g_pal_470_31_r0in4(c1,cos(th2),c3,s1,sin(th2),s3));
    s3 := sin(th3.sol2);  c3 := cos(th3.sol2);
    a := 1.609880000000000e+002*c3 + 1.050000000000000e+002;
    b := -1.609880000000000e+002*s3;
    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,c5g_pal_470_31_r0in4(c1,cos(th2),c3,s1,sin(th2),s3));
    c5g_pal_470_31_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);
    c5g_pal_470_31_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 c5g_pal_470_31_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 := c5g_pal_470_31_solve23(p4,sol1(theta1_sols),nj2,nj3,1);
    c5g_pal_470_31_solve123 := nsols + c5g_pal_470_31_solve23(p4,sol2(theta1_sols),nj2,nj3,3);
  end
  else c5g_pal_470_31_solve123:= 0;
end;
 
{ This is the Inverse Kinematic Function for c5g_pal_470_31}
{ 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_c5g_pal_470_31(guy: object; wrelb: frame): boolean;
var i,nsols: integer;
    sing : boolean;
    wrel0: frame;
    mrobot : object;
    nearj : jvr;
    in_envelope,at_least_one_good : boolean;
 		rc:robot_constants;
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 + 1.140000000000000e+002] as frame;
  nsols:= c5g_pal_470_31_solve123(pos(wrel0), nearj.j1, nearj.j2, nearj.j3); 
  if nsols = 0 then 
  begin
    in_envelope:=false; 
    for i:=1 to 8 do dset_all_sols(i,[false,nearj] as flagged_jvr);
  end
  else 
  begin
    in_envelope:=true;
    dset_nsols(all_sols,4);                          { temporarily for solve456 }
    for i:=1 to 4 do
    begin
      if valid(all_sols.solutions[i-1]) then
        sing:= solve456_orth(1.0000, 1.0000, ornt(wrel0), nearj.j4 as_type rangle, i) 
      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);
  { make the njoints = 6 just so tune_all_solutions work }
  	rc := robot_constants_of(mrobot);
  	dset_njoints(rc, 6);
		if at_least_one_good then
    at_least_one_good:= tune_all_solutions(guy,wrelb);
  dset_njoints(rc, 4);
 
  dset_in_env(all_sols,in_envelope);
  dset_all_jvrs(mrobot,all_sols);
  dset_sols_computed(mrobot,true);
  invkin_c5g_pal_470_31:= in_envelope and at_least_one_good;
end;
 
