 
{ 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_c5g_pal_180_31(guy : object; j : jvr) : rbnds;
 
var llim,ulim : real;
    a : av;
    jlim : jvr;
 
begin
  ulim	:=	-30.0 * dtor ;
  llim	:=	-161.0 * dtor ;;
  if J.J2<(-52.0*dtor) then	ulim	:=	J.J2 + ( 22.0*dtor) ;  		
  if J.J2>(-43.0*dtor) then	llim	:=	J.J2 - (118.0*dtor) ;
  joint3_limits_c5g_pal_180_31 := [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_c5g_pal_180_31(guy: object;j:jvr): jvr_bnds;
var jlr : joint_limited_robot;
begin
  jlr := guy to_view joint_limited_robot;
  joint_limits_c5g_pal_180_31:=
    mk_jvr_bnds( 
       jlr.joint_limits[1],
       jlr.joint_limits[2],
       reduce(joint3_limits_c5g_pal_180_31(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_c5g_pal_180_31(guy: object;j:jvr): jvr_bnds;
var
    lims3 : rbnds;
    cim_lims : jvr_bnds;
    julim, jllim, emulim, emllim : jvr;
begin
  cim_lims := joint_limits_c5g_pal_180_31(guy,j);
  julim := j;
  jllim := j;
  julim.j3 := ub(cim_lims[3]);
  jllim.j3 := lb(cim_lims[3]);
  emulim := cim_to_c5g_pal_180_31_jvr(guy,julim);
  emllim := cim_to_c5g_pal_180_31_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_c5g_pal_180_31_jvr(guy,julim);
  emllim := cim_to_c5g_pal_180_31_jvr(guy,jllim);
  em_joint_limits_c5g_pal_180_31 := 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_c5g_pal_180_31(x:object; j:jvr): boolean;
var pjl: jvr_bnds;
begin
  pjl:=joint_limits_c5g_pal_180_31(x,j);
  in_joint_limits_c5g_pal_180_31:= 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;
