[DIY Robot][Matlab Robotics Toolbox][manipulator simulation][1] – Robot Kinematics

Reference for Development Environment Configuration – Manipulator simulation [matlab][1] – Robotics Toolbox (petercorke) 이용해서 one link manipulator PID 제어

1. Example for Forward Kinematics for Various Robots based on DH parameters

L(1) = Link([0 0.3 0.5 pi/8 ] );
L(2) = Link([0 0.3 0.5 pi/8 ] );
L(3) = Link([0 0.3 0.5 pi/8 ] );
L(4) = Link([0 0.3 0.5 pi/8 ] );
L(5) = Link([0 0.3 0.5 pi/8 ] );
%Serial Link를 가진 로봇 생성
Arm2D = SerialLink(L, 'name', '2DOF serial1 ');
%Base HT 입력, Tool HT는 자동으로 Identity 행렬임
Arm2D.base = [0 0 1 0; 1 0 0 0; 0 1 0 0; 0 0 0 1];
%각도궤적 생성
q = zeros(100,5);
for n=1:100
    t= n/10;
    q(n,1) = pi/4 * sin (2*pi*(t/3));
    q(n,2) = -pi/4 * sin (2*pi*(t/3)+pi/8);
    q(n,3) = -pi/4 * sin (2*pi*(t/3)+pi/8*2);
    q(n,4) = -pi/4 * sin (2*pi*(t/3)+pi/8*4);
    q(n,5) = -pi/4 * sin(2*pi*(t/3)+pi/8*4);
end

Arm2D.plot(q);

1.1. Forward Kinematics

ex1

clc; clear;

% link별 DH 파라미터 입력
L(1) = Link([0 0.5 0 pi/2 0]); % 5번째 파라미터 0 추가 (revolute joint)
% theta, d , a, alpha, , prismatic_onoff
% theta=0 일 때 변수
L(2) = Link([0 0 0 0 1]); % prismatic joint
L(2).offset = 0.5; % prismatic joint에도 offset을 설정 가능

% Serial Link를 가진 로봇 생성
Arm2D = SerialLink(L, 'name', '2DOF serial2');

% 특정 각도에 대한 로봇 자세 출력
W = [-1 1 -1 1 -1 2];
Arm2D.plot([0 0], 'workspace', W); % Prismatic joint가 있을 경우 workspace를 수동 입력해야 한다.

% 도궤적에 대한 로봇 자세 출력
q = zeros(100,2);
for n = 1:100
    t = n/10;
    q(n,1) = pi/4 * sin(2*pi*(t/3));
    q(n,2) = max(0.1, -0.5 * sin(2*pi*(t/3)) + 0.5); % prismatic joint 최소값 제한
end

Arm2D.plot(q, 'workspace', W);

ex2

clear all;

% link별 DH 파라미터 입력
L(1) = Link([0   0.5   0   -pi/2]);      % 5번째 값에 1을 넣으면 prismatic joint
L(2) = Link([0   0     0   0  1]);             % prismatic joint에도 offset을 설정 가능
L(2).offset = 0.5;
L(3) = Link([0   1     0   0]); 
% L(3).offset = -pi/2;

% Serial Link를 가진 로봇 생성
Arm2D = SerialLink(L, 'name', '2DOF serial2');

% 특정 각도에 대한 로봇 자세 출력
W = [-3 3 -1 3 -3 3];
Arm2D.plot([0 0.5 0], 'workspace', W); % Prismatic joint가 있을 경우 workspace를 수동으로 입력해야 한다.

% 각도궤적에 대한 로봇 자세 출력
q = zeros(100,3); 
for n = 1:100
    t = n/10;
    q(n,1) = pi/4 * sin(2*pi*(t/3));
    q(n,2) = -0.5 * sin(2*pi*(t/3)) + 0.5; % prismatic joint는 0보다 커야 함
    q(n,3) = pi/4 * sin(2*pi*(t/3));
end

Arm2D.plot(q, 'workspace', W, 'raise');

ex4

clear all;

% link별 DH 파라미터 입력 셋타 디 에이 알파
L(1) = Link([0   0     0    -pi/2]); L(1).offset = -pi/2;
L(2) = Link([0   1      0.5   0]); L(2).offset = -pi/2;
L(3) = Link([0   0.2   0.2       0 ]); L(3).offset = 0;

% Serial Link를 가진 로봇 생성
Arm2D = SerialLink(L, 'name', '2DOF serial1');

% Base HT 입력, Tool HT는 자동으로 Identity 행렬임
Arm2D.base = [0 1 0 0; -1 0 0 0; 0 0 1 0; 0 0 0 1];

% 특정 자세에 대한 로봇자세 출력
Arm2D.plot([0 0 0]);

ex5

L(1) = Link([0   0.5   0    -pi/2 ]);
L(2) = Link([0   0     0.5  0     ]);
L(2).offset = -pi/2; % 링크별로 각도 offset 넣을 수 있다.
L(3) = Link([0   0     0    pi/2  ]);
L(3).offset = pi/2;  % 링크별로 각도 offset 넣을 수 있다.
L(4) = Link([0   0.5   0   -pi/2 ]);
L(5) = Link([0   0     0    pi/2  ]);
L(6) = Link([0   0     0    0     ]);

iiwa6 = SerialLink(L, 'name', 'iiwa');
iiwa6.tool = [0 1 0 0; 0 0 1 0; 1 0 0 0.5; 0 0 0 1];
iiwa6.plot([0 0 0 0 0 0]);

% 각도 궤적 생성
q = zeros(20,6);
for n=1:20
    t= n/10;
    q(n,1) = 0;% pi/2 * sin(2*pi*(t/2));
    q(n,2) = -pi/4 * sin(2*pi*(t/2));
    q(n,3) = pi/2 * sin(2*pi*(t/2));
    q(n,4) = 0;% -pi/4 * sin(2*pi*(t/2));
    q(n,5) = pi/4 * sin(2*pi*(t/2));
    q(n,6) = -pi * sin(2*pi*(t/2));
end

% 로봇 모션 출력
iiwa6.plot(q, 'raise', 'loop', 'delay', 0.1, 'trail',{'r','-','LineWidth',2}); % 0.1초마다 출력

2. Path Planing Tutorial

clear;
t = 0:0.1:10;
T = 5;
% 직선 테스트 모션
q1 = zeros(size(t,2), 1);
q2 = pi*sin(2*pi*t'/T) + pi/2 ;
q3 = -pi*2*sin(2*pi*t'/T);
global Arm3D;
Robot3DMotion( [q1 q2 q3 ]);

1. P2P

t = 0:0.1:10;
T = 2.5;
% P2P 테스트 모션
x(1,:) = [0.5 0 0.5];
x(2,:) = [0 0.9 0.9];
q(1,:) = Robot3DInvKin([x(1,:),1,1]);
q(2,:) = Robot3DInvKin([x(2,:),1,1]);
qtrj = jtraj(q(1,:),q(2,:), 10) ;
%qtrj = [ qtrj; jtraj(q(2,:),q(1,:), 10) ];
Robot3DMotion( qtrj );

2. Continuous path control

t = 0:0.1:10;
T = 2.5;
% Cont. Path 테스트 모션
x(1,:) = [0.5 0 0.5];
x(2,:) = [0 0.9 0.9];
xtrj = jtraj(x(1,:),x(2,:), 10) ;
for n=1:size(xtrj,1)
    q(n,:) = Robot3DInvKin([xtrj(n,:),1,1]);
    %q(2*size(xtrj,1)+1-n,:) = q(n,:);
end
Robot3DMotion( q );