%--------------------------------------------------------------------------
% 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 OF THE MANIPULATOR ARM
%//////////////////////////////////////////////////////////////////////////
% Graphical representation of the manipulator at the initial configuration
 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
 
 qi_l = [0; 0; 0; pi/2];                 % Left arm initial configuration
 qi_r = [0; 0; 0; -pi/2];                % Right arm initial configuration
 q_lim = [5*pi/6;2*pi/3;2*pi/3;2*pi/3]; % Joint physical limits
 dq_max = [3 3 3 3];                    % Maximum joint angular velocities
 
 INum = [];
 
 global P_trace_l P_trace_r
% RUN INSIDE THE SERVOMOTOR CONTROLLER ####################################
 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 )
%##########################################################################                            
%//////////////////////////////////////////////////////////////////////////
% MOTION CONTROL
%//////////////////////////////////////////////////////////////////////////
% 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
%--------------------------------------------------------------------------
% Select the 
%--------------------------------------------------------------------------
while true,
% RUN INSIDE MAIN COMPUTER (MATLAB GUI) ###################################
%-- Control the while cycle -----------------------------------------------
 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
%-- Set the target variables ---------------------------------------------- 
 XYZ_f = input('Input target coordinates [L:X Y Z; R:X Y Z]: ');
 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 coordinate is inside the work volume? --------------------------
% if ( XYZ_f(1)^2+XYZ_f(2)^2+(XYZ_f(3)-d1)^2 > (l2+l3+(l4+0.15))^2 ), 
%    disp('WorkSpaceOut: press a key to continue...'), pause, continue
% else plot3(XYZ_f(1),XYZ_f(2),XYZ_f(3),'k.'), end
%-- Set the execution time ------------------------------------------------  
 Texe = input('Input execution time [L R]: ');
 if isempty(Texe), Texe = [4 4]; end    % Default execution 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
%##########################################################################

%--------------------------------------------------------------------------
% Move the joints using the variables: CONTROL_mode, XYZ_target
%--------------------------------------------------------------------------
 Q = []; dQ = []; ddQ = [];
 if CTRL_mode(1) == 1,  % Point-to-point motion: LEFT ARM -----------------
    % Inverse kinematics algorithm: final joint configuration
    [qf_sol,IsVal] = InvKin_4dof(qi_l,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            
    [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: LEFT ARM -----------------
    % Inverse kinematics algorithm: final joint configuration
    [qf_sol,IsVal] = InvKin_4dof(qi_r,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            
    [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 && 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
        % Exceeds maximum joint velocity allowed
        str = sprintf('\n Maximum joint speed limit exceeded')
        disp(str)        
    end 
 else
    % Change the velocity parameter in real-time
    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]);            
    for tt = 2:DIM,
        if tt <= DIM_l, % LEFT ARM ----------------------------------------
           if CTRL_mode(1) == 2,                                       
              % Off-line trajectory planning in the Cartesian space
              [Pee,dPee,ddPee] = CartTrajPlanning( t_l(tt),t_l(end),qi_l,XYZ_f(1,:),L,+1,3 );
              % Off-line inverse kinematics algorithm: trajectory planning
              [qf_sol,IsVal] = InvKin_4dof(qi_l,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;
              % SEND COMMAND TO THE SERVO & RUN INSIDE ITS CONTROLLER ###################
              q_l_tt = q_l_tt+dQ_l_ref(:,tt)*Ts;                
              Q_l(:,tt) = q_l_tt;  % Save time course            
           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,                                       
              % Off-line trajectory planning in the Cartesian space
              [Pee,dPee,ddPee] = CartTrajPlanning( t_r(tt),t_r(end),qi_r,XYZ_f(2,:),L,-1,3 );
              % Off-line inverse kinematics algorithm: trajectory planning
              [qf_sol,IsVal] = InvKin_4dof(qi_r,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;
              % SEND COMMAND TO THE SERVO & RUN INSIDE ITS CONTROLLER ###################
              q_r_tt = q_r_tt+dQ_r_ref(:,tt)*Ts;                
              Q_r(:,tt) = q_r_tt;  % Save time course            
           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
end %----------------------------------------------------------endWHILETRUE
%--------------------------------------------------------------------------
% Exit
%--------------------------------------------------------------------------
 clc, disp('ByeBye!!...--------------------------------------------------')
 close all, return
% Differentiation followed by Integration
%clear all, clc
%Ts = 1e-3; 
%t = 0:Ts:1; 
%DIM_l = length(t); 
% Define x(t) and obtain the numerical diff dx(t)
% t = t_l;
% % x = sin(2*pi*4*t);
% x = Q_l_ref(1,:);
% dx = NumericalDiff(t_l,x);
% % Integrate dx
% INTG_mode = 0;
% switch INTG_mode,
%     case 0,
%            my_x(1) = x(1);
%            my_x(2) = dx(2)*Ts;
%            for kk = 3:DIM_l,
%                my_x(kk) = my_x(kk-1)+dx(kk)*Ts;
%            end 
%     case 1,
%            my_x(1) = x(1);
%            my_x(2) = my_x(1)+dx(1)*Ts;
%            for kk = 3:DIM_l-1, 
%                my_x(kk) = my_x(kk-2)+dx(kk-1)*2*Ts; 
%            end 
%            my_x(DIM_l) = my_x(DIM_l-1)+dx(DIM_l)*Ts;
% end
% % Plot time courses
% figure
% subplot(2,1,1), plot(t,x,'r', t,my_x,'b-.')
% subplot(2,1,2), plot(t,dx,'r')

          