%--------------------------------------------------------------------------
% File:         Sim_swingleg.m
% Author: 		Filipe Silva
% Date:			30-June-2011
% Purpose:      Humanoid Project - kinematic modelling/simulation of the
%               6-dof swing leg
%--------------------------------------------------------------------------
clear all
close all
clc
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% SIMULATION PARAMETERS & INITIALIZATIONS
%//////////////////////////////////////////////////////////////////////////
% Link lengths ------------------------------------------------------------
 l1= 0.143; l1_1 = 0.07; l1_2 = 0.073; l2= 0.139; l3 = 0.05; % Link lenghts
 L = [l1 l1_1 l1_2 l2 l3];                                   % Vector L
% Global variables (only animation purposes!) -----------------------------
 global Hh P_trace
 Hh = (l1+l2+l3)*0.9; 
% Joint angular&speed limits ----------------------------------------------
 q_lim = [  -pi  -pi  -pi  -pi;...     % Joint physical limits
             pi   pi   pi  pi];        % Joint physical limits
 dq_max = [3 3 3 3 3 3]';              % Maximum joint angular velocities 
% Initial joint angles ----------------------------------------------------
 XYZ_i = [-0.12 0 0];                           % Foot's initial coordinates
 [qi,IsVal] = InvKin_6dof(XYZ_i,L,q_lim); % Initial joint angles
 if IsVal == 0,
    disp('Out of volume space')
    pause
 end
 z_gnd = 0;                              % Initial surface z-axis level
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% GRAPHICAL REPRESENTATION OF THE LEG @ INITIAL CONFIGURATION
%//////////////////////////////////////////////////////////////////////////
% Joint and body coordinates ----------------------------------------------
 P_link = DirKin_6dof(qi,L);  % +1 left arm; -1 right arm
 P_trace = P_link(:,end);
 [P_joint,P_foot] = BodyCoordinates( qi,L );
 CreateWorldBodies( P_link,P_joint,P_foot,P_trace,z_gnd,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 {1,2}: ');
 if isempty(CTRL_mode), CTRL_mode = 1; end
% Select the target coordinates ------------------------------------------- 
 XYZ_f = input('Input FOOT target coordinates (1x3): ');
 if isempty(XYZ_f), XYZ_f(1,:) = [0.12 0 0]; end
 plot3(XYZ_f(:,1),XYZ_f(:,2),XYZ_f(:,3),'k.')
% Target coordinates are inside the work volume? --------------------------
 if ( XYZ_f(1)^2+XYZ_f(2)^2+(XYZ_f(3)-Hh)^2 > (l1+l2+l3)^2 ), 
    disp('WorkSpaceOut: press a key to continue...'), 
    pause, continue
 end
% Select the execution time -----------------------------------------------  
 Texe = input('Input execution time (s): ');
 if isempty(Texe), Texe = 4; end    % Default execution time
%--------------------------------------------------------------------------
%//////////////////////////////////////////////////////////////////////////
% VIRTUAL TIME
%//////////////////////////////////////////////////////////////////////////
% Defines the virtual time ------------------------------------------------ 
 Ts = 0.005;                    % Sampling time
 t = 0:Ts:Texe;                 % Executation time
 DIM = length(t);               % 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 = []; dQ = []; ddQ = [];
 if CTRL_mode == 1,  % Point-to-point motion: LEFT ARM --------------------
    % Inverse kinematics algorithm: final joint configuration
    [qf_sol,IsVal] = InvKin_6dof(XYZ_f,L,q_lim); % SWING LEG --------------
    if IsVal == 0, disp('NoPhysicalSolution...'), qf = qi;
    else qf = qf_sol; end    
    % Trajectory planning & motion animation (call "setPosId" ...) --------
    [Q,dQ,ddQ,counter] = setJointPosition(t,t(end),qi,qf,3);         
    % Animates the swing leg motion ---------------------------------------
    if counter ~= 6, % && ~sum(NUM_out),    % There is ALL-joint motion               
       P_trace = [];
       for tt=1:20:DIM, 
           AnimateMotion(Q(:,tt),z_gnd,L);
       end
       % Re-initialization of the joint variables
         qi = Q(:,end); dqi = [0 0 0 0 0 0]'; ddqi = [0 0 0 0 0 0]';
    else
        % Null motion!!
        str = sprintf('\n ARMS in the specified coordinates!')
        disp(str)        
    end
 else               % Continuous motion along a straight line -------------
     % Variables initialization--------------------------------------------
      Q_ref = qi;
     dQ_ref = [0 0 0 0 0 0]';
     q_tt = qi;
     Q(:,1) = q_tt;
     % Run online until texe ---------------------------------------------- 
     P_trace = [];
     % Get current hip XYZ_coordinates  
     P = DirKin_6dof(qi,L);
     XYZ_i = P(:,4)';
     for tt = 2:DIM,        
         % Trajectory planning in the Cartesian space
         [Pee,dPee,ddPee] = CartTrajPlanning( t(tt),t(end),XYZ_i,XYZ_f,L,3 );
         % Inverse kinematics algorithm
         [qf_sol,IsVal] = InvKin_6dof(Pee,L,q_lim);
         if IsVal == 0, disp('NoPhysicalSolution...'),  Q_ref(:,tt) = qi; pause
         else Q_ref(:,tt) = qf_sol; end    
         % Estimate the joint velocities: dQ_l_ref = NumericalDiff(t_l,Q_l_ref);
         dQ_ref(:,tt) = ( Q_ref(:,tt)-Q_ref(:,tt-1) )/Ts;
         % Speed physical limits ------------------------------------------
         for ii = 1:6,
             if abs(dQ_ref(ii,tt)) > dq_max(ii), 
                dQ_ref(ii,tt) = sign(dQ_ref(ii,tt))*dq_max(ii);
                str = sprintf('SWING_LEG: joint %d exceeds the speed limits',ii)
                disp(str), pause
             end
          end
          % Call "setSpeedReadPos" to update the joint speed --------------
          q_tt = q_tt+dQ_ref(:,tt)*Ts;                
          
           Q(:,tt) = q_tt;  % Save time course  
          dQ(:,tt) = dQ_ref(:,tt);
          % Animates the swing leg motion ---------------------------------------
          AnimateMotion(Q(:,tt),z_gnd,L);
     end
     % Re-initialization of the joint variables ---------------------------
      qi = Q(:,end); dqi = [0 0 0 0 0 0]'; ddqi = [0 0 0 0 0 0]';  
      tt, pause
 end
 % Plot the time courses for Q, d Q and ddQ (numerically!)
 %PlotGraphics( t_l,Q_l,dQ_l )
end %----------------------------------------------------------endWHILETRUE
%--------------------------------------------------------------------------
% Exit
%--------------------------------------------------------------------------
 clc, disp('ByeBye!!...--------------------------------------------------')
 close all, return 