# Closed Kinematic Chain Forward Kinematics using DH method

I'm dealing with the direct kinematic of a robotic leg, composed by a four bar mechanism coupled with a slider-crank. I understood that I need to identify the closed loops of my leg and I did it: the first one is the four bar linkage and the second one is the slider-crank.Now, I would like to obtain a MATLAB simulation of the motion of my leg and for this reason I'm writing a script that you can find below. It is related to the four bar mechanism since I'm struggling to write it for the slider-crank. And I also find hard to merge the two parts since the leg is composed by their coupling.

    clear all
close all
clc
%% Input Parameter (Change k in order to vary end-effector trajectory)
% Conversion factor from centimetres to metres
conv = 0.01;
% Incremental step
h = 0.1;
% Lenght of crank (link L1) in [m]
l = 5*conv;
k = 2;
% Length of links of mechanism
L0 = 5*l;
L1 = 5*l;
L2 = 5*l;
L3 = 5*l;

% Actuator - Costant angular velocity
Theta1 = [0:h:2*pi];

%% Denavit Hartenberg - Forward Kinematic
syms theta1  theta2 theta3
% For example T30_1s means transformation matrix from Joint 3 to Joint 0
% for the 1 closed loop (p), if second branch is considered (s)
%% First closed chain

T10 =  DH(L1,0,0,theta1);
% Transformation matrix Joint 2 to Joint 1
T21 = DH(L2,0,0,theta2);
% Joint 2
T20 = T10*T21;
% Transformation matrix Joint 3 to Joint 2
T32 = DH(L3,0,0,theta3);
% Joint 3 (first branch)
T30_1p = T10*T21*T32;
% Transformation matrix Joint 3 to Joint 0
T30_1s = DH(L0,0,0,3/2*pi);

%% Closure equation for first closed loop

eq1 = T30_1p(1:3,end) == T30_1s(1:3,end);

[Theta2,Theta3] = solve(eq1 ,[theta2 theta3]);

% Subs solutions of first closure equation for first leg
angle2 = double(subs(Theta2(1),theta1,Theta1));
angle3 = double(subs(Theta3(1),theta1,Theta1));

%%

J0 = [0 0 0]';
Joint1 = T10(1:3,end);
Joint2 = T20(1:3,end);
Joint3 = T30_1p(1:3,end);

% Compute joint trajectory for the  leg

for i = 1:length(Theta1)

J1(:,i) = double(subs(Joint1,{theta1 theta2(1) theta3(1)},{Theta1(i) angle2(i) angle3(i)}));
J2(:,i) = double(subs(Joint2,{theta1 theta2(1) theta3(1)},{Theta1(i) angle2(i) angle3(i)}));
J3(:,i) = double(subs(Joint3,{theta1 theta2(1) theta3(1)},{Theta1(i) angle2(i) angle3(i)}));

end

%% Movie
figure('units','normalized','outerposition',[0 0 1 1])
view(2)
set(gca,'nextplot','replacechildren');
v = VideoWriter('SingleLegFoot.avi');
open(v);
hold on
grid on
for i = 1:length(Theta1)
clf
hold on
grid on
hold on
grid on
axis equal
xlim([-0.5 1.2]);ylim([-1.6 0.5]);xlabel('X [m]');ylabel('Y [m]')
title('1 DOF Leg mechanism');
set(gca,'FontSize',14)
h12 = plot([J0(1,1),J1(1,i)],[J0(2,1),J1(2,i)],'r','LineWidth',2);
h13 = plot([J1(1,i),J2(1,i)],[J1(2,i),J2(2,i)],'r','LineWidth',2);
h14 = plot([J2(1,i),J3(1,i)],[J2(2,i),J3(2,i)],'r','LineWidth',2);
hold on

h1 = plot(J0(1,:),J0(2,:),'blackv','MarkerSize',10);
text(J0(1,1)+0.02,J0(2,1),'0');
h2 = plot(J1(1,i),J1(2,i),'o','MarkerSize',7,'MarkerEdgeColor','black','MarkerFaceColor',[0.7529 0.7529 0.7529]);
text(J1(1,i)-0.01,J1(2,i)+0.03,'1');
h3 = plot(J2(1,i),J2(2,i),'o','MarkerSize',7,'MarkerEdgeColor','black','MarkerFaceColor',[0.7529 0.7529 0.7529]);
text(J2(1,i)+0.02,J2(2,i),'2');
h4 = plot(J3(1,i),J3(2,i),'blackv','MarkerSize',10);
text(J3(1,1)+0.02,J3(2,1),'3');

hold off
frame = getframe(gcf);
writeVideo(v,frame);
end
close(v);
%% Denavit - Hartenberg Transformation Matrix