2D平面上的二連杆機器臂反向運動模拟（matlab代碼示例）

2D平面上的二連杆機器臂反向運動模拟

matlab上先下載下傳 robotics system toolbox工具箱

robot = rigidBodyTree('DataFormat','column','MaxNumBodies',3);
L1 = 1; %the length of L1
L2 = 1; %the length of L2

%Add 'link1' body with 'joint1' joint.
body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint,trvec2tform([0 0 0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');

%Add 'link2' body with 'joint2' joint.
body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint, trvec2tform([0,-L1,0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'link1');

body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([L2, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link2');

t = (0:0.1:10)'; % Time
count = length(t);
center = [1 -1 0];
radius = 0.5;
theta = t*(2*pi/t(end));
points = center + radius*[cos(theta) sin(theta) zeros(size(theta))];

q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);

ik = inverseKinematics('RigidBodyTree', robot);
weights = [0, 0, 0, 1, 1, 0];
endEffector = 'tool';

qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ik(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

%使用該特定機器人配置為解決方案的每一幀繪制機器人。此外，繪制所需的軌迹。
%在軌迹的第一個配置中顯示機器人。調整繪圖以顯示繪制圓的二維平面。繪制所需的軌迹。

figure
show(robot,qs(1,:)');
view(2)
ax = gca;
ax.Projection = 'orthographic';
hold on
plot(points(:,1),points(:,2),'k')
axis([-0.8 2 -2 4])

%設定一個rateControl對象，以每秒 15 幀的固定速率顯示機器人軌迹。從逆運動學求解器中顯示每個配置中的機器人。觀察手臂跟蹤所示的圓形軌迹。

framesPerSecond = 15;
r = rateControl(framesPerSecond);

for i = 1:count
    show(robot,qs(i,:)','PreservePlot',false);
    drawnow
    waitfor(r);
end
           

此代碼參考了matlab官網提供的教程文檔

[1]: https://www.mathworks.com/help/robotics/ug/2d-inverse-kinematics-example.html