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
%% 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)}));


%% Movie
figure('units','normalized','outerposition',[0 0 1 1])
v = VideoWriter('SingleLegFoot.avi');
hold on
grid on
for i = 1:length(Theta1)
    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');
    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);
    h2 = plot(J1(1,i),J1(2,i),'o','MarkerSize',7,'MarkerEdgeColor','black','MarkerFaceColor',[0.7529 0.7529 0.7529]);
    h3 = plot(J2(1,i),J2(2,i),'o','MarkerSize',7,'MarkerEdgeColor','black','MarkerFaceColor',[0.7529 0.7529 0.7529]);
    h4 = plot(J3(1,i),J3(2,i),'blackv','MarkerSize',10);

    hold off
    frame = getframe(gcf);
%% Denavit - Hartenberg Transformation Matrix

function dh = DH(link,a_i,d_i,theta_i)
    dh = [cos(theta_i) -sin(theta_i)*cos(a_i)   sin(theta_i)*sin(a_i)     link*cos(theta_i);
          sin(theta_i)  cos(theta_i)*cos(a_i)   -cos(theta_i)*sin(a_i)     link*sin(theta_i);
               0         sin(a_i)                   cos(a_i)                        d_i;
               0             0                          0                           1       ];

I also attach my notes, hopefully someone will understand and maybe will help me, I'm quite desperate. Of course the same matlab script I used for the four bar linkage, does not work for the slider crank. Thank you! enter image description here enter image description here


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.