FORWARD KINEMATICS OF A 2R ROBOTIC ARM SIMULATION USING MATLAB

OBJECTIVE:

To simulate the forward kinematics of a 2R Robotic Arm using MATLAB

INTRODUCTION:

Robot kinematics is the study of the motion of robot manipulator where the position, velocity and acceleration of all the links are calculated without considering the forces that cause this motion. A manipulator is composed of several links which are affixed to each other either by revolute or prismatic joints from the base frame through the end-effector. End-effector is the device which is attached to the manipulator that interacts with its environment to perform certain tasks . Forward kinematics is the method for determining the orientation and position of the end effector, given the joint angles and link lengths of the robot arm.

A 2R robotic arm consists of a base, two links which rotate in 2D palne and two joints where one joint is between base and link 1 and the other is between link 1 and link 2. The schematic of 2R robotic arm is given in the following figure

The lengths of links 1 and 2 are denoted by L1, L2 respectively and the angles of inclination of links 1 and 2 with respect to horizontal are denoted by `theta`1 and `theta`2 respectively. 

Start point and end point of link 1 are considered as (X0, Y0) and (X1, Y1)

Start point and end point of link 2 are considered as (X1, Y1)and (X2, Y2)

INPUT DATA:

L1 = Length of link 1 = 1 m

L2 = Length of link 2 = 0.5 m 

`theta`1 = Angle of inclination of link 1 varies from 0 to 90 degrees 

`theta`2 = Angle of inclination of link 2 varies from 0 to 90 degrees

Base and link 1 joined at origin

X0 = 0

Y0 = 0

CALCULATION FORMULAE:

Coordinates of end point of link 1 and start point of link 2 (X1, Y1) are calculated using L1 and `theta`1

X1 = L1 * cos(`theta`1)

Y1 = L1 * sin(`theta`1)

Coordinates of end point of link 1 and start point of link 2 (X2, Y2) are calculated using X1, Y1, L2 and `theta`2

X2 = X1 + L2 * cos(`theta`2)

Y2 = Y1 + L2 * sin(`theta`2)

CODE:

% FORWARD KINEMAATICS OF A 2R ROBOTIC ARM MANIPULATOR
clear all
close all
clc

% inputs
% Lengths of link 1 and link 2
l1 = 1;
l2 = 0.5;

% Both link 1 and link 2 rotate from 0 to 90 degrees and the range divided in to
% certain number of divisions to get animated plot
theta1 = linspace(0,90,30);
theta2 = linspace(0,90,60);

% Adding a variable to indicate the number of frame in the loop
count = 1;

% First 'for' loop is to calculate link lengths at each angle(theta1) of link 1
for i = 1:length(theta1)
    % Second 'for' loop is to calculate link lengths at each angle theta2 of link 2
    % wrt to particular link1 angle(theta1(i))
    for j = 1:length(theta2)
        % Starting point coordinates of link 1
        x0 = 0;
        y0 = 0;
        
        % Calculating the end coordintes of link 1 at particular angle theta1(i)
        x1 = l1*cosd(theta1(i));
        y1 = l1*sind(theta1(i));
        
        % Calculating the end coordintes of link 2 at particular angle theta2(i)
        % wrt to starting point
        x2 = x1 + l2*cosd(theta2(j));
        y2 = y1 + l2*sind(theta2(j));

        % Plotting
        plot([x0 x1],[y0 y1],[x1 x2],[y1 y2],'linewidth',3)
        axis([-0.1 1.6 -0.1 1.6])
        pause(0.01)
        
        % Storing the current frame into array M using getframe command
        % gcf returns current figure handle
        M(count) = getframe(gcf);
        count = count+1;
    end
end

% plays the movie frames in array M once & to play 'n' times use movie(M,n)
% movie(M)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%% CREATING VIDEO FILE %%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Using VideoWriter object to create a video file from an array M
% Command for VideoWriter
% v = VideoWriter(filename,profile) or VideoWriter(filename)

    % Different profiles for VideoWriter
    % 'Archival' - Motion JPEG 2000 file with lossless compression
    % 'Motion JPEG AVI' - AVI file using Motion JPEG encoding
    % 'Motion JPEG 2000' - Motion JPEG 2000 file
    % 'MPEG-4' - MPEG-4 file with H.264 encoding
    % 'Uncompressed AVI' - Uncompressed AVI file with RGB24 video
    % 'Indexed AVI' - Uncompressed AVI file with indexed video
    % 'Grayscale AVI' - Uncompressed AVI file with grayscale video
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

v = VideoWriter('forward_kinematics.avi','Uncompressed AVI');
open(v) % Open the video file 'v'
writeVideo(v,M) % Writes data from an array M to the video file 'v'
close(v) % Close the video file 'v'

CODE EXPLANATION:

1. Input variables like lengths of links are declared along with additional variable 'count' whose value is set to 1 ('count' is used to indicate number of frame in the  'for' loop).

2. In order to simulate the movement of links, the angle of inclination for both links is divided into certain 'N' number of divisions using linspace command.

    theta1 = linspace(0,90,N);
    theta2 = linspace(0,90,N);

3. For each angle of inclination(`theta`1) of link 1, link 2 will move from 0 to 90 degrees(`theta`2). Hence two loops are required. One loop is for every angle of theta1 and another loop, which is used inside the first loop, is for every angle of theta2.

4. Now under for loop, end coordinates of link 1 (X1, Y1) and link 2 (X2, Y2) are calculated. In order to get animated movie,

first plot command is used to plot the position of links and that plot frame is stored in to array M using getframe command

    M(count) = getframe(gcf);

gcf returns the current frame handle and count varible is increased by one value

5. Both for loops are ended and syntax for creating a movie file using VideoWriter command is used. 

Syntax for creating a movie using VideoWriter from an array M:

Command for VideoWriter

v = VideoWriter(filename,profile) or VideoWriter(filename)

open(v)                   % Open the video file 'v'
writeVideo(v,M)       % Writes data from an array M to the video file 'v'
close(v)                  % Close the video file 'v'

Different profiles for VideoWriter

'Archival'                   - Motion JPEG 2000 file with lossless compression
'Motion JPEG AVI'       - AVI file using Motion JPEG encoding
'Motion JPEG 2000'     - Motion JPEG 2000 file
'MPEG-4'                    - MPEG-4 file with H.264 encoding
'Uncompressed AVI'    - Uncompressed AVI file with RGB24 video
'Indexed AVI'              - Uncompressed AVI file with indexed video
'Grayscale AVI'           - Uncompressed AVI file with grayscale video

OUTPUT:

The final output animation can be seen in the following link

 https://youtu.be/5jZKVhfgwDg

ERRORS:

ERROR 1: Cannot find exact (case sensitive) match for 'Videowriter'

Step to rectify: It is case sensitive function, so changed 'Videowriter' to 'VideoWriter'

ERROR 2: Unrecognised function or variable 'writevedio'

Step to rectify: It is case sensitive function, so changed 'writevideo' to 'writeVideo'

ERROR 3: Used first loop variable 'i' instead of second loop variable 'j' while calculating end coordinates of link 2 (X2, Y2)

The output animation incase of this error is given in the following link

 https://youtu.be/aqYiLxjd1_M