新建 文本文档 (3).txt

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

         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.

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

    1.5238

   -1.4768

   -0.0000

   -0.0470

   0.0000

% which is not the same as the original joint angle

    qr

qr =

         0    1.5708   -1.5708         0         0         0

% However both result in the same end-effector position

    fkine(p560, qi) - fkine(p560, qr)

ans =

1.0e-015 *

         0   -0.0000   -0.1249   -0.0867

    0.0000         0   -0.0000         0

    0.1249    0.0000         0    0.1110

         0         0         0         0

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

T1 =

    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 destination

T2 =

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

    toc

Elapsed time is 1.082161 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 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)')

% This joint space trajectory can now be animated

    plot(p560, q)
 
   vel = [1 0 0 0 0 0]'; % translational motion in the X direction

    qvel = 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.

% 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

    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.

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

    rank( jacobn(p560, qr) )

ans =

     5

% and the singular values are

    svd( jacobn(p560, qr) )

ans =

    1.9066

    1.7321

    0.5660

    0.0166

    0.0081

    0.0000

% 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

% Yoshikawa

   maniplty(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 

% directions

    maniplty(p560, q, 'asada')

ans =

     []

% Both of these measures would indicate that this particular pose is not well

% conditioned.

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

7 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 matrix oriented

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

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

    t = [0:.056:2];              % create time vector

    [q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectory

    tau = 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)')

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

% Now lets plot that as a fraction of the total torque required over the 

% trajectory

    subplot(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)')

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

    ylabel('Inertia on joint 1 (kgms2)')

% Clearly the inertia seen by joint 1 varies considerably over this path.

% This is one of many challenges to control design in robotics, achieving 

% stability and high-performance in the face of plant variation. In fact 

% for this example the inertia varies by a factor of

    max(M11)/min(M11)

ans =

    1.6947

8 Forward dynamics

% Forward dynamics is the computation of joint accelerations given position and

% velocity state, and actuator torques. It is useful in simulation of a robot

% control system.

% Consider a Puma 560 at rest in the zero angle pose, with zero applied joint 

% torques. The joint acceleration would be given by

    accel(p560, qz, zeros(1,6), zeros(1,6))

ans =

   -0.2462

   -8.6829

    3.1462

    0.0021

    0.0603

    0.0001

% To be useful for simulation this function must be integrated. fdyn() uses the

% MATLAB function ode45() to integrate the joint acceleration. It also allows 

% for a user written function to compute the joint torque as a function of 

% manipulator state.

% To simulate the motion of the Puma 560 from rest in the zero angle pose 

% with zero applied joint torques

    tic

    [t q qd] = fdyn(nofriction(p560), 0, 10);

    toc

Elapsed time is 61.604237 seconds.

% and the resulting motion can be plotted versus time

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

% Clearly the robot is collapsing under gravity, but it is interesting to 

% note that rotational velocity of the upper and lower arm are exerting 

% centripetal and Coriolis torques on the waist joint, causing it to rotate.

% This can be shown in animation also

    clf

    plot(p560, q)