~【精炼，值得】MasteringRoboticsToolbox

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