Make writing a habit together! This is the 9th day of my participation in the “Gold Digging Day New Plan · April More text Challenge”. Click here for more details.

I’ll put the 0 out front

Planar 2 r robot in this paper, based on past blogs (rod) of kinematics and dynamics modeling + attach a kinematics simulation model, namely, to control the trajectory of robot arm end perform specific, in fact, in the manipulator workspace can draw arbitrary graphics, with love here as an example, the effect diagram as shown below, to see the end of this article believe that you can do it!

1 Generate love track

The arm length of the two-link manipulator is defined as 1, so the working space should be in a circle of radius 2.

x_ = -1.15:0.01:1.15;
y1 = real(1/2*(x_.^2.^ (1/3)+(x_.^4.^ (1/3) -4*x_.^2+4.) ^ (1/2)));
y2 = real(1/2*(x_.^2.^ (1/3)-(x_.^4.^ (1/3) -4*x_.^2+4.) ^ (1/2)));
x = [x_,fliplr(x_)];
y = [y1,y2];
plot(x,y);
saveddata.x = x;
saveddata.y = y;
Copy the code

The effect is as follows:

Save it as a.mat file for reuse:

save a2 saveddata
Copy the code

2. Inverse kinematics of manipulator

According to: Kinematics and dynamics modeling of planar 2R robot (two-link) + attached simulation model, the inverse kinematics solution of planar 2R robot is:

The forward kinematics is

The following encapsulates a function that implements the above model:

%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
% @file : IKrob.m
% @function : [theta] = IKrob(coord,l)
%Brief: Inverse kinematics solution function of two-axis manipulator
%Version: 1.0
%Input: coord ------------- Cartesian space coordinates
%L ------------- connecting rod length
%Output: Theta ------------- joint Angle of the manipulator
%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
function [theta] = IKrob(coord,l)

x = coord(:,1);
y = coord(:,2);
L1 = l(1);
pts = size(x);

fai = abs(acos(sqrt(x.^2+y.^2)/(2*L1)));
beta = abs(atan(y./x));
theta2 = acos((x.^2+y.^2)/(2*L1^2)-1);
theta1 = zeros(pts(1), 1);

for k=1:pts(1)
    if(x(k,1) >= 0 && y(k,1) >= 0)
        theta1(k,1) = pi/2 - (beta(k,1) + fai(k,1));    
    elseif(x(k,1) < 0 && y(k,1) >= 0)
        theta1(k,1) = - pi/2 + (beta(k,1) - fai(k,1));
    elseif(x(k,1) < 0 && y(k,1) < 0)
        theta1(k,1) = -pi/2 - (beta(k,1) - fai(k,1));
    elseif(x(k,1) >= 0 && y(k,1) < 0)
        theta1(k,1) = pi/2 + (beta(k,1) - fai(k,1));
    end
end
theta = [theta1 theta2];
end
Copy the code

After the inverse kinematics is realized, only the pose parameters of the current manipulator need to be reversed and recorded according to the trajectory points.

3 Realize the manipulator to draw the specified trajectory

3.1 Reading Data

%Read trajectory informationLoad A2. mat % SavedData contains coordinate data x,y, etc. Trajactory_length = size(savedData.x,2); Trajcoord = [savedData.x ', savedData.y ']; Trajcoord (:,1) = trajcoord(:,1) -1; % Change the position of the track to facilitate the movement of the mechanical arm DT = 0.02;%% Inverse kinematics of manipulator to find joint space trajectory (solve thetaA(including theTA1, theta2)), and make manipulator motion diagram (require the position of joint 1) <---------thetaA = zeros(trajactory_length,2); % initialize theta Angle midtrajA = zeros(trajactory_length,2); % initializes the position of joint 1Copy the code

3.2 Drawing the manipulator

figure Robotarm = VideoWriter('Robotarm.avi'); % Create a new file called robotarm.avi, open(Robotarm); % Open the Robotarm.avi file Axis ([-2 1.3-1.8 1.5]) % Hold on plot(trajcoord(:,1), Trajcoord (:,2),'r-',' lineWidth ',2); H1 = line([0 midtrajA(1,1)],[0 midtrajA(1,2)],'LineWidth',3); H2 = line([midtrajA(1,1) trajcoord(1,1)],[midtrajA(1,2) trajcoord(1,2)],'LineWidth',3); H3 = plot(midtrajA(1,1),midtrajA(1,2),'bo','LineWidth',6); % draw joint 1 M=moviein(trajactory_length); % is preceded by plot to help initialize the movieinCopy the code

3.3 Inverse pose

%Computational inverse kinematicstheta = IKrob(trajcoord, l); % to work out the corresponding joint angles < -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- midtrajA = [l (1) * sin (theta (:, 1)), l (1) * cos (theta (:, 1))]; for k=1:trajactory_length delete(h1); delete(h2); delete(h3); The axis ([2.5 1.3 1.8 1.5]) h1 = line ([0 midtrajA (k, 1)], [0 midtrajA (k, 2)], 'our LineWidth, 3); H2 = line([midtrajA(k,1) trajcoord(k,1)],[midtrajA(k,2) trajcoord(k,2)],'LineWidth',3); H3 = plot(midtrajA(k,1),midtrajA(k,2),'bo','LineWidth',6); % draw joint 1 M(:,k)= getFrame; WriteVideo (Robotarm,M(:,k)); end%Movie (30 M, 1); % play once at 30 frames per secondclose(Robotarm); % offCopy the code

The results are saved in ‘robotarm.avi’ and played directly.

4 develop

This project aims to practice the kinematic model of mechanical arm. Simple writing applications can be realized by changing graphics into letters, such as:

Complete code please private letter ~~

Welcome to my AI channel “AI Technology Club”.