matlab机器人工具箱matlabrobotics_toolbox_demo

MATLAB ROBOTTOOLrtdemo演示目录一、rtdemo机器人工具箱演示 (2)二、transfermations坐标转换 (2)三、Trajectory齐次方程 (8)四、forward kinematics 运动学正解 (12)四、Animation 动画 (18)五、Inverse Kinematics运动学逆解 (23)六、Jacobians雅可比---矩阵与速度 (32)七、Inverse Dynamics逆向动力学 (45)八、Forward Dynamics正向动力学 (52)一、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 bytransl(0.5, 0.0, 0.0)ans =1.0000 0 0 0.50000 1.0000 0 00 0 1.0000 00 0 0 1.0000%% a rotation of 90degrees about the Y axis byroty(pi/2)ans =0.0000 0 1.0000 00 1.0000 0 00 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 00 0 1.0000 00 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 00 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 byt * [0 0 0 1]'ans =0.50001.0000pause % any key to continue%% the orientation of the new coordinate frame may be expressed in terms of% Euler anglestr2eul(t)ans =0 1.5708 -1.5708%% or roll/pitch/yaw anglestr2rpy(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 examplerotx(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 00 0 0 1.0000rotz(-pi/8) * rotx(pi/2)ans =0.9239 0.0000 -0.3827 0-0.3827 0.0000 -0.9239 00 1.0000 0.0000 00 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 operationssubplot(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 beforesubplot(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 qzqzqz =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.45210 1.0000 0 -0.15000 0 1.0000 0.43180 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 vectorq = jtraj(qz, qr, t); % compute the joint coordinate trajectory%% then the homogeneous transform for each set of joint coordinates is given byT = fkine(p560, q);%% where T is a 3-dimensional matrix, the first two dimensions are a 4x4% homogeneous transformation and the third dimension istime.%% For example, the first point isT(:,:,1)ans =1.0000 0 0 0.45210 1.0000 0 -0.15000 0 1.0000 0.43180 0 0 1.0000%% and the tenth point isT(:,:,10)ans =1.0000 -0.0000 0 0.4455-0.0000 1.0000 0 -0.15000 0 1.0000 0.50680 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 timesubplot(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 generatedt = [0:.056:2]'; % generate a time vectorq = jtraj(qz, qr, t); % generate joint coordinate trajectory%% the overloaded function plot() animates a stick figure robotmoving% 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.%% Let's make a clone of the Puma robot, but change its name and base locationp560_2 = p560;p560_ = 'another Puma';p560_2.base = transl(-0.5, 0.5, 0);hold onplot(p560_2, q);pause % any key to continue% We can also have multiple views of the same robot clfplot(p560, qr);figureplot(p560, qr);view(40,50)plot(p560, q)pause % any key to continue%% Sometimes it's 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 0]q =0 -0.7854 -0.7854 0 0.3927 0T = 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 startingvalue 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);qi'ans =-0.0000-0.7854-0.7854-0.00000.39270.0000%% Compared with the original valueqq =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 Puma's wrist axes are% aligned resulting in the loss of one degree of freedom.T = fkine(p560, qr);qi = ikine(p560, T);qi'ans =-0.00001.5238-1.4768-0.0000-0.04700.0000%% which is not the same as the original joint angleqrqr =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.06940.0000 0 -0.0000 00.0902 0.0000 0 0.11100 0 0 0pause % any key to continue% Inverse kinematics may also be computed for a trajectory.% If we take a Cartesian straight line patht = [0:.056:2]; % create a time vectorT1 = transl(0.6, -0.5, 0.0) % define the start pointT1 =1.0000 0 0 0.60000 1.0000 0 -0.50000 0 1.0000 00 0 0 1.0000T2 = transl(0.4, 0.5, 0.2) % and destinationT2 =1.0000 0 0 0.40000 1.0000 0 0.50000 0 1.0000 0.20000 0 0 1.0000T = ctraj(T1, T2, length(t)); % compute a Cartesian path%% now solve the inverse kinematics. When solving for atrajectory, the% starting joint coordinates for each point is taken as the result of the% previous inverse solution.%ticq = 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.%% Let's examine the joint space trajectory that results in straightline% Cartesian motionsubplot(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.10000.1000 0 0.2000 0.2000-0.1000 -0.2000 0 00 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 transformT = transl(100, 200, 300) * roty(pi/8) * rotz(-pi/4);%% then the differential motion in the second frame would be given byDT = tr2jac(T) * D;DT'ans =-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 manipulator's Jacobian matrix relates differential joint coordinate% motion to differential Cartesian motion;%% d X = 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 0]q =0.1000 0.7500 -2.2500 0 0.7500J = jacob0(p560, q)J =0.0746 -0.3031 -0.0102 0 0 00.7593 -0.0304 -0.0010 0 0 00 0.7481 0.4322 0 0 00.0000 0.0998 0.0998 0.9925 0.0998 0.67820 -0.9950 -0.9950 0.0996 -0.99500.06811.0000 0.0000 0.0000 0.0707 0.0000 0.7317%% or the T6 coordinate frameJ = jacobn(p560, q)J =0.1098 -0.7328 -0.3021 0 0 00.7481 0.0000 0.0000 0 0 00.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 00.7317 0.0000 0.0000 0.7317 0.00001.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 robot's% end-effector.pause % any key to continue%% Many control schemes require the inverse of the Jacobian. The Jacobian% in this example is not singulardet(J)ans =-0.0632%% and may be invertedJi = inv(J)Ji =0.0000 1.3367 -0.0000 0.0000 0.0000 0-2.4946 0.6993 -2.4374 -0.0000 -0.0000 02.7410 -1.2106 5.9125 0.0000 0.0000 00.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 Whitney's 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 directionqvel = Ji * vel;qvel'ans =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 frameqvel = Ji * d6X; % compute required joint velocity as before qvel'ans =-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 direction. pause % any key to continue%% At a manipulator singularity or degeneracy the Jacobian becomes singular.% At the Puma's `ready' position for instance, two of the wrist joints are% aligned resulting in the loss of one degree of freedom. This is revealed by% the rank of the Jacobianrank( jacobn(p560, qr) )ans =5%% and the singular values aresvd( jacobn(p560, qr) )ans =1.90661.73210.56600.01660.00810.0000pause % any key to continue%% When not actually at a singularity the Jacobian can provide information% about how `well-conditioned' the manipulator is for making certain motions,% and is referred to as `manipulability'.%% A number of scalar manipulability measures have been proposed. One by% Yoshikawamaniplty(p560, q, 'yoshikawa')ans =[]%% is based purely on kinematic parameters of the manipulator. %% Another by Asada takes into account the inertia of the manipulator which% affects the acceleration achievable in different directions. This measure% varies from 0 to 1, where 1 indicates uniformity of acceleration in all% directionsmaniplty(p560, q, 'asada')ans =[]%% Both of these measures would indicate that this particular pose is not well% conditioned.pause % any key to continue% An interesting class of manipulators are those that are redundant, that is,% they have more than 6 degrees of freedom. Computing the joint motion for% such a manipulator is not straightforward. Approaches have been suggested% based on the pseudo-inverse of the Jacobian (which will not be square) or% singular value decomposition of the Jacobian.%echo off七、Inverse Dynamics逆向动力学%% Inverse dynamics computes the joint torques required to achieve the specified% state of joint position, velocity and acceleration.% The recursive Newton-Euler formulation is an efficient matrixoriented% algorithm for computing the inverse dynamics, and is implemented in the% function rne().%% Inverse dynamics requires inertial and mass parameters of each link, as well% as the kinematic parameters. This is achieved by augmenting the kinematic% description matrix with additional columns for the inertial and mass% parameters for each link.%% For example, for a Puma 560 in the zero angle pose, with all joint velocities% of 5rad/s and accelerations of 1rad/s/s, the joint torques required are%tau = rne(p560, qz, 5*ones(1,6), ones(1,6))tau =-79.4048 37.1694 13.5455 1.0728 0.9399 0.5119pause % any key to continue% As with other functions the inverse dynamics can be computed for each point% on a trajectory. Create a joint coordinate trajectory and compute velocity% and acceleration as wellt = [0:.056:2]; % create time vector[q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectorytau = rne(p560, q, qd, qdd); % compute inverse dynamics %% Now the joint torques can be plotted as a function of time plot(t, tau(:,1:3))xlabel('Time (s)');ylabel('Joint torque (Nm)')pause % any key to continue%% Much of the torque on joints 2 and 3 of a Puma 560 (mounted conventionally) is% due to gravity. That component can be computed using gravload()taug = gravload(p560, q);plot(t, taug(:,1:3))xlabel('Time (s)');ylabel('Gravity torque (Nm)')pause % any key to continue% Now lets plot that as a fraction of the total torque required over the% trajectorysubplot(2,1,1)plot(t,[tau(:,2) taug(:,2)])xlabel('Time (s)');ylabel('Torque on joint 2 (Nm)')subplot(2,1,2)plot(t,[tau(:,3) taug(:,3)])xlabel('Time (s)');ylabel('Torque on joint 3 (Nm)')pause % any key to continue%% The inertia seen by the waist (joint 1) motor changes markedly with robot% configuration. The function inertia() computes the manipulator inertia matrix% for any given configuration.%% Let's compute the variation in joint 1 inertia, that is M(1,1), as the% manipulator moves along the trajectory (this may take a few minutes)M = inertia(p560, q);M11 = squeeze(M(1,1,:));plot(t, M11);xlabel('Time (s)');。