%--------------------------------------------------------------------------
% File:            Sim_bipedwalking.m
% Author: 	  Filipe Silva
% Date:			15-Set-2014
% Purpose:   Simulate a crouching movement: up/down with
%                    the knees bent
%--------------------------------------------------------------------------
clear all
close all
clc

SimParam
%//////////////////////////////////////////////////////////////////////////
% INITIAL CONFIGURATION
%//////////////////////////////////////////////////////////////////////////
% Graphical representation of the humanoid robot (lower limbs)
%-------------------------------------------------------------------------- 
 P_ref_r = [0 0 0]; P_ref_l = [0 l3 0]; 
 qi_r = [0 0 0 0 0 0]';   qi_l = [0 0 0 0 0 0]'; 
 [P_r,P_hip] = DirKin6dof_stance( P_ref_r,qi_r,L,+1 );
 P_l = DirKin6dof_stance( P_ref_l,qi_l,L,-1 );
 [C_r,F_r,H_rl,T_hip] = JointCoord_stance( P_ref_r,qi_r,L,+1 );
 [C_l,F_l] = JointCoord_stance( P_ref_l,qi_l,L,-1 );
 [COG_r,COG_rl] = RobCOG_stance( P_ref_r,qi_r,L,M,+1 );
 [COG_l] = RobCOG_stance( P_ref_l,qi_l,L,M,-1 );
 COG = RobCOG(COG_r,COG_rl,COG_l,M);
 CreateWorldBodies( P_r,C_r,F_r,COG_r,H_rl,COG_rl,P_l,C_l,F_l,COG_l,COG ) 
%//////////////////////////////////////////////////////////////////////////
% SPECIFIES THE INITIAL CONFIGURATION
%//////////////////////////////////////////////////////////////////////////
%-- HIP POSITION (xh,yh) ---------------------------------------------- 
 xh_i = input('Hip XX-coord [0]: ');
 if isempty(xh_i), xh_i = 0; end  
 yh_i = input('Hip YY-coord [l3/2]: ');
 if isempty(yh_i), yh_i = l3/2; end  
%-- HIP HEIGHT zh ---------------------------------------------- 
 Perc_max = input('Hip height [100%Ltotal]: ');
 if isempty(Perc_max), 
     zh_i = 0.95*(l0+l1+l2); 
 else zh_i = Perc_max*(l0+l1+l2);
 end 
 HIP_xyz_i = [xh_i yh_i zh_i]
 disp(sprintf('-------------------------------'))
%//////////////////////////////////////////////////////////////////////////
% INITIAL CONFIGURATION
%//////////////////////////////////////////////////////////////////////////
%-- Set the execution time ------------------------------------------------  
 Ts = 0.007;  % Step size ------------------------------------------------------
 Texe = 1;
 t = 0:Ts:Texe;
 DIM = length(t);
%--------------------------------------------------------------------------
% Inverse Kinematics: final joint configuration
%-------------------------------------------------------------------------- 
 [qf_r,IsVal] = InvKin6dof_stance([xh_i yh_i zh_i],L,+1);
 if IsVal ~= 1, disp('WorkSpaceOut...'), continue, end
 qf_l([1 4 5]) = qf_r([1 4 5]);     
 qf_l([2 3 6 ]) = -qf_r([2 3 6]); 
%--------------------------------------------------------------------------
% Trajectory Planning: joint space motion planning
%--------------------------------------------------------------------------
counter_r = 0; counter_l = 0;
for kk = 1:length(qi_r),
      [Q_r(kk,:),dQ_r(kk,:),ddQ_r(kk,:),flag] = PolyTrajGenerator(t,t(end),qi_r(kk),qf_r(kk),5);
      counter_r = counter_r+flag;
      [Q_l(kk,:),dQ_l(kk,:),ddQ_l(kk,:),flag] = PolyTrajGenerator(t,t(end),qi_l(kk),qf_l(kk),5);
      counter_l = counter_l+flag;
end
%--------------------------------------------------------------------------
% Animate Motion: move synchronously the stance & swing legs
%--------------------------------------------------------------------------
 if counter_r == 6 && counter_l == 6,  % Null motion!!
    str = sprintf('\n LEGs already there!')
    disp(str)        
 else
     P_trace = [];
     for tt=1:DIM, 
         Animation3D_DS(P_ref_r,P_ref_l,Q_r(:,tt),Q_l(:,tt),L,M,+1)
     end
 end
%//////////////////////////////////////////////////////////////////////////
% START MOTION ANIMATION
%//////////////////////////////////////////////////////////////////////////
% 1- Input the kinematic parameters
% 2- Calculates the motion planning
% 3- Animates the overall motion
%--------------------------------------------------------------------------
% Variables initialization 
%--------------------------------------------------------------------------
runAGAIN = 1;
while runAGAIN,

%-- Input the KINEMATIC PARAMETERS ----------------------------------------
 %-- Specifies the SEPARATION BETWEEN LEGS ---------------------------------
 %SBLegs = input('Separation between legs (m): ');
 %if isempty(SBLegs), SBLegs = 3*l3/4; end
%-- HIP POSITION (xh,yh) ---------------------------------------------- 
 str = sprintf('Hip XX-coord [%0.5g]: ',xh_i);
 xh_f = input(str);
 if isempty(xh_f), xh_f = xh_i; end  
 str = sprintf('Hip YY-coord [%0.5g]: ',yh_i);
 yh_f = input(str);
 if isempty(yh_f), yh_f = yh_i; end   
%-- HIP HEIGHT zh ---------------------------------------------- 
 str = sprintf('Hip height [%0.5g]: ',zh_i/(l0+l1+l2));
 Perc_max = input(str);
 if isempty(Perc_max), 
     zh_f = l0+0.95*(l1+l2); 
 else zh_f = Perc_max*(l0+l1+l2);
 end 
 HIP_xyz_f = [xh_f yh_f zh_f]
 disp(sprintf('-------------------------------'))
 %//////////////////////////////////////////////////////////////////////////

%//////////////////////////////////////////////////////////////////////////
% FIRST STEP: move down
%//////////////////////////////////////////////////////////////////////////
%-- Set the execution time ------------------------------------------------  
 Vf = 0.1; %-- Mean progression velocity 
 Texe = abs(HIP_xyz_f(3)-HIP_xyz_i(3))/Vf;
 Texe=2
 t = 0:Ts:Texe;
 DIM = length(t);
%--------------------------------------------------------------------------
% DOUBLE-SUPPORT PHASE 
% Trajectory Planning: hip Cartesian space motion planning
%--------------------------------------------------------------------------
 counter = 0;
 P_hip = zeros(3,DIM); dP_hip = zeros(3,DIM); ddP_hip = zeros(3,DIM); % Pre-allocation for speed
 [P_hip(1,:),dP_hip(1,:),ddP_hip(1,:),flag] = PolyTrajGenerator(t,t(end),HIP_xyz_i(1),HIP_xyz_f(1),5);
 counter = counter + flag;
 [P_hip(2,:),dP_hip(2,:),ddP_hip(2,:),flag] = PolyTrajGenerator(t,t(end),HIP_xyz_i(2),HIP_xyz_f(2),5);
 counter = counter + flag;
 [P_hip(3,:),dP_hip(3,:),ddP_hip(3,:),flag] = PolyTrajGenerator(t,t(end),HIP_xyz_i(3),HIP_xyz_f(3),5);
 counter = counter + flag;
 
%--------------------------------------------------------------------------
% Inverse Kinematics: trajectory of the joint angles
%--------------------------------------------------------------------------
 Q_r = []; Q_l = [];
 VALID_motion = 1;
 for kk = 1:DIM,
     [qf_r,IsVal] = InvKin6dof_stance(P_hip(:,kk),L,+1);
     if IsVal ~= 1, 
         disp('Desired HIP coordinates out of the workspace!.. try again'), 
         Q_r(:,kk) = Q_r(:,kk-1);  Q_l(:,kk) = Q_l(:,kk-1); 
         VALID_motion = 0; break         
     else
            Q_r(:,kk) = qf_r;     
            Q_l([1 4 5],kk) = qf_r([1 4 5]);     
            Q_l([2 3 6 ],kk) = -qf_r([2 3 6]);     
     end
 end
 
%--------------------------------------------------------------------------
% Animate Motion: move synchronously the stance & swing legs
%--------------------------------------------------------------------------
if VALID_motion,
    P_trace = [];
    for tt=1:DIM, 
          if tt == DIM, SAVE_ans = 0; end
          Animation3D_DS(P_ref_r,P_ref_l,Q_r(:,tt),Q_l(:,tt),L,M,+1);
    end
    HIP_xyz_i = HIP_xyz_f;
    xh_i = xh_f; yh_i = yh_f; zh_i = zh_f;
end
clear P_hip dP_hip ddP_hip
 
 
 end %----------------------------------------------------------endWHILETRUE
