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:

r11 r21 r31

r12 r22 r32

r13 r23 r33

Z = Atan2(r21 cos(Y) , r11 cos(Y) ) X = Atan2(r32 cos(Y) , r33 cos(Y) )

Y = Atan2 - r31 , r 2 11 + r 2 21

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: Cq - Sq 0 0 Sq qi : 0 0 Cq 0 0 0 0 1 0 0 1 Translation along X:

1 0 x : 0 0

0 1 0 0

0 0 1 0

x 0 0 1

Rotation about X: 1 0 0 0 0 Cq - Sq 0 qi : 0 Sq Cq 0 0 0 0 1

Translation along Y:

y :

1 0 0 0 0 1 0 y 0 0 1 0 0 0 0 1 1 0 0 0 0 1 0 0 0 0 1 z 0 0 0 1

Rotation about Y: Cq 0 Sq 0 1 0 qi : - Sq 0 Cq 0 0 0

Translation along Z: 0 0 0 1

z :

Where Cq = cos q , and Sq = sin q

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. y

x z #1
100

#2
50

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: " 0 0 a 100 50 ! #1 #2 d 0 0

And entered into Robotics Toolbox: len1=link([0,100,0,0]) len2=link([0,50,0,0]) L{1}=len1 L{2}=len2 R=robot(L) drivebot(R)

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)

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: z y x #2
100 20

#3
50

#1 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 coordinate 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: y

x z
20

#1 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. " 0 90 0 0 a 0 0 100 50 ! #1 0 #2 #3 d 20 0 0 0

And entered into Robotics Toolbox: 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: 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) plot(t,vel) note jtaj can return a matrix, the first column is position, the second column is velocity t=[0:0.5:3]'

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 Start Stop number angle 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) subplot(6,1,1);plot(t,pos(:,1)) subplot(6,1,2);plot(t,pos(:,2)) subplot(6,1,3);plot(t,pos(:,3)) 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 pos is a matrix representing all 6 joint motions this is a method of having several plots on the screen at once (subplots)

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))