 
{ File k2.sil -  ( Joint limits functions ) }
 
 
{  This function computes the joint limits for joint 3 for    }
{  the robot GUY and the position J.  GUY is the robot object }
{  and J is the current joint vector in the CIM convention.   }
function joint3_limits_nj_500_27(guy : object; j : jvr) : rbnds;
 
var llim,ulim : real;
    a : av;
    jlim : jvr;
 
begin
  tune_j2j3(j);
  a := jvr_to_av_nj_500_27(guy,j);
  a.a3 := 8.999999999999972e+001;
  jlim := av_to_jvr_nj_500_27(guy,a);
  llim := max(jlim.j3,-2.460914245312006e+000);
  a.a3 := -9.000000000000027e+001;
  jlim := av_to_jvr_nj_500_27(guy,a);
  ulim := min(jlim.j3,1.396263401595464e+000);
  joint3_limits_nj_500_27 := [llim, ulim] as rbnds;
end;
 
 
{  This functions computes the set of joint limits for the robot }
{  GUY at the position J.  GUY is the robot object and J is the  }
{  current joint vector in the CIM convention.                   }
function joint_limits_nj_500_27(guy: object;j:jvr): jvr_bnds;
var jlr : joint_limited_robot;
begin
  jlr := guy to_view joint_limited_robot;
  joint_limits_nj_500_27:=
    mk_jvr_bnds( 
       jlr.joint_limits[1],
       jlr.joint_limits[2],
       reduce(joint3_limits_nj_500_27(guy,j),jlr.percent[3]),
       jlr.joint_limits[4],
       jlr.joint_limits[5],
       jlr.joint_limits[6] );
end;
 
 
{  This functions computes the set of joint limits for the robot }
{  GUY at the position J to be displayed in the monitor window.  }
{  GUY is the robot object and J is the current joint vector in  }
{  the CIM convention.                                           }
function em_joint_limits_nj_500_27(guy: object;j:jvr): jvr_bnds;
var
    lims3 : rbnds;
    cim_lims : jvr_bnds;
    julim, jllim, emulim, emllim : jvr;
begin
  cim_lims := joint_limits_nj_500_27(guy,j);
  julim := j;
  jllim := j;
  julim.j3 := ub(cim_lims[3]);
  jllim.j3 := lb(cim_lims[3]);
  emulim := cim_to_nj_500_27_jvr(guy,julim);
  emllim := cim_to_nj_500_27_jvr(guy,jllim);
  lims3 := [emllim[3],emulim[3]] as rbnds;
  julim := mk_jvr(ub(cim_lims[1]),
                  ub(cim_lims[2]),
                  ub(cim_lims[3]),
                  ub(cim_lims[4]),
                  ub(cim_lims[5]),
                  ub(cim_lims[6]));
  jllim := mk_jvr(lb(cim_lims[1]),
                  lb(cim_lims[2]),
                  lb(cim_lims[3]),
                  lb(cim_lims[4]),
                  lb(cim_lims[5]),
                  lb(cim_lims[6]));
  emulim := cim_to_nj_500_27_jvr(guy,julim);
  emllim := cim_to_nj_500_27_jvr(guy,jllim);
  em_joint_limits_nj_500_27 := mk_jvr_bnds([emllim[1],emulim[1]] as rbnds,
                                         [emllim[2],emulim[2]] as rbnds,
                                         lims3,
                                         [emllim[4],emulim[4]] as rbnds,
                                         [emllim[5],emulim[5]] as rbnds,
                                         [emllim[6],emulim[6]] as rbnds);
end;
 
 
{  This function determines whether J is inside the joint limits }
{  for robot X.  The argument X is the robot object and is never }
{  used by Meta-Kinematics-generated code.  The argument J is    }
{  the current joint vector in the CIM convention.               }
function in_joint_limits_nj_500_27(x:object; j:jvr): boolean;
var pjl: jvr_bnds;
begin
  pjl:=joint_limits_nj_500_27(x,j);
  in_joint_limits_nj_500_27:= in_bnds(j[1],pjl[1]) and
                               in_bnds(j[2],pjl[2]) and
                               in_bnds(j[3],pjl[3]) and
                               in_bnds(j[4],pjl[4]) and
                               in_bnds(j[5],pjl[5]) and
                               in_bnds(j[6],pjl[6]);
end;
