www.gusucode.com > 机器人工具箱 - robot源码程序 > robot\demos\rttgdemo.m
%RTTRDEMO Trajectory demo % Copyright (C) 1993-2002, by Peter I. Corke % $Log: not supported by cvs2svn $ % Revision 1.3 2002-04-02 12:26:49 pic % Handle figures better, control echo at end of each script. % Fix bug in calling ctraj. % % Revision 1.2 2002/04/01 11:47:18 pic % General cleanup of code: help comments, see also, copyright, remnant dh/dyn % references, clarification of functions. % % $Revision: 1.1 $ %*****************************trajectory**************************************** figure(2) echo on % 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 continue echo off