TRC3200: Computational Lab 2

Lecturer: Dr. Madhavan Shanmugavel

Student id: 25496670
Abstract
This lab involves MatLab programming to simulate and animate the motion of a RPR robotic
arm.
Aim:
To write a GUI animating a 3-axis robot arm from a start position, P s (xs, ys, zs) to a final
position Pf (xf, yf, zf).
Problem definition:
The figure shows a RPR three-axis robot arm where R stands for Revolute joint and P for
Prismatic joint. It has three degrees of freedom. They are defined by A , B , S A . Position
of point C is defined as PC is relative to the base axis. The values given are
l 1=1.5 ml 2=0.5 m . The maximum extension of SA should be 2.5m
Derivation of forward and inverse kinematics
Forward kinematics:
1) Transformation from frame 0 to 1

cos ( A ) sin ( A )
0
sin ( A ) cos ( A )
1[ T ] =
0
0
0
0

0
0
1
0

0
0
l2
1

][

1 0 0 S A
0 1 0
0 0 1
0 0 0

S A ,max
2
0
0
1

2) Transformation from frame 0 to 2

cos ( A ) sin ( A )
0
sin ( A ) cos ( A )
2[ T ] =
0
0
0
0

][

0 0 cos ( B )
0 0
0
1 l 2 sin ( B )
0
0 1

Inverse kinematics:

cos ( A ) [ l 2 cos ( B ) + S A ]

0 {P }C = sin ( A ) [ l 2 cos ( B ) + S A ]
l 2 sin ( B ) +l 1
1
A =atan (

yc
)
xc

0 sin ( B ) S A
1
0
0
0 cos ( B ) 0
0
0
1

][ ]
1 0 0
0 1 0
0 0 1
0 0 0

l2
2
0
0
1

l 1z c
)
l2
x c l 2 cos ( B ) +cos ( A )
S A=
cos ( A )
B =asin (

Programming in MATLab
1)Plotting box
function box3d(Pts,colour)
%box3d plots a box using given points in xyz coordinate
%
This function uses the built-in matlab function fill3 to plot a box
% Pts - 3x8 matrix obtained by using parameters function
% colour - any value to select the number
tridmat = zeros(3,4,6);
tridmat(:,:,1) = [Pts(1:3,1),Pts(1:3,2),Pts(1:3,3),Pts(1:3,4)];
tridmat(:,:,2) = [Pts(1:3,5),Pts(1:3,6),Pts(1:3,7),Pts(1:3,8)];
tridmat(:,:,3) = [Pts(1:3,2),Pts(1:3,3),Pts(1:3,7),Pts(1:3,6)];
tridmat(:,:,4) = [Pts(1:3,3),Pts(1:3,4),Pts(1:3,8),Pts(1:3,7)];
tridmat(:,:,5) = [Pts(1:3,1),Pts(1:3,2),Pts(1:3,6),Pts(1:3,5)];
tridmat(:,:,6) = [Pts(1:3,1),Pts(1:3,4),Pts(1:3,8),Pts(1:3,5)];
%Plotting the box plane by plane in a single plot
fill3(tridmat(1,:,1),tridmat(2,:,1),
hold on
fill3(tridmat(1,:,2),tridmat(2,:,2),
fill3(tridmat(1,:,3),tridmat(2,:,3),
fill3(tridmat(1,:,4),tridmat(2,:,4),
fill3(tridmat(1,:,5),tridmat(2,:,5),
fill3(tridmat(1,:,6),tridmat(2,:,6),
hold off
end

tridmat(3,:,1), colour);
tridmat(3,:,2),
tridmat(3,:,3),
tridmat(3,:,4),
tridmat(3,:,5),
tridmat(3,:,6),

colour);
colour);
colour);
colour);
colour);

The figure shows an example of the plot

that has been created using the function.
The origin is at middle of the base of the
box.

2)Setting parameters
function Threebyeight=parameters(thickness, width, height);
%this function takes in parameters such as height, width and thickness and
%generates a 3x8 matrix
%
%height in z axis
%width in y axis
%thickness in x axis
%l_origin is a 1x3 matrix representing coordinate of local origin
%
%the origin of the box is determined at the center of the base
%base is the x-y plane at z=0
p1 = [-thickness/2 ; -width/2; 0];
p2 = [thickness/2 ; -width/2; 0];
p3 = [thickness/2 ; width/2; 0];
p4 = [-thickness/2 ; width/2; 0];
p5 = [-thickness/2 ; -width/2; height];
p6 = [thickness/2 ; -width/2; height];
p7 = [thickness/2 ; width/2; height];
p8 = [-thickness/2 ; width/2; height];

Threebyeight=[p1, p2, p3, p4, p5, p6, p7, p8];

end

3) Static Model

function static_model( theta_A, theta_B, S_A )

%static_model function generates a static model of the RPR robotic arm
%
Detailed explanation goes here
l1 = 1.5; %in meters
l2 = 0.5; %in meters
S_A_max = 2.5; %max value of S_A in meters
% Dim = |thickness1, thickness2, thickness3|
%
|width1
, width2
, width3
|
%
|height1
, height2
, height3
|
Dim = [0.1, S_A_max, l2; ...
0.1, 0.1, 0.1;...
l1, 0.1, 0.1]; %Dimensions
skeleton1=parameters(Dim(1,1), Dim(2,1), Dim(3,1));
skeleton2=parameters(Dim(1,2),Dim(2,2), Dim(3,2));
skeleton3=parameters(Dim(1,3), Dim(2,3), Dim(3,3));
skeletons = zeros(4,8,3);
cpy=ones(1,8);
skeletons(:,:,1) = [skeleton1;cpy];
skeletons(:,:,2) = [skeleton2;cpy];
skeletons(:,:,3) = [skeleton3;cpy];

%rotational kinematics by theta_A without 4th column

rot_A = [cos(theta_A), -sin(theta_A), 0;...
sin(theta_A), cos(theta_A), 0; ...
0, 0, 1; 0, 0, 0];
%rotational kinematics by theta_B and translation of S_A along x-axis
rot_B = [cos(theta_B), 0, sin(theta_B), S_A; 0, 1, 0, 0; ...
-sin(theta_B), 0, cos(theta_B), 0; 0, 0, 0, 1];
transform = zeros(4,4,3);
transform(:,:,1) = [rot_A,[0;0;0;1]]; %transform for box 1
transform(:,:,2) = [rot_A,[0;0;l1;1]]* ...
[1,0,0,S_A-S_A_max/2;0,1,0,0;0,0,1,0;0,0,0,1]; %transform for box 2
transform(:,:,3) = [rot_A,[0;0;l1;1]]* rot_B*...
[1,0,0,l2/2;0,1,0,0;0,0,1,0;0,0,0,1];
%finding the final coordinates
skeletons(:,:,1)= transform(:,:,1)*skeletons(:,:,1);
skeletons(:,:,2)= transform(:,:,2)*skeletons(:,:,2);
skeletons(:,:,3)= transform(:,:,3)*skeletons(:,:,3);
%storing the coordinates inside final as 3x8 matrix
final=zeros(3,8,3);
final(:,:,1)=skeletons(1:3,1:8,1);
final(:,:,2)=skeletons(1:3,1:8,2);
final(:,:,3)=skeletons(1:3,1:8,3);
box3d(final(:,:,1),0); %box 1
axis([-3 3 -3 3 0 3]);

xlabel('x');
ylabel('y');
zlabel('z');
grid
hold on
box3d(final(:,:,2),1); %box 2
hold on
box3d(final(:,:,3),2); %box 3
end

4) Animating the arm

Basically, the static model function is used to draw the plot on top of another using a for loop
function animate_arm( Ps, Pf, iteration,speed)
%UNTITLED3 Summary of this function goes here
%
Detailed explanation goes here
data=zeros(iteration,3);
data = [linspace(Ps(1,1),Pf(1,1),iteration)' , ...
linspace(Ps(1,2),Pf(1,2),iteration)' , ...
linspace(Ps(1,3),Pf(1,3),iteration)'];
time=1/speed;
for i=1:iteration
hold off
static_model(data(i,1), data(i,2), data(i,3));
pause(time)
end
end

5) GUI for Simulation

A simple GUI to get the input from users and create the desired output to see if the values are
valid.
function varargout = gui(varargin)
% GUI MATLAB code for gui.fig
%
GUI, by itself, creates a new GUI or raises the existing
%
singleton*.
%
%
H = GUI returns the handle to a new GUI or the handle to
%
the existing singleton*.
%
%
GUI('CALLBACK',hObject,eventData,handles,...) calls the local
%
function named CALLBACK in GUI.M with the given input arguments.
%
%
GUI('Property','Value',...) creates a new GUI or raises the
%
existing singleton*. Starting from the left, property value pairs are
%
applied to the GUI before gui_OpeningFcn gets called. An
%
unrecognized property name or invalid value makes property application
%
stop. All inputs are passed to gui_OpeningFcn via varargin.

%
%
*See GUI Options on GUIDE's Tools menu.
%
instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

Choose "GUI allows only one

% Edit the above text to modify the response to help gui

% Last Modified by GUIDE v2.5 15-Sep-2016 16:17:36
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',
mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @gui_OpeningFcn, ...
'gui_OutputFcn', @gui_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback',
[]);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before gui is made visible.

function gui_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject
handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% varargin
command line arguments to gui (see VARARGIN)
% Choose default command line output for gui
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes gui wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = gui_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject
handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure

varargout{1} = handles.output;

function thetaA_Callback(hObject, eventdata, handles)

% hObject
handle to thetaA (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of thetaA as text
%
str2double(get(hObject,'String')) returns contents of thetaA as a double
handles.thetaA = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function thetaA_CreateFcn(hObject, eventdata, handles)
% hObject
handle to thetaA (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function thetaA2_Callback(hObject, eventdata, handles)

% hObject
handle to thetaA2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of thetaA2 as text
%
str2double(get(hObject,'String')) returns contents of thetaA2 as a double
handles.thetaA2 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function thetaA2_CreateFcn(hObject, eventdata, handles)
% hObject
handle to thetaA2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function thetaB_Callback(hObject, eventdata, handles)

% hObject
handle to thetaB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of thetaB as text
%
str2double(get(hObject,'String')) returns contents of thetaB as a double
handles.thetaB = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function thetaB_CreateFcn(hObject, eventdata, handles)
% hObject
handle to thetaB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function thetaB2_Callback(hObject, eventdata, handles)

% hObject
handle to thetaB2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of thetaB2 as text
%
str2double(get(hObject,'String')) returns contents of thetaB2 as a double
handles.thetaB2 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function thetaB2_CreateFcn(hObject, eventdata, handles)
% hObject
handle to thetaB2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function Sa_Callback(hObject, eventdata, handles)

% hObject
handle to Sa (see GCBO)

% eventdata
% handles

reserved - to be defined in a future version of MATLAB

structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of Sa as text

%
str2double(get(hObject,'String')) returns contents of Sa as a double
handles.Sa = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function Sa_CreateFcn(hObject, eventdata, handles)
% hObject
handle to Sa (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function Sa2_Callback(hObject, eventdata, handles)

% hObject
handle to Sa2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of Sa2 as text
%
str2double(get(hObject,'String')) returns contents of Sa2 as a double
handles.Sa2 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function Sa2_CreateFcn(hObject, eventdata, handles)
% hObject
handle to Sa2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in Animate.

function Animate_Callback(hObject, eventdata, handles)
% hObject
handle to Animate (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
if get(handles.fwd,'Value')
handles.Ps=[handles.thetaA,handles.thetaB,handles.Sa];

handles.Pf=[handles.thetaA2,handles.thetaB2,handles.Sa2];
animate_arm(handles.Ps, handles.Pf, 10, 1);
elseif get(handles.inverse,'Value')
handles.Pc1=[handles.x1,handles.y1,handles.z1];
handles.Pc2=[handles.x2,handles.y2,handles.z2];
handles.Pcf1=inverse_kinematics(handles.Pc1);
handles.Pcf2=inverse_kinematics(handles.Pc2);
animate_arm(handles.Pcf1,handles.Pcf2,10,1);
end
% --- Executes on button press in fwd.
function fwd_Callback(hObject, eventdata, handles)
% hObject
handle to fwd (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of fwd

% --- Executes on button press in inverse.

function inverse_Callback(hObject, eventdata, handles)
% hObject
handle to inverse (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of inverse

function x1_Callback(hObject, eventdata, handles)

% hObject
handle to x1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of x1 as text
%
str2double(get(hObject,'String')) returns contents of x1 as a double
handles.x1 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function x1_CreateFcn(hObject, eventdata, handles)
% hObject
handle to x1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');

end

function x2_Callback(hObject, eventdata, handles)

% hObject
handle to x2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of x2 as text
%
str2double(get(hObject,'String')) returns contents of x2 as a double
handles.x2 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function x2_CreateFcn(hObject, eventdata, handles)
% hObject
handle to x2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function y1_Callback(hObject, eventdata, handles)

% hObject
handle to y1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of y1 as text
%
str2double(get(hObject,'String')) returns contents of y1 as a double
handles.y1 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function y1_CreateFcn(hObject, eventdata, handles)
% hObject
handle to y1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function y2_Callback(hObject, eventdata, handles)

% hObject
handle to y2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of y2 as text
%
str2double(get(hObject,'String')) returns contents of y2 as a double
handles.y2 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function y2_CreateFcn(hObject, eventdata, handles)
% hObject
handle to y2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function z1_Callback(hObject, eventdata, handles)

% hObject
handle to z1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of z1 as text
%
str2double(get(hObject,'String')) returns contents of z1 as a double
handles.z1 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function z1_CreateFcn(hObject, eventdata, handles)
% hObject
handle to z1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function z2_Callback(hObject, eventdata, handles)

% hObject
handle to z2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of z2 as text

%
str2double(get(hObject,'String')) returns contents of z2 as a double
handles.z2 = str2double(get(hObject,'String'));
guidata(hObject, handles);
% --- Executes during object creation, after setting all properties.
function z2_CreateFcn(hObject, eventdata, handles)
% hObject
handle to z2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles
empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

Published with MATLAB R2015b

Conclusion
The simulation helps in understanding the motion of RPR arm and the ranges of motion of
the end point, C. This program is really helpful in knowing whether the parameters set is
valid or not.
References
[1] "3D Polygon - draw a box with Matlab", Matrixlab-examples.com, 2016. [Online].
Available: http://www.matrixlab-examples.com/3d-polygon.html. [Accessed: 15- Sep- 2016].
[2] MatLab Documentation. http://www.mathworks.com/index.html?s_tid=gn_logo