Hands-on Robotics Toolbox
Practical Laboratory Lecturer: Sam Wane D107 s.o.wane@staffs.ac.uk
First Robot
To control a robot, type:
puma560 ←
drivebot(p560) ←
The command ‘puma560’ defines a DH matrix for the Puma 560 robot.
Ensure you can see both figures that are open. Use the slider controls in figure 2 to move each joint
of the robot gin figure 1.
As you move the joints of the robot, look at the joint values and the x, y, z values shown in figure 2,
this program is performing a forward kinematic calculation of the position of the tool tip in metres.
Notice that the orientation of the tool tip is shown under ax, ay, az and is in radians.
Moving joints manually
Close both the figure windows. We are going to type in each robot joint angle manually. Type:
qz=[0 0 0 0 0 0]
plot(p560,qz)
This sets each of the angles of the robot to zero. The format is:
qz=[j1 j2 j3 j4 j5 j6] where j1 = joint 1 is in radians etc.
Remember, if you want to change degrees to radians you can use:
Radian=30*pi/180
Close the figure and try entering a variety of angles for the joints e.g.:
qz=[0 0.1 0 0 0.2 0]
plot(p560,qz)
Forward kinematics
The homogenous matrix for the end point position is returned using ‘fkine’.
Define a value for ‘qz’ as in the above section, and type:
T=fkine(p560,qz)
Again, enter a variety of joint angles and find the end point position. E.g.
qz=[0 0.4 0 0.2 0.1 0]
T=fkine(p560,qz)
End point position and orientation
Now, ‘T’ is a 4 x 4 homogenous matrix representing the end point orientation and translation of the
Puma 560 robot. The end point x,y,z is easy to see as it is in the forth column of this matrix. However,
the orientation is a little more tricky as it is embedded in the first three columns and rows of the matrix.
Manually, this can be calculated from:
333231
232221
131211
rrr
rrr
rrr
( )
( )
( ))Ycos(r,)Ycos(rAtan2X
)Ycos(r,)Ycos(rAtan2Z
rr,r-Atan2Y
3332
1121
21
2
11
2
31
=
=
+=
The rotation matrix is embedded within the homogenous matrix, this can be translated into RPY
values by the use of ‘tr2rpy’.
Type:
tr2rpy(T)
and this will show you the rotations about Z Y X in that order with a space between each value.
Inverse kinematics
The joint angles can be returned from the homogenous matrix using ‘ikine’. For example, to find the
joint angles of the Puma 560 to reach a position x=0.25, y=0, z=0.2 with no roll, pitch or yaw:
T=transl(0.25,0,0.2)
qz=ikine(p560,T)
Enter a variety of x,y,z positions and write down the joint angles of the robot for each point. What
happens if the robot is unable to reach the point (outside the workspace) ?
Defining your own Homogenous Matrices
Recall that to define a homogenous matrix, you would use:
Rotation about Z:
1000
0100
00
00
:
qq
q-q
q
CS
SC
i
Translation along X:
1000
0100
0010
001
:
x
x
Rotation about X:
1000
00
00
0001
:
qq
q-q
q
CS
SC
i
Translation along Y:
1000
0100
010
0001
:
y
y
Rotation about Y:
1000
00
0010
00
:
qq-
qq
q
CS
SC
i
Translation along Z:
1000
100
0010
0001
:
z
z
Where q=q cosC , and q=q sinS
For example:
We will now create a rotation matrix about the Y-axis of -45˚ and a translation matrix of 50 in the x
direction and multiply them together into a homogenous matrix ‘T’.
t=-45/180*pi;
ct=cos(t);
st=sin(t);
ry=[ct 0 st 0;0 1 0 0;-st 0 ct 0;0 0 0 1];
tx=[1 0 0 50;0 1 0 0;0 0 1 0;0 0 0 1];
T=ry*tx
Now ‘T’ is a homogenous matrix representing the rotation about the Y-axis and a translation along the
x-axis.
Rather than looking these up and defining them manually, Robotics Toolbox has two commands,
transl, and rotn.
The homogenous matrix defining x,y,z positions can be built using ‘transl’.
For example:
T=transl(0.25,0,0.2)
Will define ‘T’ as a homogenous matrix with x=0.25, y=0, and z=0.2.
A 4 x 4 rotation matrix about x,y or z can be defined by using ‘rotx’, ‘roty’ or ‘rotz’.
For example, to create a rotation matrix defining rotation about the x-axis by 0.2 radians you would
use:
T=rotx(0.2)
You can combine a translation and rotation thus:
T=transl(0.25,0,0.2)*roty(0.2)
Something to think about
Why is the above result different to:
T=roty(0.2)*transl(0.25,0,0.2) ?
Defining your own robot using DH Matrices
A simple two-link manipulator
Imagine a manipulator that, viewed from above works in a planar motion (such as a SCARA robot). All
rotations will be about the z-axis and the DH matrix will be quite simple.
Both rotations are about the z-axis, so ‘θ’ is the angle of rotation in both cases, and the link lengths go
into the ‘a’ parameter:
α a θ d
0 100 β1 0
0 50 β2 0
And entered into Robotics Toolbox:
Using the toolbox for forward kinematic calculations
Type:
qz=[0 0]
this sets both the joint angles to θ1 and θ2 to 0 radians.
Type:
T=fkine (R,qz)
This will return a 4 x 4 matrix, the top right figure represents the x position, the one below it, y, and
followed by z. In this case, x=150, y=0, and z=0.
Type:
ikine(R,T)
and both of the joint angles are returned.
Now try placing different angles for θ1 and θ2, you will have to retype T=fkine(R,qz) after redefining the
value qz.
Jacobian Matrix
The Jacobian matrix can be calculated using ‘jacob0’ as in:
qz=[0,0]
J=jacob0(R,qz)
len1=link([0,100,0,0])
len2=link([0,50,0,0])
L{1}=len1
L{2}=len2
R=robot(L)
drivebot(R)
100 50
y
x
z β1 β2
Two link manipulator with rotating base
A two link manipulator with a rotating base (such as a revolute robot) and viewed from the side (y-axis
pointing into the page) can be visualised as follows:
The first rotation is about the z-axis, and so the first ‘θ’ value is β1.
This is followed by a link length along the z-axis, hence the ‘d’ value is 20.
The next rotations appear to be about the y-axis, but this cannot happen in DH matrices, so the co-
ordinate frame has to be rotated. We will rotate it about the x-axis by 90º so that the z-axis points out
of the page towards you. A rotation about the x-axis gives us an α value of 90º. The co-ordinate frame
will now appear as:
The following rotations of are now around the z-axis, and the link lengths along the x-axis, hence the
rotations β2 and β3 and the link length values are placed into ‘θ’ and ‘a’ respectively.
α a θ d
0 0 β1 20
90 0 0 0
0 100 β2 0
0 50 β3 0
And entered into Robotics Toolbox:
z
x
y
β1
20
100 50
z
x
y
β2 β3
β1
20
len1=link([0,0,0,20])
len2=link([1.507,0,0,0])
len3=link([0,100,0,0])
len4=link([0,50,0,0])
L{1}=len1
L{2}=len2
L{3}=len3
L{4}=len4
R=robot(L)
drivebot(R)
Looking at joint trajectories
The ‘jtraj’ will return an array of joint trajectories which can be plotted against time. The parameters of
‘jtraj’ are (start joint angles, end joint angles, time).
For example, to plot the trajectory of a single joint moving from 15 to 75 degrees in a total time of 3
seconds with a sample time of 0.5 seconds:
t=[0:0.5:3]' define a time matrix, ‘t’. Consists of start time, sample time, end time. Note the
apostrophe at the end.
pos=jtraj(15,75,t) Start angle, end angle and return the position values into an array ‘pos’
plot(t,pos) plot the result
To look at the joint velocity:
[pos vel]=jtraj(15,75,t) note jtaj can return a matrix, the first column is position,
plot(t,vel) the second column is velocity
To examine multiple joints, the start and end angles can be a matrix:
>> start=[15 45 30 20 60 30] starting angles
>> stop=[75 0 120 25 30 0] end angles
And this moves the joints as:
Joint
number
Start
angle
Stop
angle
1 15 75
2 45 0
3 30 120
4 20 25
5 60 30
6 30 0
start=[15 45 30 20 60 30]
stop=[75 0 120 25 30 0]
pos=jtraj(start,stop,t) ‘pos’ is a matrix representing all 6
joint motions
subplot(6,1,1);plot(t,pos(:,1)) this is a method of having
subplot(6,1,2);plot(t,pos(:,2)) several plots on the screen
subplot(6,1,3);plot(t,pos(:,3)) at once (subplots)
subplot(6,1,4);plot(t,pos(:,4))
subplot(6,1,5);plot(t,pos(:,5))
subplot(6,1,6);plot(t,pos(:,6))
and to show multiple velocities:
[pos vel]=jtraj(start,stop,t)
subplot(6,1,1);plot(t,vel(:,1))
subplot(6,1,2);plot(t,vel(:,2))
subplot(6,1,3);plot(t,vel(:,3))
subplot(6,1,4);plot(t,vel(:,4))
subplot(6,1,5);plot(t,vel(:,5))
subplot(6,1,6);plot(t,vel(:,6))
To look at the joint acceleration:
[pos vel acc]=jtraj(15,75,t) note jtaj can return a matrix, the first column is position,
plot(t,acc) second column is velocity, the third is acceleration
Getting Robotics Toolbox and some support
To download and setup robotics toolbox:
http://www.petercorke.com
Scroll down to the “How to get it (download)” section.
Fill out the form.
Select the pkzip format and select ‘Open’.
Click the ‘Extract’ button and select: ‘C:\program files\ MATLAB\R2006a\toolbox’
This also includes the manual.
Now you can start MATLAB and close the pkzip program.
Setting up Robotics Toolbox in MATLAB:
NB: the symbol ← represents the ENTER key and must be pressed after each line.
path(path,'c:\Program Files\MATLAB\R2006a\toolbox\robot') ←
savepath ←
You will only need to do the above once.
Forum
http://groups.google.com/group/robotics-tool-box
Some Robot Toolbox Example Codes
%test roll,pitch,yaw
30*pi/180
T=roty(0.5236)
tr2rpy(T)
T=roty(0.5236)*rotx(0.2)
tr2rpy(T)
%moving joints in the qz matrix
puma560
qz=[0 0 0 0 0 0]
plot(p560,qz)
T=fkine(p560,qz)
qz=[0 0.1 0 0 0 0]
plot(p560,qz)
%defining qz from an end point (note IK only works with 6DoF arms)
T=transl(0.25,0,0.2)
qz=ikine(p560,T)
plot(p560,qz)
%creating own 3 link robot
l1=link([0 100,0,0])
l2=link([0 50,0,0])
l3=link([0 40,0,0])
L{1}=l1
L{2}=l2
L{3}=l3
R=robot(L)
drivebot(R)
%trajectory of Puma defining start angles and end angles
qz=[0 0 0 0 0 0]
qr=[0.2 0 0.4 2 1 0]
t=[0:0.01:2]'
q=jtraj(qz,qr,t)
subplot(4,1,1);plot(t,q(:,1))
subplot(4,1,2);plot(t,q(:,3))
subplot(4,1,3);plot(t,q(:,4))
subplot(4,1,4);plot(t,q(:,5))
%showing velocities of joints
[q,qd,qdd]=jtraj(qz,qr,t)
subplot(4,1,1);plot(t,qd(:,1))
subplot(4,1,2);plot(t,qd(:,3))
subplot(4,1,3);plot(t,qd(:,4))
subplot(4,1,4);plot(t,qd(:,5))