新建 文本文档 (3).txt

Robotics Toolbox for matlab简要帮助文件 可以用于机器人路径规划

Robotics Toolbox for Matlab

1 Transformations

% In the field of robotics there are many possible ways of representing 

% positions and orientations, but the homogeneous transformation is well 

% matched to MATLAB’s 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 1]'

ans =

    0.5000

         0

         0

    1.0000

% 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.5708

% 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

2 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];

% A polynomial trajectory between the 2 poses is computed using jtraj()

    q = jtraj(qz, qr, t);

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

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

3 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

    qz

qz =

     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 

% manipulator

% 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.0000

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

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

    grid
4 Animation

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

% We can also place additional robots into a figure.

% Let's 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);

% We can also have multiple views of the same robot

    clf

    plot(p560, qr);

    figure

    plot(p560, qr);

    view(40,50)

    plot(p560, q)

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

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

    qi'

ans =

    0.0000

   -0.7854

   -0.7854

    0.0000

    0.3927

   -0.0000

% Compared with the original value

    q

q =