《matlab机器人工具箱matlabrobotics-toolbox-demo.docx》由会员分享，可在线阅读，更多相关《matlab机器人工具箱matlabrobotics-toolbox-demo.docx（46页珍藏版）》请在taowenge.com淘文阁网|工程机械CAD图纸|机械工程制图|CAD装配图下载|SolidWorks_CaTia_CAD_UG_PROE_设计图分享下载上搜索。
1、如有侵权，请联系网站删除，仅供学习与交流matlab机器人工具箱matlabrobotics-toolbox-demo【精品文档】第 46 页MATLAB ROBOTTOOLrtdemo演示目录一、rtdemo机器人工具箱演示1二、transfermations坐标转换1三、Trajectory齐次方程4四、forward kinematics 运动学正解7四、Animation 动画10五、Inverse Kinematics运动学逆解14六、 Jacobians雅可比-矩阵与速度18七、Inverse Dynamics逆向动力学23八、Forward Dynamics正向动力学27一、rt
2、demo机器人工具箱演示 rtdemo二、transfermations坐标转换% In the field of robotics there are many possible ways of representing % positions and orientations, but the homogeneous transformation is well % matched to MATLABs powerful tools for matrix manipulation.% Homogeneous transformations describe the relationship
3、s between Cartesian % coordinate frames in terms of translation and orientation. % A pure translation of 0.5m in the X direction is represented by transl(0.5, 0.0, 0.0)ans = 1.0000 0 0 0.5000 0 1.0000 0 0 0 0 1.0000 0 0 0 0 1.0000% a rotation of 90degrees about the Y axis by roty(pi/2)ans = 0.0000 0
4、 1.0000 0 0 1.0000 0 0 -1.0000 0 0.0000 0 0 0 0 1.0000% and a rotation of -90degrees about the Z axis by rotz(-pi/2)ans = 0.0000 1.0000 0 0 -1.0000 0.0000 0 0 0 0 1.0000 0 0 0 0 1.0000% these may be concatenated by multiplication t = transl(0.5, 0.0, 0.0) * roty(pi/2) * rotz(-pi/2)t = 0.0000 0.0000
5、1.0000 0.5000 -1.0000 0.0000 0 0 -0.0000 -1.0000 0.0000 0 0 0 0 1.0000% If this transformation represented the origin of a new coordinate frame with respect% to the world frame origin (0, 0, 0), that new origin would be given by t * 0 0 0 1ans = 0.5000 0 0 1.0000pause % any key to continue% the orie
6、ntation of the new coordinate frame may be expressed in terms of% Euler angles tr2eul(t)ans = 0 1.5708 -1.5708% or roll/pitch/yaw angles tr2rpy(t)ans = -1.5708 0.0000 -1.5708pause % any key to continue% It is important to note that tranform multiplication is in general not % commutative as shown by
7、the following example rotx(pi/2) * rotz(-pi/8)ans = 0.9239 0.3827 0 0 -0.0000 0.0000 -1.0000 0 -0.3827 0.9239 0.0000 0 0 0 0 1.0000 rotz(-pi/8) * rotx(pi/2)ans = 0.9239 0.0000 -0.3827 0 -0.3827 0.0000 -0.9239 0 0 1.0000 0.0000 0 0 0 0 1.0000pause % any key to continueecho off三、Trajectory齐次方程% The pa
8、th will move the robot from its zero angle pose to the upright (or % READY) pose.% First create a time vector, completing the motion in 2 seconds with a% sample interval of 56ms. t = 0:.056:2;pause % hit any key to continue% A polynomial trajectory between the 2 poses is computed using jtraj() q = j
9、traj(qz, qr, t);pause % hit any key to continue% For this particular trajectory most of the motion is done by joints 2 and 3,% and this can be conveniently plotted using standard MATLAB operations subplot(2,1,1) plot(t,q(:,2) title(Theta) xlabel(Time (s); ylabel(Joint 2 (rad) subplot(2,1,2) plot(t,q
10、(:,3) xlabel(Time (s); ylabel(Joint 3 (rad)pause % hit any key to continue% We can also look at the velocity and acceleration profiles. We could % differentiate the angle trajectory using diff(), but more accurate results % can be obtained by requesting that jtraj() return angular velocity and % acc
11、eleration as follows q,qd,qdd = jtraj(qz, qr, t);% which can then be plotted as before subplot(2,1,1) plot(t,qd(:,2) title(Velocity) xlabel(Time (s); ylabel(Joint 2 vel (rad/s) subplot(2,1,2) plot(t,qd(:,3) xlabel(Time (s); ylabel(Joint 3 vel (rad/s)pause(2)% and the joint acceleration profiles subp
12、lot(2,1,1) plot(t,qdd(:,2) title(Acceleration) xlabel(Time (s); ylabel(Joint 2 accel (rad/s2) subplot(2,1,2) plot(t,qdd(:,3) xlabel(Time (s); ylabel(Joint 3 accel (rad/s2)pause % any key to continueecho off四、forward kinematics 运动学正解% Forward kinematics is the problem of solving the Cartesian positio
13、n and % orientation of a mechanism given knowledge of the kinematic structure and % the joint coordinates.% Consider the Puma 560 example again, and the joint coordinates of zero,% which are defined by qz qzqz = 0 0 0 0 0 0% The forward kinematics may be computed using fkine() with an appropropriate
14、 % kinematic description, in this case, the matrix p560 which defines % kinematics for the 6-axis Puma 560. fkine(p560, qz)ans = 1.0000 0 0 0.4521 0 1.0000 0 -0.1500 0 0 1.0000 0.4318 0 0 0 1.0000% returns the homogeneous transform corresponding to the last link of the % manipulatorpause % any key t
15、o continue% fkine() can also be used with a time sequence of joint coordinates, or % trajectory, which is generated by jtraj() t = 0:.056:2; % generate a time vector q = jtraj(qz, qr, t); % compute the joint coordinate trajectory% then the homogeneous transform for each set of joint coordinates is g
16、iven by T = fkine(p560, q);% where T is a 3-dimensional matrix, the first two dimensions are a 4x4 % homogeneous transformation and the third dimension is time.% For example, the first point is T(:,:,1)ans = 1.0000 0 0 0.4521 0 1.0000 0 -0.1500 0 0 1.0000 0.4318 0 0 0 1.0000% and the tenth point is
17、T(:,:,10)ans = 1.0000 -0.0000 0 0.4455 -0.0000 1.0000 0 -0.1500 0 0 1.0000 0.5068 0 0 0 1.0000pause % any key to continue% Elements (1:3,4) correspond to the X, Y and Z coordinates respectively, and % may be plotted against time subplot(3,1,1) plot(t, squeeze(T(1,4,:) xlabel(Time (s); ylabel(X (m) s
18、ubplot(3,1,2) plot(t, squeeze(T(2,4,:) xlabel(Time (s); ylabel(Y (m) subplot(3,1,3) plot(t, squeeze(T(3,4,:) xlabel(Time (s); ylabel(Z (m)pause % any key to continue% or we could plot X against Z to get some idea of the Cartesian path followed% by the manipulator. subplot(1,1,1) plot(squeeze(T(1,4,:
19、), squeeze(T(3,4,:); xlabel(X (m) ylabel(Z (m) gridpause % any key to continueecho off四、Animation 动画clf% The trajectory demonstration has shown how a joint coordinate trajectory% may be generated t = 0:.056:2; % generate a time vector q = jtraj(qz, qr, t); % generate joint coordinate trajectory% the
20、 overloaded function plot() animates a stick figure robot moving % along a trajectory. plot(p560, q);% The drawn line segments do not necessarily correspond to robot links, but % join the origins of sequential link coordinate frames.% A small right-angle coordinate frame is drawn on the end of the r
21、obot to show% the wrist orientation.% A shadow appears on the ground which helps to give some better idea of the% 3D object.pause % any key to continue% We can also place additional robots into a figure.% Lets make a clone of the Puma robot, but change its name and base location p560_2 = p560; p560_
22、2.name = another Puma; p560_2.base = transl(-0.5, 0.5, 0); hold on plot(p560_2, q);pause % any key to continue% We can also have multiple views of the same robot clf plot(p560, qr); figure plot(p560, qr); view(40,50) plot(p560, q)pause % any key to continue% Sometimes its useful to be able to manual
23、ly drive the robot around to% get an understanding of how it works. drivebot(p560)% use the sliders to control the robot (in fact both views). Hit the red quit% button when you are done.echo off五、Inverse Kinematics运动学逆解% Inverse kinematics is the problem of finding the robot joint coordinates,% give
24、n a homogeneous transform representing the last link of the manipulator.% It is very useful when the path is planned in Cartesian space, for instance % a straight line path as shown in the trajectory demonstration.% First generate the transform corresponding to a particular joint coordinate, q = 0 -
25、pi/4 -pi/4 0 pi/8 0q = 0 -0.7854 -0.7854 0 0.3927 0 T = fkine(p560, q);% Now the inverse kinematic procedure for any specific robot can be derived % symbolically and in general an efficient closed-form solution can be % obtained. However we are given only a generalized description of the % manipulat
26、or in terms of kinematic parameters so an iterative solution will % be used. The procedure is slow, and the choice of starting value affects % search time and the solution found, since in general a manipulator may % have several poses which result in the same transform for the last% link. The starti
27、ng point for the first point may be specified, or else it% defaults to zero (which is not a particularly good choice in this case) qi = ikine(p560, T); qians = -0.0000 -0.7854 -0.7854 -0.0000 0.3927 0.0000% Compared with the original value qq = 0 -0.7854 -0.7854 0 0.3927 0% A solution is not always
28、possible, for instance if the specified transform % describes a point out of reach of the manipulator. As mentioned above % the solutions are not necessarily unique, and there are singularities % at which the manipulator loses degrees of freedom and joint coordinates % become linearly dependent.paus
29、e % any key to continue% To examine the effect at a singularity lets repeat the last example but for a% different pose. At the ready position two of the Pumas wrist axes are % aligned resulting in the loss of one degree of freedom. T = fkine(p560, qr); qi = ikine(p560, T); qians = -0.0000 1.5238 -1.
30、4768 -0.0000 -0.0470 0.0000% which is not the same as the original joint angle qrqr = 0 1.5708 -1.5708 0 0 0pause % any key to continue% However both result in the same end-effector position fkine(p560, qi) - fkine(p560, qr)ans = 1.0e-015 * 0 -0.0000 -0.0902 -0.0694 0.0000 0 -0.0000 0 0.0902 0.0000
31、0 0.1110 0 0 0 0pause % any key to continue% Inverse kinematics may also be computed for a trajectory.% If we take a Cartesian straight line path t = 0:.056:2; % create a time vector T1 = transl(0.6, -0.5, 0.0) % define the start pointT1 = 1.0000 0 0 0.6000 0 1.0000 0 -0.5000 0 0 1.0000 0 0 0 0 1.00
32、00 T2 = transl(0.4, 0.5, 0.2)% and destinationT2 = 1.0000 0 0 0.4000 0 1.0000 0 0.5000 0 0 1.0000 0.2000 0 0 0 1.0000 T = ctraj(T1, T2, length(t); % compute a Cartesian path% now solve the inverse kinematics. When solving for a trajectory, the % starting joint coordinates for each point is taken as
33、the result of the % previous inverse solution. tic q = ikine(p560, T); tocElapsed time is 0.315656 seconds.% Clearly this approach is slow, and not suitable for a real robot controller % where an inverse kinematic solution would be required in a few milliseconds.% Lets examine the joint space trajec
34、tory that results in straightline % Cartesian motion subplot(3,1,1) plot(t,q(:,1) xlabel(Time (s); ylabel(Joint 1 (rad) subplot(3,1,2) plot(t,q(:,2) xlabel(Time (s); ylabel(Joint 2 (rad) subplot(3,1,3) plot(t,q(:,3) xlabel(Time (s); ylabel(Joint 3 (rad)pause % hit any key to continue% This joint spa
35、ce trajectory can now be animated plot(p560, q)pause % any key to continueecho off六、 Jacobians雅可比-矩阵与速度% Jacobian and differential motion demonstration% A differential motion can be represented by a 6-element vector with elements% dx dy dz drx dry drz% where the first 3 elements are a differential t
36、ranslation, and the last 3 % are a differential rotation. When dealing with infinitisimal rotations, % the order becomes unimportant. The differential motion could be written % in terms of compounded transforms% transl(dx,dy,dz) * rotx(drx) * roty(dry) * rotz(drz)% but a more direct approach is to u
37、se the function diff2tr() D = .1 .2 0 -.2 .1 .1; diff2tr(D)ans = 0 -0.1000 0.1000 0.1000 0.1000 0 0.2000 0.2000 -0.1000 -0.2000 0 0 0 0 0 0pause % any key to continue% More commonly it is useful to know how a differential motion in one % coordinate frame appears in another frame. If the second frame
38、 is % represented by the transform T = transl(100, 200, 300) * roty(pi/8) * rotz(-pi/4);% then the differential motion in the second frame would be given by DT = tr2jac(T) * D; DTans = -29.5109 69.7669 -42.3289 -0.2284 -0.0870 0.0159% tr2jac() has computed a 6x6 Jacobian matrix which transforms the
39、differential % changes from the first frame to the next.pause % any key to continue% The manipulators Jacobian matrix relates differential joint coordinate % motion to differential Cartesian motion;% dX = J(q) dQ% For an n-joint manipulator the manipulator Jacobian is a 6 x n matrix and% is used is
40、many manipulator control schemes. For a 6-axis manipulator like% the Puma 560 the Jacobian is square% Two Jacobians are frequently used, which express the Cartesian velocity in% the world coordinate frame, q = 0.1 0.75 -2.25 0 .75 0q = 0.1000 0.7500 -2.2500 0 0.7500 0 J = jacob0(p560, q)J = 0.0746 -0.3031 -0.0102 0 0 0 0.7593 -0.0304 -0.0010 0 0 0 0 0.7481 0.4322 0 0 0 0.0000 0.0998 0.0998 0.9925 0.0998 0.6782 0 -0.9950 -0.9950 0.0996 -0.9950