% construct a simple 3 - linkage robot to simulate walking. assume that a
% 1 DOF foot is in contact with an external weight during the period of 
% foot - earth contact. in the period of raising the foot no external force
% is applied on it. so the problem is to design a path, solve the inverse 
% kinematics, applying the external force in some parts of this path and
% finally solve the inverse dynamics.

% construct the robot r1 using 3 links as thigh, shank, and foot the robot r1.
% also another robot: r0 is required. this robot is only comprised of two links: 
% thigh and shank.
% this robot will be used to solve inverse kinamatics of path
% following because the foot is assumed to be always parallel to the earth.
% hence the foot joint position can be found usnig the thigh and shank
% positions

clear all

% set the robot dynamic and kinematic specifications

w = 600; % body weight

thigh_length = 0.6;
shank_length = 0.6;
foot_length = 0.2;

thigh_CG = 0.3; % thigh center of gravity distance to its joint
shank_CG = 0.3; % shank center of gravity distance to its joint
foot_CG = 0.1; % foot center of gravity distance to its joint

thigh_mass=10;
shank_mass=10;
foot_mass=1;

thigh_mInertia=[1 0 0; 0 10 0;0 0 10];
shank_mInertia=[1 0 0; 0 10 0;0 0 10];
foot_mInertia=[.1 0 0; 0 1 0;0 0 1];

L1 = link([0 thigh_length 0 0 0],'standard');
L2 = link([0 shank_length 0 0 0],'standard');
L3 = link([0 foot_length 0 0 0],'standard');

L1.m = thigh_mass;
L1.I = thigh_mInertia;
L1.G = 100;
L1.r = [thigh_CG 0 0];
L1.Jm = .1;

L2.m = shank_mass;
L2.I = shank_mInertia;
L2.G = 100;
L2.r = [shank_CG 0 0];
L2.Jm = .1;

L3.m = foot_mass;
L3.I = foot_mInertia;
L3.G = 100;
L3.r = [foot_CG 0 0];
L3.Jm = .1;

% construct the main robot: r1
r1 = robot({L1,L2,L3});

% construct the auxiliary robot r0
r0 = robot({L1,L2});

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% define the foot - earth contact path: path A
hA = 0.9*(thigh_length + shank_length);
T0 = transl([hA foot_length 0]);
T1 = transl([hA -foot_length 0]);

t_course = 1; % time of tracking the path A
t_inc = 0.02; % time increment

t = [0:t_inc:t_course];
r = jtraj(0,1,t); % create a smooth normalized path (between 0 and 1). this
% normalized path will be used by ctraj to divide between T0 and T1

TC_A = ctraj(T0,T1,r);
% plot(t,transl(TC))

% solve the inverse kinematics for r0
q_A = ikine(r0,TC_A,[0 0],[1 1 0 0 0 0]);

% obtain the foot positon using thigh and shank positions
q_A(:,3) = pi/2 - ( q_A(:,1) + q_A(:,2) );

% create the joint velocities and joint accelerations from q and t using
% jtraj

[qc_A qd_A qdd_A] = jtraj(q_A(1,:),q_A(end,:),t);

subplot(3,1,1),plot(t,qc_A)
subplot(3,1,2),plot(t,qd_A)
subplot(3,1,3),plot(t,qdd_A)

% solve the inverse dynamics problem for path A

% set the gravity vector
grav = [0 0 9.81];

% determinig the external forces due to the body weight on foot in the foot
% coordinate frame
% assume the weight is vertical to the foot and is exerting on its middle region 
% therefore to concur with the toolbox notation (f_ext should act on the end of
% manipulator) an external moment should be added to compensate for the actual
% force translation. 

f_ext_A = [ 0 w 0; 0 0 -w*foot_length/2];

% find tau (forces on joints i.e. motor torques)
tau_A = rne(r1, qc_A, qd_A, qdd_A, grav, f_ext_A);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% define the foot - earth non - contacting path: path B
% this path is assumed to be composed of two vertical lines (B1 and B3) and a 
% horizental line (B2) which connects the two vertical lines upper points. the two 
% paths B and A form a rectangle together.
% no external force is applied to the foot in path B.

% define the straight part of path B
hB = 0.7*(thigh_length + shank_length);
T2 = transl([hB -foot_length 0]);
T3 = transl([hB foot_length 0]);

TC_B1 = ctraj(T1,T2,r);
TC_B2 = ctraj(T2,T3,r);
TC_B3 = ctraj(T3,T0,r);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% solve the inverse kinematics for r0 for path B1
% the initial conditions for this problem are qc_A(end,1) , qc_A(end,2)
q_B1 = ikine(r0,TC_B1,[qc_A(end,1) qc_A(end,2)],[1 1 0 0 0 0]);

% obtain the foot positon using thigh and shank positions
q_B1(:,3) = pi/2 - ( q_B1(:,1) + q_B1(:,2) );

% create the joint velocities and joint accelerations from q and t using
% jtraj

[qc_B1 qd_B1 qdd_B1] = jtraj(q_B1(1,:),q_B1(end,:),t);

% solve the inverse dynamics problem for path B1
% find tau (forces on joints i.e. motor torques)

tau_B1 = rne(r1, qc_B1, qd_B1, qdd_B1, grav);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% solve the inverse kinematics for r0 for path B2
% the initial conditions for this problem are qc_B1(end,1) , qc_B1(end,2)
q_B2 = ikine(r0,TC_B2,[qc_B1(end,1) qc_B1(end,2)],[1 1 0 0 0 0]);

% obtain the foot positon using thigh and shank positions
q_B2(:,3) = pi/2 - ( q_B2(:,1) + q_B2(:,2) );

% create the joint velocities and joint accelerations from q and t using
% jtraj

[qc_B2 qd_B2 qdd_B2] = jtraj(q_B2(1,:),q_B2(end,:),t);

% solve the inverse dynamics problem for path B2
% find tau (forces on joints i.e. motor torques)

tau_B2 = rne(r1, qc_B2, qd_B2, qdd_B2, grav);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% solve the inverse kinematics for r0 for path B3
% the initial conditions for this problem are qc_B2(end,1) , qc_B2(end,2)
q_B3 = ikine(r0,TC_B3,[qc_B2(end,1) qc_B2(end,2)],[1 1 0 0 0 0]);

% obtain the foot positon using thigh and shank positions
q_B3(:,3) = pi/2 - ( q_B3(:,1) + q_B3(:,2) );

% create the joint velocities and joint accelerations from q and t using
% jtraj

[qc_B3 qd_B3 qdd_B3] = jtraj(q_B3(1,:),q_B3(end,:),t);

% solve the inverse dynamics problem for path B3
% find tau (forces on joints i.e. motor torques)

tau_B3 = rne(r1, qc_B3, qd_B3, qdd_B3, grav);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% kinematic motion simulation 

q = [q_A; q_B1; q_B2; q_B3];
qd = [qd_A; qd_B1; qd_B2; qd_B3];
qdd = [qdd_A; qdd_B1; qdd_B2; qdd_B3];

close('all') 

pov = [1 0 5];
view(pov)

plot(r1,q)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% uncontrolled motion simulation
% apply the torques obtained in the inverse dynamics section to produce
% uncontrolled motion accelerations qdd_uc. the velocities and positions 
% i.e. qd_uc and q_uc will be obtained assuming constant acceleration in
% each time increment.

tau = [tau_A; tau_B1; tau_B2; tau_B3];

q_uc{1} = q(1,:);
qd_uc{1} = [0 0 0];
qdd_uc{1} = [0 0 0];

for i=1:1:4*length(t)-1
    
    qdd_uc_t = accel(r1, q_uc{i}, qd_uc{i}, tau(i,:));
    qdd_uc{i} = qdd_uc_t';
    qd_uc{i+1} = qd_uc{i} + qdd_uc{i}(1:end)*t_inc;
    q_uc{i+1} = q_uc{i} + qd_uc{i}(1:end)*t_inc + 1/2*qdd_uc{i}(1:end)*t_inc^2;
    
end

% obtain the matrix representation of the cell array q_uc

for i=1:4*length(t)
    
    q_uc_v(i,1:3)=q_uc{i}(1:3);
    
end

% plot the robot with uncontrolled data

pov = [1 0 5];
view(pov)

plot(r1,q_uc_v)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% implement an inverse dynamics control. this control is done through
% applying torque as follows:
%                           
%                           tau[k+1] = M(q[k])*( edd[k] + Kd*ed[k] +
%                           Kp*e[k]) + M(q[k])*(d^2(q[k])/dt^2) +
%                           C*d(q[k])/dt + g(q)
%                     
% where:
%
%                           edd[k] = (d^2(q_d[k])/dt^2 - d^2(q[k])/dt^2)
%                           ed[k] = (d(q_d[k])/dt - d(q[k])/dt)
%                           e[k] = q_d[k] - q[k]

% set proportional and derivative gains: Kp and Kd

Kp = 1;
Kd = 1;

q_idc{1} = q(1,:);
qd_idc{1} = [0 0 0];
qdd_idc{1} = [0 0 0];

for i=1:1:4*length(t)-1
    
    if i<length(t)
        tau_idc_t1 = rne(r1, q_idc{i}, qd_idc{i}, qdd_idc{i}, grav, f_ext_A);
    else
        tau_idc_t1 = rne(r1, q_idc{i}, qd_idc{i}, qdd_idc{i}, grav);
    end
    
    jointspace_inertia = inertia(r1, q_idc{i});
    
    e = q(i,:) - q_idc{i};
    ed = qd(i,:) - qd_idc{i};
    edd = qdd(i,:) - qdd_idc{i};
    
    tau_idc_t2 = tau_idc_t1' + jointspace_inertia*(Kp*e' + Kd*ed' + edd');
    
    qdd_idc_t = accel(r1, q_idc{i}, qd_idc{i}, tau_idc_t2);

    qdd_idc{i+1} = qdd_idc_t';
    qd_idc{i+1} = qd_idc{i} + qdd_idc{i}(1:end)*t_inc;
    q_idc{i+1} = q_idc{i} + qd_idc{i}(1:end)*t_inc + 1/2*qdd_idc{i+1}(1:end)*t_inc^2;
    
end

% obtain the matrix representation of the cell array q_idc

for i=1:4*length(t)
    
    q_idc_v(i,1:3)=q_idc{i}(1:3);
    
end

% plot the robot with controlled data

pov = [1 0 5];
view(pov)

plot(r1,q_idc_v)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% compute and show the heel path in the desired, uncontrolled and
% controlled cases

T = fkine(r1,q);
X = T(1,4,1:4*length(t));
X = X(:);
Y(1:4*length(t)) = T(2,4,1:4*length(t));
Y = Y(:);

T_uc = fkine(r1,q_uc_v);
X_uc = T_uc(1,4,1:4*length(t));
X_uc = X_uc(:);
Y_uc(1:4*length(t)) = T_uc(2,4,1:4*length(t));
Y_uc = Y_uc(:);

T_idc = fkine(r1,q_idc_v);
X_idc = T_idc(1,4,1:4*length(t));
X_idc = X_idc(:);
Y_idc(1:4*length(t)) = T_idc(2,4,1:4*length(t));
Y_idc = Y_idc(:);

plot(Y,-X,'k*',Y_uc,-X_uc,'b',Y_idc,-X_idc,'r') 

title('the heel desired path(balck), uncontrolled path(blue) and controlled path(red)')
xlabel('the foot heel vertical distance to the hipbone hinge (meter)')
ylabel('the foot heel horizontal distance to the hipbone hinge (meter)')

% show the foot angle with respect to the ground in the desired, uncontrolled and
% controlled cases

footangle_uc = -pi/2 + (q_uc_v(:,1) + q_uc_v(:,2) + q_uc_v(:,3));

footangle_idc = -pi/2 + (q_idc_v(:,1) + q_idc_v(:,2) + q_idc_v(:,3));

tA = [0:t_inc:t_course];
tB1 = [t_course:t_inc:2*t_course];
tB2 = [2*t_course:t_inc:3*t_course];
tB3 = [3*t_course:t_inc:4*t_course];
t_total = [tA tB1 tB2 tB3];

figure
plot(t_total,zeros(4*length(t),1),'k*',t_total,footangle_uc*180/pi,'b',t_total,footangle_idc*180/pi,'r') 

title('the foot desired angle (balck), uncontrolled angle (blue) and controlled angle (red) relative to the ground')
xlabel('time (second)')
ylabel('angle (degree)')