matlab机器人工具箱matlabrobotics-toolbox-demo

MATLAB ROBOTTOOLrtdemo演示目录一、rtdemo机器人工具箱演示1二、transfermations坐标转换1三、Trajectory齐次方程4四、forward kinematics 运动学正解7四、Animation 动画10五、Inverse Kinematics运动学逆解14六、 Jacobians雅可比-矩阵与速度18七、Inverse Dynamics逆向动力学23八、Forward Dynamics正向动力学27一、rtdemo机器人工具箱演示 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 relationships 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 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 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 orientation 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 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.0000%pause % any key to continueecho off三、Trajectory齐次方程% The path 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 = jtraj(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(:,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 % acceleration 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 subplot(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 position 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 % 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 to 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 given 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 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) subplot(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,:), 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 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 robot 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_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 manually 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,% given 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 -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 % manipulator 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 starting 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 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.pause % 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.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 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.0000 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 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 trajectory 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 space 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 translation, 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 use 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 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 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 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 0.0681 1.0000 0.0000 0.0000 0.0707 0.0000 0.7317% or the T6 coordinate frame J = jacobn(p560, q)J = 0.1098 -0.7328 -0.3021 0 0 0 0.7481 0.0000 0.0000 0 0 0 0.1023 0.3397 0.3092 0 0 0 -0.6816 0 0 0.6816 0 0 -0.0000 -1.0000 -1.0000 -0.0000 -1.0000 0 0.7317 0.0000 0.0000 0.7317 0.0000 1.0000% Note the top right 3x3 block is all zero. This indicates, correctly, that% motion of joints 4-6 does not cause any translational motion of the robots% end-effector.pause % any key to continue% Many control schemes require the inverse of the Jacobian. The Jacobian% in this example is not singular det(J)ans = -0.0632% and may be inverted Ji = inv(J)Ji = 0.0000 1.3367 -0.0000 0.0000 0.0000 0 -2.4946 0.6993 -2.4374 -0.0000 -0.0000 0 2.7410 -1.2106 5.9125 0.0000 0.0000 0 0.0000 1.3367 -0.0000 1.4671 -0.0000 0 -0.2464 0.5113 -3.4751 -0.0000 -1.0000 0 -0.0000 -1.9561 0.0000 -1.0734 0.0000 1.0000pause % any key to continue% A classic control technique is Whitneys resolved rate motion control% dQ/dt = J(q)-1 dX/dt% where dX/dt is the desired Cartesian velocity, and dQ/dt is the required% joint velocity to achieve this. vel = 1 0 0 0 0 0; % translational motion in the X direction qvel = Ji * vel; qvelans = 0.0000 -2.4946 2.7410 0.0000 -0.2464 -0.0000% This is an alternative strategy to computing a Cartesian trajectory % and solving the inverse kinematics. However like that other scheme, this% strategy also runs into difficulty at a manipulator singularity where% the Jacobian is singular.pause % any key to continue% As already stated this Jacobian relates joint velocity to end-effector % velocity expressed in the end-effector reference frame. We may wish % instead to specify the velocity in base or world coordinates.% We have already seen how differential motions in one frame can be translated % to another. Consider the velocity as a differential in the world frame, that% is, d0X. We can write% d6X = Jac(T6) d0X% T6 = fkine(p560, q); % compute the end-effector transform d6X = tr2jac(T6) * vel; % translate world frame velocity to T6 frame qvel = Ji * d6X; % compute required joint velocity as before qvelans = -0.1334 -3.5391 6.1265 -0.1334 -2.5874 0.1953% Note that this value of joint velocity is quite different to that calculated% above, which was for motion in the T6 X-axis