%--------------------------------------------------------------------------
% File:         Sim_manip.m
% Author: 		Filipe Silva
% Date:			23-May-2011
% Purpose:      Humanoid Project - kinematic modelling/simulation of the
%               2x 5-dof manipulator arms
%--------------------------------------------------------------------------
clear all
close all
clc
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% SIMULATION PARAMETERS & INITIALIZATIONS
%//////////////////////////////////////////////////////////////////////////
% Link lengths ------------------------------------------------------------
 lt = 0.35; lt_l= 0.4; lt_w = 0.1; lt_h = 0.25; % Trunk's dimensions
 lr = 0.0125; l0 = 0.05;                         % Cylinders & shoulder
 l1= 0.35; l1_1 = 0.2; l1_2 = 0.15; l2= 0.26;   % Arm's link lenghts
 L = [lt lt_l lt_w lt_h lr l0 l1 l1_1 l1_2 l2 ];% Vector L
% Initial joint angles ----------------------------------------------------
 qi_l = [0; 0; 0; pi/2];                % Left arm initial configuration
 qi_r = [0; 0; 0; -pi/2];               % Right arm initial configuration
% Joint angular&speed limits ----------------------------------------------
 q_lim = [  -pi/4  0   -pi/2  0;...     % Joint physical limits
           3*pi/4  pi   pi/2  3*pi/4];  % Joint physical limits
 dq_max = [10 10 10 10]';                   % Maximum joint angular velocities
% Global variables (only animation purposes!) ----------------------------
 global P_trace_l P_trace_r
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% GRAPHICAL REPRSENTATION OF THE ARMS @ INITIAL CONFIGURATION
%//////////////////////////////////////////////////////////////////////////
% Joint and body coordinates ----------------------------------------------
 P_link_l = DirKin_4dof(qi_l,L,+1);  % +1 left arm; -1 right arm
 P_link_r = DirKin_4dof(qi_r,L,-1);  % +1 left arm; -1 right arm
 P_trace_l = P_link_l(:,end);
 P_trace_r = P_link_r(:,end);
 [P_joint_l,P_tool_l] = BodyCoordinates( qi_l,L,+1 );
 [P_joint_r,P_tool_r] = BodyCoordinates( qi_r,L,-1 );
 CreateWorldBodies( P_link_l,P_link_r,P_joint_l,P_joint_r,...
                    P_tool_l,P_tool_r,P_trace_l,P_trace_r,L )
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% USER INPUT FROM THE KEYBOARD
% Run inside the main computer (PC running Matlab or PC104+)
%//////////////////////////////////////////////////////////////////////////
while true,
% Select the control flow -------------------------------------------------
 clc
 CTRL_flag = input('Press C to continue or E to exit: ','s');
 if isempty(CTRL_flag), CTRL_flag = 'c'; end
 if CTRL_flag == 'e' | CTRL_flag == 'E', break, end
% Select the control mode -------------------------------------------------
 CTRL_mode = input('Input CONTROL_mode [L R]: ');
 if isempty(CTRL_mode), CTRL_mode = [1 1]; end
% Select the target coordinates ------------------------------------------- 
 XYZ_f = input('Input target coordinates [L;R]: ');
 if isempty(XYZ_f), 
    if CTRL_mode(1) == 1, XYZ_f(1,:) = [0 lt_l/2+l0 lt+lt_h-lr-l1-l2];
    else                  XYZ_f(1,:) = [l2 lt_l/2+l0 lt+lt_h-lr-l1]; end
    if CTRL_mode(2) == 1, XYZ_f(2,:) = [0 -lt_l/2-l0 lt+lt_h-lr-l1-l2];
    else                  XYZ_f(2,:) = [l2 -lt_l/2-l0 lt+lt_h-lr-l1]; end
 end
 plot3(XYZ_f(:,1),XYZ_f(:,2),XYZ_f(:,3),'k.')
% Target coordinates are inside the work volume? --------------------------
 if ( XYZ_f(1,1)^2+(XYZ_f(1,2)-(lt_l/2+l0))^2+(XYZ_f(1,3)-(lt+lt_h-lr))^2 > (l1+l2)^2 ), 
    disp('LeftWorkSpaceOut: press a key to continue...'), pause, continue
 else plot3(XYZ_f(1,1),XYZ_f(1,2),XYZ_f(1,3),'k.'), end
 if ( XYZ_f(2,1)^2+(XYZ_f(2,2)+lt_l/2+l0)^2+(XYZ_f(2,3)-(lt+lt_h-lr))^2 > (l1+l2)^2 ), 
    disp('RightWorkSpaceOut: press a key to continue...'), pause, continue
 else plot3(XYZ_f(2,1),XYZ_f(2,2),XYZ_f(2,3),'k.'), end
% Select the execution time -----------------------------------------------  
 Texe = input('Input execution time [L R]: ');
 if isempty(Texe), Texe = [4 4]; end    % Default execution time
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% VIRTUAL TIME
%//////////////////////////////////////////////////////////////////////////
% Defines the virtual time ------------------------------------------------ 
 Ts = 0.005;                        % Sampling time
 t_l = 0:Ts:Texe(1);                % Executation time
 t_r = 0:Ts:Texe(2);                % Executation time
 DIM_l = length(t_l);               % Dimension of time vector
 DIM_r = length(t_r);               % Dimension of time vector
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% MOTION CONTROL
% Run inside the dsPIC microcontroller
%//////////////////////////////////////////////////////////////////////////
% The high-level motion directives include:
% 1- Point-to-point motion with trajectory planning in the joint space
% 2- Continuous motion (straight-line) with trajectory planning in the 
%    Cartesian space (3 different implementations!)
%--------------------------------------------------------------------------
 Q_l = []; dQ_l = []; ddQ_l = [];
 Q_r = []; dQ_r = []; ddQ_r = [];
 if CTRL_mode(1) == 1,  % Point-to-point motion: LEFT ARM -----------------
    % Inverse kinematics algorithm: final joint configuration
    [qf_sol,IsVal] = InvKin_4dof(qi_l,q_lim,XYZ_f(1,:),L,+1); % LEFT ARM ------
    if IsVal == 0, disp('NoPhysicalSolution...'), qf_l = qi;
    else qf_l = qf_sol; end    
    % Trajectory planning & motion animation (call "setPosId" ...) 
    [Q_l,dQ_l,ddQ_l,counter_l] = setJointPosition(t_l,t_l(end),qi_l,qf_l,3);         
 end
 if CTRL_mode(2) == 1,  % Point-to-point motion: RIGTH ARM -----------------
    % Inverse kinematics algorithm: final joint configuration
    [qf_sol,IsVal] = InvKin_4dof(qi_r,q_lim,XYZ_f(2,:),L,-1); % RIGHT ARM -----
    if IsVal == 0, disp('NoPhysicalSolution...'), qf_r = qi;
    else qf_r = qf_sol; end
    % Trajectory planning & motion animation (call "setPosId" ...)           
    [Q_r,dQ_r,ddQ_r,counter_r] = setJointPosition(t_r,t_r(end),qi_r,qf_r,3);            
 end
%--------------------------------------------------------------------------
% IF CTRL_MODE == [1 1] ANIMATES THE PtP MOTION
%--------------------------------------------------------------------------
 if CTRL_mode(1) == 1 && CTRL_mode(2) == 1,
    if counter_l ~= 4 | counter_r ~= 4, % && ~sum(NUM_out),    % There is joint motion               
       % Animates the robot arm's motion 
       P_trace_l = [];  P_trace_r = [];
       DIM = max([DIM_l DIM_r]);
       for tt=1:20:DIM, 
           if tt <= DIM_l, q_l_tt = Q_l(:,tt); else q_l_tt = Q_l(:,end); end
           if tt <= DIM_r, q_r_tt = Q_r(:,tt); else q_r_tt = Q_r(:,end); end
           AnimateMotion(q_l_tt,q_r_tt,L);
       end
       % Re-initialization of the joint variables
         qi_l = Q_l(:,end);   qi_r = Q_r(:,end);
        dqi_l = [0 0 0 0]';  dqi_r = [0 0 0 0]';
       ddqi_l = [0 0 0 0]'; ddqi_r = [0 0 0 0]';       
    else
        % Null motion!!
        str = sprintf('\n ARMS in the specified coordinates!')
        disp(str)        
    end 
 else
%--------------------------------------------------------------------------
% IF CTRL_MODE ~= [1 1] IMPLEMENTS THE CONTINUOUS MOTION
%--------------------------------------------------------------------------  
    % Change the velocity parameter in real-time --------------------------
    VEL_mode = 0;       % 0: inv_kin & diff
                        % 1: Pseudo-inverse
                        % 2: CLIK
    % Variables initialization --------------------------------------------
    if CTRL_mode(1) == 2,
        Q_l_ref = qi_l;
       dQ_l_ref = [0 0 0 0]';
       q_l_tt = qi_l;
       Q_l(:,1) = q_l_tt;
    end
    if CTRL_mode(2) == 2,
         Q_r_ref = qi_r;
        dQ_r_ref = [0 0 0 0]';
        q_r_tt = qi_r;
        Q_r(:,1) = q_r_tt;
    end
    P_trace_l = [];  P_trace_r = [];
    DIM = max([DIM_l DIM_r]);
    % On-line control based on dsPIC timer --------------------------------
    for tt = 2:DIM,
        if tt <= DIM_l, % LEFT ARM ----------------------------------------
           if CTRL_mode(1) == 2,                                       
              % Trajectory planning in the Cartesian space
              [Pee,dPee,ddPee] = CartTrajPlanning( t_l(tt),t_l(end),qi_l,XYZ_f(1,:),L,+1,3 );
              % Inverse kinematics algorithm
              switch VEL_mode,
                  case 0,% Inv_kin: position
                         %tic
                         %for zz = 1:100000,
                         [qf_sol,IsVal] = InvKin_4dof(qi_l,q_lim,Pee,L,+1);
                         if IsVal == 0, disp('NoPhysicalSolution...'), 
                              Q_l_ref(:,tt) = qi_l;
                         else Q_l_ref(:,tt) = qf_sol; 
                         end    
                         % Estimate the joint velocities: dQ_l_ref = NumericalDiff(t_l,Q_l_ref);
                         dQ_l_ref(:,tt) = ( Q_l_ref(:,tt)-Q_l_ref(:,tt-1) )/Ts;                         
                         %end
                         %toc, pause                         
                  case 1,% Inv_kin: pseudo inverse (pinv: matlab)
                         %tic
                         %for zz = 1:100000,
                         J = Jacobian_4dof(q_l_tt,L,+1);
                         dQ_l_ref(:,tt) = pinv(J)*dPee;
                         %end
                         %toc, pause                         
                  case 2, % Inv_kin: Jacobian transpose: dQ = J'*Ke
                         %tic
                         %for zz = 1:100000,
                         J = Jacobian_4dof(q_l_tt,L,+1);
                         P_actual_l = DirKin_4dof(q_l_tt,L,+1);
                         K_gain = [500 0 0; 0 500 0 ; 0 0 500];
                         err = (Pee-P_actual_l(:,3));
                         dQ_l_ref(:,tt) = J'*(dPee+K_gain*err);
                         %end
                         %toc, pause
              end
              % Speed physical limits -------------------------------------
              for ii = 1:4,
                  if abs(dQ_l_ref(ii,tt)) > dq_max(ii), 
                     dQ_l_ref(ii,tt) = sign(dQ_l_ref(ii,tt))*dq_max(ii);
                     str = sprintf('Left_ARM: joint %d exceeds the speed limits',ii)
                     disp(str)
                  end
              end
              % Call "setSpeedReadPos" to update the joint speed ----------
              q_l_tt = q_l_tt+dQ_l_ref(:,tt)*Ts;                
               Q_l(:,tt) = q_l_tt;  % Save time course  
              dQ_l(:,tt) = dQ_l_ref(:,tt);
           else
               q_l_tt = Q_l(:,tt);      
           end
        else
            q_l_tt = Q_l(:,end);             
        end % LEFT ARM ----------------------------------------------------
        
        if tt <= DIM_r, % RIGHT ARM ---------------------------------------
           if CTRL_mode(2) == 2,                                       
              % Trajectory planning in the Cartesian space
              [Pee,dPee,ddPee] = CartTrajPlanning( t_r(tt),t_r(end),qi_r,XYZ_f(2,:),L,-1,3 );
              % Inverse kinematics algorithm: trajectory planning
              switch VEL_mode,
                  case 0,% Inv_kin: position 
                         [qf_sol,IsVal] = InvKin_4dof(qi_r,q_lim,Pee,L,-1);
                         if IsVal == 0, disp('NoPhysicalSolution...'), 
                            Q_r_ref(:,tt) = qi_r;
                         else Q_r_ref(:,tt) = qf_sol; 
                         end    
                         % Estimate the joint velocities: dQ_r_ref = NumericalDiff(t_r,Q_r_ref);
                         dQ_r_ref(:,tt) = ( Q_r_ref(:,tt)-Q_r_ref(:,tt-1) )/Ts;
                  case 1,% inv_kin: pseudo inverse
                         J = Jacobian_4dof(q_r_tt,L,-1);
                         dQ_r_ref(:,tt) = pinv(J)*dPee;
                  case 2,% inv_kin: pseudo inverse
                         J = Jacobian_4dof(q_r_tt,L,-1);
                         P_actual_r = DirKin_4dof(q_r_tt,L,-1);
                         K_gain = [500 0 0; 0 500 0 ; 0 0 500];
                         err = (Pee-P_actual_r(:,3));
                         dQ_r_ref(:,tt) = J'*(dPee+K_gain*err);
              end
              % Speed physical limits -------------------------------------
              for ii = 1:4,
                  if abs(dQ_r_ref(ii,tt)) > dq_max(ii), 
                     dQ_r_ref(ii,tt) = sign(dQ_r_ref(ii,tt))*dq_max(ii);
                     str = sprintf('Right_ARM: joint %d exceeds the speed limits',ii)
                     disp(str)
                  end
              end
              % Call "setSpeedReadPos" to update the joint speed ----------
              q_r_tt = q_r_tt+dQ_r_ref(:,tt)*Ts;                
               Q_r(:,tt) = q_r_tt;  % Save time course   
              dQ_r(:,tt) = dQ_r_ref(:,tt);
           else
               q_r_tt = Q_r(:,tt);      
           end
        else
            q_r_tt = Q_r(:,end); 
        end % RIGHT ARM ---------------------------------------------------
        
        AnimateMotion(q_l_tt,q_r_tt,L);                        
    end    
    % Re-initialization of the joint variables ----------------------------
      qi_l = Q_l(:,end);   qi_r = Q_r(:,end);
     dqi_l = [0 0 0 0]';  dqi_r = [0 0 0 0]';
    ddqi_l = [0 0 0 0]'; ddqi_r = [0 0 0 0]';
            
 end
 
 % Plot the time courses for Q, dQ and ddQ (numerically!)
 %PlotGraphics( t_l,Q_l,dQ_l )
end %----------------------------------------------------------endWHILETRUE
%--------------------------------------------------------------------------
% Exit
%--------------------------------------------------------------------------
 clc, disp('ByeBye!!...--------------------------------------------------')
 close all, return          