0
$\begingroup$

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

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       ];
end

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

$\endgroup$

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.