From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 14:54:28.07 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:28:35 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152128.KAA03707@hoiho> Content-transfer-encoding: 7BIT %QVMUL Multiply vector by unit-quaternion % % VT = qvmul(Q, V) % % Rotate the vector V by the unit-quaternion Q. % % See also QQMUL, QINV % Copyright (C) 1993 Peter Corke function vt = qvmul(q, v) v = v(:)'; if numcols(v) ~= 3, error('Must be 3-vector'); end v = [0 v]; % turn it into a quaternion p = qqmul(qinv(q), qqmul(v,q)) vt = p(2:4)'; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 15:30:42.48 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:31:57 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152131.KAA03763@hoiho> Content-transfer-encoding: 7BIT %CINERTIA Compute the Cartesian manipulator inertia matrix % % CINERTIA(DYN, Q) for an n-axis manipulator returns the nxn symmetric % inertia matrix which relates Cartesian force/torque to Cartesian % acceleration. % DYN describes the manipulator dynamics and kinematics, and Q is % an n element vector of joint state. % % See also INERTIA, DYN, RNE. % Copyright (C) 1993 Peter Corke function Mx = cinertia(dyn, q) J = jacob0(dyn, q); Ji = inv(J); M = inertia(dyn, q); Mx = Ji' * M * Ji; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 15:34:04.60 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:30:38 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152130.KAA03742@hoiho> Content-transfer-encoding: 7BIT %QINTERP Interpolate rotations expressed by unit-quaternions % % QI = qinterp(Q1, Q2, R) % % Return a unit-quaternion that interpolates between Q1 and Q2 as R moves % from 0 to 1. This is a spherical linear interpolation (slerp) that can % be interpretted as interpolation along a great circle arc on a sphere. % % See also TR2Q % Copyright (C) 1993 Peter Corke function qi = qinterp(q1, q2, r) if (r<0) | (r>1), error('R out of range'); end theta = acos(q1*q2'); qi = (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 15:37:29.17 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:31:06 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152131.KAA03751@hoiho> Content-transfer-encoding: 7BIT %LINKTRAN Compute the link transform from kinematic parameters % % LINKTRAN(alpha, an, theta, dn) % LINKTRAN(DH, q) is a homogeneous % transformation between link coordinate frames. % % alpha is the link twist angle % an is the link length % theta is the link rotation angle % dn is the link offset % sigma is 0 for a revolute joint, non-zero for prismatic % % In the second case, q is substitued for theta or dn according to sigma. % % Based on the standard Denavit and Hartenberg notation. % Copright (C) Peter Corke 1993 function t = linktran(a, b, c, d) if nargin == 4, alpha = a; an = b; theta = c; dn = d; else if numcols(a) < 4, error('too few columns in DH matrix'); end alpha = a(1); an = a(2); if numcols(a) > 4, if a(5) == 0, % revolute theta = b; dn = a(4); else % prismatic theta = a(3); dn = b; end else theta = b; % assume revolute if sigma not given dn = a(4); end end sa = sin(alpha); ca = cos(alpha); st = sin(theta); ct = cos(theta); t = [ ct -st*ca st*sa an*ct st ct*ca -ct*sa an*st 0 sa ca dn 0 0 0 1]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 15:44:15.71 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:32:47 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152132.KAA03782@hoiho> Content-transfer-encoding: 7BIT %CORIOLIS Compute the manipulator coriolis matrix % % CORIOLIS(DYN, Q, QD) for an n-axis manipulator returns the n element % coriolis torque vector at the specified pose and velocity. % DYN describes the manipulator dynamics and kinematics. % % If Q and QD are row vectors, CORIOLIS(DYN,Q,QD) is a row vector % of joint torques. % If Q and QD are matrices, each row is interpretted as a joint state % vector, and CORIOLIS(DYN,Q,QD) is a matrix each row being the % corresponding joint % torques. % % See also DYN, RNE, ITORQUE, GRAVLOAD. % Copright (C) Peter Corke 1993 function c = coriolis(dyn, q, qd) c = rne(dyn, q, qd, zeros(size(q)), [0;0;0]); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 15:58:49.05 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:33:09 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152133.KAA03791@hoiho> Content-transfer-encoding: 7BIT %CROSS Vector cross product % % CROSS(V1, V2) returns the vector cross product V1 x V2 % % Copyright (C) Peter Corke 1990 function n = cross(u, v) n = zeros(3,1); n(1) = u(2)*v(3) - u(3)*v(2); n(2) = u(3)*v(1) - u(1)*v(3); n(3) = u(1)*v(2) - u(2)*v(1); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 16:02:10.94 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:33:39 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152133.KAA03804@hoiho> Content-transfer-encoding: 7BIT %TR2JAC Compute a Jacobian to map differentials between frames % % TR2JAC(T) returns a 6x6 Jacobian matrix to map differentials between % frames related by the homogeneous transform T. % % See also TR2DIFF, DIFF2TR, DIFF % Copyright (C) Peter Corke 1993 function J = tr2jac(t) J = [ t(1:3,1)' cross(t(1:3,4),t(1:3,1))' t(1:3,2)' cross(t(1:3,4),t(1:3,2))' t(1:3,3)' cross(t(1:3,4),t(1:3,3))' zeros(3,3) t(1:3,1:3)' ]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 16:08:24.60 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:32:32 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152132.KAA03777@hoiho> Content-transfer-encoding: 7BIT %FKINE Forward robot kinematics for serial link manipulator % % FKINE(DH, Q) computes the forward kinematics for each joint space % point defined by Q. DH describes the manipulator kinematics in standard % Denavit Hartenberg notation. % % DH has one row of kinematic parameters for each axis. Each row is of % the form % [alpha A theta D] for an all revolute manipulator % or [alpha A theta D sigma] for a mixed revolute/hybrid manipulator % % For an n-axis manipulator Q is an n element vector or an m x n matrix. The % elements are interpretted as joint angle or link length according to % the form of DH or the j'th sigma value (0 for revolute, other for prismatic). % % If Q is a vector it is interpretted as the generalized joint coordinates, and % FKINE(DH, Q) returns a 4x4 homogeneous transformation for the final link of % the manipulator. % % If Q is a matrix, the rows are interpretted as the generalized % joint coordinates for a sequence of points along a trajectory. Q(i,j) is % the j'th joint parameter for the i'th trajectory point. In this case % FKINE(DH, Q) returns an m x 16 matrix with each row containing a 'flattened' % homogeneous transform corresponding to the input joint state. A row can % be unflattened using reshape(v, 4, 4). % % See also LINKTRAN, MFKINE. % Copright (C) Peter Corke 1993 function t = fkine(dh, q) % % evaluate fkine for each point on a trajectory of % theta_i or q_i data % n = numrows(dh); if length(q) == n, t = eye(4,4); for i=1:n, t = t * linktran(dh(i,:), q(i)); end else if numcols(q) ~= n, error('bad data') end t = []; for qv=q', % for each trajectory point tt = eye(4,4); for i=1:n, tt = tt * linktran(dh(i,:), qv(i)); end t = [t; tt(:)']; end end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 16:15:46.36 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:33:51 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152133.KAA03809@hoiho> Content-transfer-encoding: 7BIT %CTRAJ Compute a Cartesian trajectory between two points % % TC = TRAJ(T0, T1, N) % TC = TRAJ(T0, T1, T) % % Returns a Cartesian trajectory TC from point T0 to T1. The number % of points is N or the length of the given time vector T. % % Each trajectory is a 16xn matrix, with one row per time step, each being % a flattened homogeneous transform (use reshape(row',4,4) to restore). % % Copyright (C) Peter Corke 1993 % MOD. HISTORY % 12/94 track changes to trinterp() function tt = ctraj(t0, t1, n) if length(n) > 1, n = length(n); end tt = []; dp = drivepar(t0, t1); for i=1:n, r = (i-1)/(n-1); t = trinterp(t0, dp, r); tt = [tt; t(:)']; end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 16:35:42.94 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:35:02 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152135.KAA03837@hoiho> Content-transfer-encoding: 7BIT %TR2DIFF Convert a transform difference to differential representation % % TR2DIFF(T) % % TR2DIFF(T1, T2) % % First form converts a homogeneous transform representing an % infinitessimal motion to a 6-element differential representation. % Such a homogeneous transform has a rotational submatrix that is, % approximately, skew symmetric. % % Second form returns the 6-element differential motion required to move % from T1 to T2 in base coordinates. % % See also DIFF2TR, DIFF, IKINE % Copyright (C) Peter Corke 1993 % MOD.HISTORY % 1/95 take mean of both values in the skew-symmetric matrix % 2/95 add two argument version as part of ikine() revamp function d = tr2diff(t1, t2) if nargin == 1, d = [ t1(1:3,4); 0.5*[t1(3,2)-t1(2,3); t1(1,3)-t1(3,1); t1(2,1)-t1(1,2)]]; else d = [ t2(1:3,4)-t1(1:3,4); 0.5*( cross(t1(1:3,1), t2(1:3,1)) + ... cross(t1(1:3,2), t2(1:3,2)) + ... cross(t1(1:3,3), t2(1:3,3)) ... )]; end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 16:39:09.38 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:35:12 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152135.KAA03841@hoiho> Content-transfer-encoding: 7BIT %TRINTERP Interpolate homogeneous transformations % % T = TRINTERP(T0, T1, R) % T = TRINTERP(T0, DP, R) % % Returns a homogeneous transform interpolation between T0 and T1 as % R varies from 0 to 1. % % The second form uses a drive parameter matrix computed by DRIVEPAR % which represents the `difference' between T0 and T1 and may % be more efficient for computing many points between the same % two endpoints. % % Robot manipulators: mathematics, programming and control % R.P. Paul, MIT Press, 1981. % % See also CTRAJ, DRIVEPAR. % Copyright (C) 1993 Peter Corke % MOD.HISTORY % 12/94 must have T0 as well as DP function t = trinterp(T0, a2, r) D = zeros(4,4); % eqn (5.69) % Using the notation of Paul, dp is % dp(1) x % dp(2) y % dp(3) z % dp(4) phi % dp(5) theta % dp(6) psi if ishomog(a2), dp = drivepar(T0, a2); else dp = a2; end srp = sin(dp(4)*r); % r*phi crp = cos(dp(4)*r); srt = sin(dp(5)*r); % r*theta crt = cos(dp(5)*r); vrt = 1 - crt; sp = sin(dp(4)); % psi cp = cos(dp(4)); D(1,2) = -srp*(sp^2*vrt+crt)+crp*(-sp*cp*vrt); D(2,2) = -srp*(-sp*cp*vrt)+crp*(cp^2*vrt+crt); D(3,2) = -srp*(-cp*srt)+crp*(-sp*srt); D(1,3) = cp*srt; D(2,3) = sp*srt; D(3,3) = crt; D(1:3,1) = cross(D(1:3,2), D(1:3,3)); D(:,4) = [dp(1:3)*r 1]'; t = T0*D; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 16:52:00.62 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:34:39 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152134.KAA03831@hoiho> Content-transfer-encoding: 7BIT %MFKINE Forward robot kinematics for serial link manipulator % % MFKINE(DH, Q) computes the forward kinematics for each joint space % point defined by Q. DH describes the manipulator kinematics in modified % Denavit Hartenberg notation. % % DH has one row of kinematic parameters for each axis. Each row is of % the form % [alpha A theta D] for an all revolute manipulator % or [alpha A theta D sigma] for a mixed revolute/hybrid manipulator % % For an n-axis manipulator Q is an n element vector or an m x n matrix. The % elements are interpretted as joint angle or link length according to % the form of DH or the j'th sigma value (0 for revolute, other for prismatic). % % If Q is a vector it is interpretted as the generalized joint coordinates, and % MFKINE(DH, Q) returns a 4x4 homogeneous transformation for the final link of % the manipulator. % % If Q is a matrix, the rows are interpretted as the generalized % joint coordinates for a sequence of points along a trajectory. Q(i,j) is % the j'th joint parameter for the i'th trajectory point. In this case % MFKINE(DH, Q) returns an m x 16 matrix with each row containing a 'flattened' % homogeneous transform corresponding to the input joint state. A row can % be unflattened using reshape(v, 4, 4). % % See also MLINKTRANS, FKINE. % Copright (C) Peter Corke 1993 function t = mfkine(dh, q) % % evaluate fkine for each point on a trajectory of % theta_i or q_i data % n = numrows(dh); if numcols(q) ~= n, error('bad data') end if length(q) == n, t = eye(4,4); for i=1:n, t = t * mlinktrans(dh(i,:), q(i)); end else t = []; for qv=q', % for each trajectory point tt = eye(4,4); for i=1:n, tt = tt * mlinktrans(dh(i,:), qv(i)); end t = [t; tt(:)']; end end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 16:57:37.56 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:30:51 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152130.KAA03746@hoiho> Content-transfer-encoding: 7BIT echo on % % 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)) pause % 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 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)') 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 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)') 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 M11 = []; for Q = q', M = inertia(p560, Q'); M11 = [M11; M(1,1)]; end 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)]) pause % any key to continue From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 17:00:52.68 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:31:23 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152131.KAA03755@hoiho> Content-transfer-encoding: 7BIT %PLOTBOT Graphical robot animation % % PLOTBOT(DH, Q) % PLOTBOT(DH, Q, OPT) % % produces a graphical animation of a robot from a % description of the kinematics, DH, and a joint trajectory Q. % For an n-axis manipulator Q is mxn for an m-point trajectory. % % OPT is a character string which may contain the following letters: % l leave trail, that is, dont erase the previous pose % w don't draw the wrist axes % r repeat, play the animation 50 times % b set the height of the robot's base in Z. Must % be the last argument. % % See also FKINE, DH. % Copright (C) Peter Corke 1993 % MOD.HISTORY % 12/94 make axis scaling adjust to robot kinematic params function plotbot(dh, q, opt) np = numrows(q); n = numrows(dh); if numcols(q) ~= n, error('Insufficient columns in q') end erasemode = 'xor'; wrist = 1; repeat = 1; base = 0.0; % % options % if nargin == 3, mopt = size(opt,2); for i=1:mopt, if (opt(i) == 'l'), erasemode = 'none'; end; if (opt(i) == 'w'), wrist = 0; end; if (opt(i) == 'r'), repeat = 50; end; if (opt(i) == 'b'), base = str2num(opt(i+1:mopt)); break; end; end; end; % % simple heuristic to figure the maximum reach of the robot % reach = sum(abs(dh(:,2))) + sum(abs(dh(:,4))); % % setup an axis in which to animate the robot % clg axis([-reach reach -reach reach -reach reach]); figure(gcf); % bring to the top xlabel('X') ylabel('Y') zlabel('Z') set(gca, 'drawmode', 'fast'); grid line('xdata', [0;0], 'ydata', [0;0], 'zdata', [-reach;base], 'color', 'magenta'); % create a line which we will % subsequently modify. Set erase mode to xor for fast % update % hr = line('color', 'yellow', 'erasemode', erasemode); if wrist, hx = line('xdata', [0;0], 'ydata', [0;0], 'zdata', [0;base], ... 'color', 'red', 'erasemode', 'xor'); hy = line('xdata', [0;0], 'ydata', [0;0], 'zdata', [0;base], ... 'color', 'green', 'erasemode', 'xor'); hz = line('xdata', [0;0], 'ydata', [0;0], 'zdata', [0;base], ... 'color', 'blue', 'erasemode', 'xor'); mag = reach/10; end for r=1:repeat, for p=1:np, % for every trajectory point x = 0; y = 0; z = base; % compute the link transforms, and record the origin of each frame % for the animation. t = [eye(3,3) [0;0;base];0 0 0 1]; for j=1:n, t = t * linktran(dh(j,:), q(p,j)); x = [x; t(1,4)]; y = [y; t(2,4)]; z = [z; t(3,4)]; end if wrist, % % compute the wrist axes % xv = t*[mag;0;0;1]; yv = t*[0;mag;0;1]; zv = t*[0;0;mag;1]; % % update the line segments, wrist axis and links % set(hx,'xdata',[t(1,4) xv(1)], 'ydata', [t(2,4) xv(2)], ... 'zdata', [t(3,4) xv(3)]); set(hy,'xdata',[t(1,4) yv(1)], 'ydata', [t(2,4) yv(2)], ... 'zdata', [t(3,4) yv(3)]); set(hz,'xdata',[t(1,4) zv(1)], 'ydata', [t(2,4) zv(2)], ... 'zdata', [t(3,4) zv(3)]); end set(hr,'xdata', x, 'ydata', y, 'zdata', z); drawnow end end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 17:44:17.86 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:35:24 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152135.KAA03846@hoiho> Content-transfer-encoding: 7BIT %TR2RPY Convert a homogeneous transform matrix to roll/pitch/yaw angles % % [A B C] = TR2RPY(TR) returns a vector of Euler angles % corresponding to the rotational part of the homogeneous transform TR. % % See also RPY2TR, TR2EUL % Copright (C) Peter Corke 1993 function rpy = tr2rpy(m) rpy = zeros(1,3); if abs(m(1,1)) < eps & abs(m(2,1)) < eps, rpy(1) = 0; rpy(2) = atan2(-m(3,1), m(1,1)); rpy(3) = atan2(-m(2,3), m(2,2)); else, rpy(1) = atan2(m(2,1), m(1,1)); sp = sin(rpy(1)); cp = cos(rpy(1)); rpy(2) = atan2(-m(3,1), cp * m(1,1) + sp * m(2,1)); rpy(3) = atan2(sp * m(1,3) - cp * m(2,3), cp*m(2,2) - sp*m(1,2)); end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 17:48:01.05 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:35:33 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152135.KAA03850@hoiho> Content-transfer-encoding: 7BIT echo on % % 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) pause % 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; DT' % % tr2jac() has computed a 6x6 Jacobian matrix which transforms the differential % changes from the first frame to the next. % pause % any key to continue % % Consider a Puma 560 robot in the pose q = [0 pi/4 -pi 0 -pi/8 0]; % % The corresponding Cartesian end-point location can be determined by forward kinematics T = fkine(p560, q); % % Now if the joint coordinate is perturbed slightly, we can compute a perturbed end-point % location delta = 1e-6; Td = fkine(p560, q+[1 0 0 0 0 0]*delta); % % and the difference between these transforms is dT = Td - T % % which may be converted to a differential motion D = tr2diff(dT)/delta; D' % % which indicates that joint 1 velocity contributes to velocity in the % X and Y directions and also some rotational velocity about the Z axis % pause % any key to continue % Extending this procedure we may build up the manipulator Jacobian matrix % which 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 % J = jacobian(p560, q) % % 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 singular det(J) % % and may be inverted Ji = inv(J) pause % 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 direction qvel = Ji * vel; qvel' % % 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 qvel' % % 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 % % The Jacobian can provide information about how `well-conditioned' the % manipulator is for making certain motions. In some sense this `condition' % is related to proximity to a singularity. % % A number of scalar manipulability measures have been proposed. One by % Yoshikawa yoshi(p560, q) % % 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 asada(p560, q) % % Both of these measures 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 From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 18:13:18.95 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:28:49 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152128.KAA03711@hoiho> Content-transfer-encoding: 7BIT %RNE Compute inverse dynamics via recursive Newton-Euler formulation % % TAU = RNE(DYN, Q, QD, QDD) % TAU = RNE(DYN, [Q QD QDD]) % % Returns the joint torque required to achieve the specified joint position, % velocity and acceleration state. % % Gravity is assumed to be acting in the -Z direction with acceleration of % 9.81m/s/s, but this may be overriden by providing a gravity acceleration % vector [gx gy gz]. % % TAU = RNE(DYN, Q, QD, QDD, GRAV) % TAU = RNE(DYN, [Q QD QDD], GRAV) % % An external force/moment acting on the end of the manipulator may also be % specified by a 6-element vector [Fx Fy Fz Mx My Mz]. % % TAU = RNE(DYN, Q, QD, QDD, GRAV, FEXT) % TAU = RNE(DYN, [Q QD QDD], GRAV, FEXT) % % where Q, QD and QDD are row vectors of the manipulator state; pos, vel, and accel. % % The torque computed also contains a contribution due to armature % inertia. % % See also DYN, FDYN, ACCEL, GRAVLOAD, INERTIA. % % Should be a MEX file. % % verified against MAPLE code, which is verified by examples % % Copyright (C) 1992 Peter Corke function tau = rne(dh_dyn, a1, a2, a3, a4, a5) z0 = [0;0;1]; grav = 9.81*z0; n = numrows(dh_dyn); if numcols(a1) == 3*n, Q = a1(:,1:n); Qd = a1(:,n+1:2*n); Qdd = a1(:,2*n+1:3*n); np = numrows(Q); if nargin >= 3, grav = a2; end if nargin == 4, fext = a3; end else np = numrows(a1); Q = a1; Qd = a2; Qdd = a3; if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ... numrows(Qd) ~= np | numrows(Qdd) ~= np, error('bad data'); end if nargin >= 5, grav = a4; end if nargin == 6, fext = a5; end end % % create local vars for mass, COG and inertia matrices % m = dh_dyn(:,6); % column vector of link mass r = dh_dyn(:,7:9); % matrix of COM data; row per link Jm = []; for j=1:n, J = [ dh_dyn(j,10) dh_dyn(j,13) dh_dyn(j,15); ... dh_dyn(j,13) dh_dyn(j,11) dh_dyn(j,14); ... dh_dyn(j,15) dh_dyn(j,14) dh_dyn(j,12) ]; Jm = [Jm J]; end tau = zeros(np,n); for p=1:np, q = Q(p,:)'; qd = Qd(p,:)'; qdd = Qdd(p,:)'; Fm = []; Nm = []; pstarm = []; Rm = []; w = zeros(3,1); wd = zeros(3,1); v = zeros(3,1); vd = grav; % % init some variables, compute the link rotation matrices % for j=1:n, alpha = dh_dyn(j,1); A = dh_dyn(j,2); if dh_dyn(j,5) == 0, theta = q(j); D = dh_dyn(j,4); else theta = dh_dyn(j,3); D = q(j); end sa = sin(alpha); ca = cos(alpha); st = sin(theta); ct = cos(theta); R = [ ct -st*ca st*sa st ct*ca -ct*sa 0 sa ca]; Rm = [Rm R]; pstar = [A; D*sa; D*ca]; pstarm = [pstarm pstar]; end % % the forward recursion % for j=1:n, R = Rm(:,3*j-2:3*j)'; pstar = pstarm(:,j); % % statement order is important here % if dh_dyn(j,5) == 0, % revolute axis wd = R*(wd + z0*qdd(j) + ... cross(w,z0*qd(j))); w = R*(w + z0*qd(j)); %v = cross(w,pstar) + R*v; vd = cross(wd,pstar) + ... cross(w, cross(w,pstar)) +R*vd; else % prismatic axis w = R*w; wd = R*wd; %v = R*(z0*qd(j) + v) + cross(w,pstar); vd = R*(z0*qdd(j)+vd) + ... cross(wd,pstar) + ... 2*cross(w,R*z0*qd(j)) +... cross(w, cross(w,pstar)) +R*vd; end J = Jm(:,3*j-2:3*j); vhat = cross(wd,r(j,:)') + ... cross(w,cross(w,r(j,:)')) + vd; F = m(j)*vhat; N = J*wd + cross(w,J*w); Fm = [Fm F]; Nm = [Nm N]; end % % the backward recursion % f = zeros(3,1); % force/moments on end of arm nn = zeros(3,1); for j=n:-1:1, pstar = pstarm(:,j); % % order of these statements is important, since both % nn and f are functions of previous f. % if j < n, R = Rm(:,3*j+1:3*j+3); nn = R*(nn + cross(R'*pstar,f)) + ... cross(pstar+r(j,:)',Fm(:,j)) + ... Nm(:,j); f = R*f + Fm(:,j); else nn = cross(pstar+r(j,:)',Fm(:,j)) + ... Nm(:,j); f = Fm(:,j); end R = Rm(:,3*j-2:3*j); if dh_dyn(j,5) == 0, % revolute tau(p,j) = nn'*(R'*z0) + dh_dyn(j,16)*qdd(j)*dh_dyn(j,17)^2; else % prismatic tau(p,j) = f'*(R'*z0) + dh_dyn(j,16)*qdd(j)*dh_dyn(j,17)^2; end end end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 18:28:18.63 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:33:18 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152133.KAA03795@hoiho> Content-transfer-encoding: 7BIT %FDYN Integrate forward dynamics % % [T Q QD] = FDYN(DYN, T0, T1) % [T Q QD] = FDYN(DYN, T0, T1, TORQFUN) % [T Q QD] = FDYN(DYN, T0, T1, TORQFUN, Q0, QD0) % % Integrates the manipulator dynamics over the time interval and returns % vectors of joint position and velocity. A control torque may be specified % by a user specified function % % TAU = TORQFUN(T, X) % % where X is a 2n-element column vector [Q; QD], and T is the current time. % If TORQFUN is not specified then zero torque is applied to the manipulator. % % See also ACCEL, RNE, DYN, ODE45. % Copyright (C) 1993 Peter Corke function [t, q, qd] = fdyn(dyn, t0, t1, torqfun, q0, qd0) global FDYN_DYN FDYN_TORQFUN; n = numrows(dyn); if nargin == 3, FDYN_TORQFUN = 0; x0 = zeros(2*n,1); elseif nargin == 4, FDYN_TORQFUN = torqfun; x0 = zeros(2*n, 1); elseif nargin == 6, FDYN_TORQFUN = torqfun; x0 = [q0(:); qd0(:)]; end FDYN_DYN = dyn; [t,y] = ode45('fdyn2', t0, t1, x0); q = y(:,1:n); qd = y(:,n+1:2*n); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 18:44:09.49 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: mailovi Organization: Date: Mon, 19 Feb 1996 09:15:04 +1300 From: Antonija Mitrovic Subject: mailovi To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602182015.JAA05285@hoiho> Content-transfer-encoding: 7BIT Djoko, bilo je nekih problema sa mailovima (izgleda u Grckoj). J a sam ti u petak poslala M fajlove -- javi da li su stigli. Tanja From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 18:47:49.93 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:14:23 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152114.KAA03546@hoiho> Content-transfer-encoding: 7BIT % % NUMCOLS(m) % % Return the number of columns in the matrix m % function c = numcols(m) [x,c] = size(m); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 18:53:02.93 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:14:55 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152114.KAA03551@hoiho> Content-transfer-encoding: 7BIT %FRICTION compute friction torque % % TAU = FRICTION(QD, B, TC) % TAU = FRICTION(QD, B, TC+, TC-) % TAU = FRICTION(QD, [B TC+ TC-]) % % Copyright (C) 1994 Peter. I. Corke function tau = friction(dh_dyn, qd) [n,nc] = size(dh_dyn); if nc < 18 error('no friction model present'); end if numcols(qd) ~= n error('bad data'); end b = dh_dyn(:,18); if nc >= 19, tcp = dh_dyn(:,19); if nc >= 20, tcm = dh_dyn(:,20); else tcm = -tcp; end else tcp = zeros(size(b)); tcm = tcp; end % refer friction valuesto link b = b.*(dh_dyn(:,17).^2); tcp = tcp.*dh_dyn(:,17); tcm = tcm.*dh_dyn(:,17); tau = qd*diag(b) + (qd>0)*diag(tcp) + (qd<0)*diag(tcm); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 18:56:35.94 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:15:11 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152115.KAA03555@hoiho> Content-transfer-encoding: 7BIT %GRAVLOAD Compute the gravity loading on manipulator joints % % GRAVLOAD(DYN, Q) is the joint gravity loading for a manipulator whose % dynamics and kinematics are described by DYN, and Q is the joint state. % % If Q is a row vector, GRAVLOAD(DYN, Q) is a row vector of joint torques. % If Q is a matrix, each row is interpretted as a joint state vector, and % GRAVLOAD(DYN,Q) is a matrix each row being the corresponding joint % torques. % % GRAVLOAD(DYN,Q,GRAV) allows an arbitrary gravity vector to override % the default of [0; 0; 9.81] % % See also DYN, RNE, ITORQUE, CORIOLIS. % Copright (C) Peter Corke 1993 function tg = gravload(dyn, q, grav) if numcols(q) ~= numrows(dyn), error('') end if nargin == 2, tg = rne(dyn, q, zeros(size(q)), zeros(size(q))); elseif nargin == 3, tg = rne(dyn, q, zeros(size(q)), zeros(size(q)), grav); end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:00:10.91 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:16:51 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152116.KAA03573@hoiho> Content-transfer-encoding: 7BIT %TR2EUL Convert a homogeneous transform matrix to EULER angle form % % [A B C] = TR2EUL(TR) returns a vector of Euler angles corresponding % to the rotational part of the homogeneous transform TR. % % See also EUL2TR, TR2RPY % Copright (C) Peter Corke 1993 function euler = tr2eul(m) euler = zeros(1,3); if abs(m(2,3)) > eps & abs(m(1,3)) > eps, euler(1) = atan2(m(2,3), m(1,3)); sp = sin(euler(1)); cp = cos(euler(1)); euler(2) = atan2(cp*m(1,3) + sp*m(2,3), m(3,3)); euler(3) = atan2(-sp * m(1,1) + cp * m(2,1), -sp*m(1,2) + cp*m(2,2)); else, euler(1) = 0; euler(2) = atan2(m(1,3), m(3,3)); euler(3) = atan2(m(2,1), m(2,2)); end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:03:45.56 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 12:17:05 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152317.MAA03974@hoiho> Content-transfer-encoding: 7BIT From MAILER-DAEMON@cosc.canterbury.ac.nz Fri Feb 16 11:34 NZD 1996 Date: Fri, 16 Feb 1996 10:34:20 +1300 From: Subject: Returned mail: User unknown Message-Id: <199602152134.KAB03822@hoiho> To: tanja@cosc.canterbury.ac.nz MIME-Version: 1.0 Content-Type: multipart/mixed; boundary="KAB03822.824420060/hoiho" Content-Length: 2018 Status: R This is a MIME-encapsulated message --KAB03822.824420060/hoiho The original message was received at Fri, 16 Feb 1996 10:34:19 +1300 from tanja@localhost ----- The following addresses had delivery problems ----- djoak (unrecoverable error) ----- Transcript of session follows ----- ... while talking to cosc.canterbury.ac.nz: >>> RCPT To: <<< 550 ... User unknown 550 djoak... User unknown ----- Original message follows ----- --KAB03822.824420060/hoiho Content-Type: message/rfc822 Date: Fri, 16 Feb 1996 10:34:19 +1300 From: Message-Id: <199602152134.KAA03822@hoiho> To: djoak@cosc.canterbury.ac.nz Content-Length: 1111 %MLINKTRANS Compute the link transform from kinematic parameters % % MLINKTRANS(alpha, an, theta, dn) % MLINKTRANS(DH, q) is a homogeneous % transformation between link coordinate frames. % % alpha is the link twist angle % an is the link length % theta is the link rotation angle % dn is the link offset % sigma is 0 for a revolute joint, non-zero for prismatic % % In the second case, q is substitued for theta or dn according to sigma. % % Based on the modified Denavit and Hartenberg notation. % Copright (C) Peter Corke 1993 function t = mlinktrans(a, b, c, d) if nargin == 4, alpha = a; an = b; theta = c; dn = d; else if numcols(a) < 4, error('too few columns in DH matrix'); end alpha = a(1); an = a(2); if numcols(a) > 4, if a(5) == 0, % revolute theta = b; dn = a(4); else % prismatic theta = a(3); dn = b; end else theta = b; % assume revolute if no sigma given dn = a(4); end end sa = sin(alpha); ca = cos(alpha); st = sin(theta); ct = cos(theta); t = [ ct -st 0 an st*ca ct*ca -sa -sa*dn st*sa ct*sa ca ca*dn 0 0 0 1]; --KAB03822.824420060/hoiho-- From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:15:29.93 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:19:48 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152119.KAA03596@hoiho> Content-transfer-encoding: 7BIT %LINERT Return the manipulator link inertia in matrix form % % LINERT(DYN, L) % % Extract the manipulator link inertia for link L from DYN and return % it in matrix form % Copyright (C) 1994 Peter Corke function J = linert(dyn, l) J = [ dh_dyn(l,10) dh_dyn(l,13) dh_dyn(l,15); ... dh_dyn(l,13) dh_dyn(l,11) dh_dyn(l,14); ... dh_dyn(l,15) dh_dyn(l,14) dh_dyn(l,12) ]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:19:14.92 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:20:04 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152120.KAA03600@hoiho> Content-transfer-encoding: 7BIT %ROTVEC Rotation about arbitrary axis % % ROTVEC(V, THETA) returns a homogeneous transformation representing a % rotation of THETA about the vector V. % % See also ROTX, ROTY, ROTZ. % Copyright (C) Peter Corke 1990 function r = rotvec(v, t) ct = cos(t); st = sin(t); vt = 1-ct; v = v(:); r = [ct -v(3)*st v(2)*st v(3)*st ct -v(1)*st -v(2)*st v(1)*st ct ]; r = [v*v'*vt+r zeros(3,1); 0 0 0 1]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:22:50.48 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:18:25 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152118.KAA03581@hoiho> Content-transfer-encoding: 7BIT %ACCEL Compute manipulator forward dynamics % % QDD = ACCEL(DYN, Q, QD, TORQUE) returns a vector of joint accelerations % that result from applying the actuator TORQUE to the manipulator in state % Q and QD. % % Uses the method 1 of Walker and Orin to compute the forward dynamics. This % form is useful for simulation of manipulator dynamics, in conjunction with % a numerical integration function. % % See also RNE, DYN, ODE45. % Copyright (C) 1993 Peter Corke function qdd = accel(dyn, q, qd, torque) q = q(:)'; qd = qd(:)'; M = inertia(dyn, q); % compute current manipulator inertia tau = rne(dyn, q, qd, zeros(size(q))); % compute gravity and coriolis torque qdd = inv(M) * (torque(:) - tau'); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:26:11.37 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:19:33 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152119.KAA03591@hoiho> Content-transfer-encoding: 7BIT %STANFORD Load kinematic and dynamic data for Stanford arm % % Defines the matrix 'stanford' which describes the kinematic % characterstics of the Stanford (Scheinman) arm. % % Kinematic data from "Modelling, Trajectory calculation and Servoing of % a computer controlled arm". Stanford AIM-177. Figure 2.3 % Dynamic data from "Robot manipulators: mathematics, programming and control" % Paul 1981, Tables 6.4, 6.6 % % Note: gear ratios not currently known, though reflected armature inertia is known, % so gear ratios set to 1. % % See also DH, DYN, PUMA560, TWOLINK. % % Copright (C) Peter Corke 1990 % alpha A theta D sigma m rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm G stanf = [ -pi/2 0 0 0.412 0 9.29 0 .0175 -0.1105 0.276 0.255 0.071 0 0 0 0.953 1 pi/2 0 0 0.154 0 5.01 0 -1.054 0 0.108 0.018 0.100 0 0 0 2.193 1 0 0 -pi/2 0 1 4.25 0 0 -6.447 2.51 2.51 0.006 0 0 0 0.782 1 -pi/2 0 0 0 0 1.08 0 0.092 -0.054 0.002 0.001 0.001 0 0 0 0.106 1 pi/2 0 0 0 0 0.63 0 0 0.566 0.003 0.003 0.0004 0 0 0 0.097 1 0 0 0 0.263 0 0.51 0 0 1.554 0.013 0.013 0.0003 0 0 0 0.020 1 ]; qz = [0 0 0 0 0 0]; qr = [0 -pi/2 pi/2 0 0 0]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:29:52.52 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:20:47 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152120.KAA03611@hoiho> Content-transfer-encoding: 7BIT echo on % % 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)) pause % any key to continue % % 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(p560, 0, 2); toc % % 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. pause % hit any key to continue % % This can be shown in animation also plotbot(p560, q) pause % hit any key to continue echo off From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:33:28.00 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:21:21 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152121.KAA03617@hoiho> Content-transfer-encoding: 7BIT echo on % % 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) % % a rotation of 90degrees about the Y axis by roty(pi/2) % % and a rotation of -90degrees about the Z axis by rotz(-pi/2) % % these may be concatenated by multiplication t = transl(0.5, 0.0, 0.0) * roty(pi/2) * rotz(-pi/2) % % 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]' pause % any key to continue % % the orientation of the new coordinate frame may be expressed in terms of % Euler angles tr2eul(t) % % or roll/pitch/yaw angles tr2rpy(t) pause % 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) rotz(-pi/8) * rotx(pi/2) % % pause % any key to continue echo off From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:44:01.88 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:24:10 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152124.KAA03657@hoiho> Content-transfer-encoding: 7BIT %TTG Return the i'th homogeneous transform from a trajectory % % TI = TTG(T, I) returns the I'th homogeneous transform from a trajectory % matrix T. % % See also CTRAJ % Copright (C) Peter Corke 1995 function T = ttg(TG, i) T = reshape(TG(i,:), 4,4); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:47:39.09 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:26:24 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152126.KAA03675@hoiho> Content-transfer-encoding: 7BIT %QNORM Normalize a quaternion % % QN = qnorm(Q) % % Return a unit-quaternion corresponding to the quaternion Q. % % See also TR2Q % Copyright (C) 1993 Peter Corke function qn = qnorm(q) qn = q / norm(q); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:50:58.83 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:25:39 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152125.KAA03662@hoiho> Content-transfer-encoding: 7BIT %TRANSL Translational transform % % T= TRANSL(X, Y, Z) % T= TRANSL( [X Y Z] ) % % [X Y Z]' = TRANSL(T) % % [X Y Z] = TRANSL(TG) % % Returns a homogeneous transformation representing a % translation of X, Y and Z. % % The third form returns the translational part of a % homogenous transform as a 3-element column vector. % % The fourth form returns a matrix of the X, Y and Z elements % extracted from a Cartesian trajectory matrix TG. % % See also ROTX, ROTY, ROTZ, ROTVEC. % Copyright (C) Peter Corke 1990 function r = transl(x, y, z) if nargin == 1, if ishomog(x), r = x(1:3,4); elseif numcols(x) == 16, r = x(:,13:15); else t = x(:); r = [eye(3) t; 0 0 0 1]; end elseif nargin == 3, t = [x; y; z]; r = [eye(3) t; 0 0 0 1]; end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 19:54:07.45 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:27:15 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152127.KAA03689@hoiho> Content-transfer-encoding: 7BIT %DRIVEPAR Compute Cartesian path drive parameters % % DP = DRIVEPAR(T0, T1) returns a vector of drive parameters required % to plan a Cartesian path between T0 and T1. % % Inputs: T0 homogeneous transform of initial position % T1 homogeneous transform of final position % % Robot manipulators: mathematics, programming and control % R.P. Paul, MIT Press, 1981. % % See also CTRAJ, TRINTERP. % Copyright (C) 1993 Peter Corke function dp = drivepar(t1, t2) % Using the notation of Paul, dp is % dp(1) x % dp(2) y % dp(3) z % dp(4) phi % dp(5) theta % dp(6) psi dp = zeros(1, 6); n1 = t1(1:3,1); o1 = t1(1:3,2); a1 = t1(1:3,3); p1 = t1(1:3,4); n2 = t2(1:3,1); o2 = t2(1:3,2); a2 = t2(1:3,3); p2 = t2(1:3,4); dp(1) = n1'*(p2 - p1); dp(2) = o1'*(p2 - p1); dp(3) = a1'*(p2 - p1); dp(6) = atan2(o1'*a2, n1'*a2); % psi sp = sin(dp(4)); cp = cos(dp(4)); dp(5) = atan2(sqrt((n1'*a2)^2 + (o1'*a2)^2), a1'*a2); % theta st = sin(dp(5)); ct = cos(dp(5)); vt = 1-ct; dp(4) = atan2(-sp*cp*vt*(n1'*n2)+ ... % phi (cp^2*vt+ct)*(o1'*n2)-sp*st*(a1'*n2), ... -sp*cp*vt*(n1'*o2)+ ... (cp^2*vt+ct)*(o1'*o2)-sp*st*(a1'*o2)); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:03:04.56 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:20:22 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152120.KAA03605@hoiho> Content-transfer-encoding: 7BIT echo on % 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 % % 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) % % returns the homogeneous transform corresponding to the last link of the manipulator pause % 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 has one row for each time step, and that row is a `flattened' 4x4 homogeneous % tranform, which can be restored to its original shape using reshape(). For % example, the first point is reshape(T(1,:), 4,4) % % and the tenth point is reshape(T(10,:), 4,4) pause % any key to continue % % Columns 13, 14 and 15 of t correspond to the X, Y and Z coordinates respectively, and % may be plotted against time subplot(3,1,1) plot(t, T(:,13)) xlabel('Time (s)'); ylabel('X (m)') subplot(3,1,2) plot(t, T(:,14)) xlabel('Time (s)'); ylabel('Y (m)') subplot(3,1,3) plot(t, T(:,15)) 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(T(:,13), T(:,15)) xlabel('X (m)') ylabel('Z (m)') grid pause % any key to continue echo off From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:06:23.58 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Mon, 19 Feb 1996 09:08:48 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602182008.JAA05256@hoiho> Content-transfer-encoding: 7BIT \documentclass[12pt,a4paper]{article} \usepackage{times} \textheight 24cm \textwidth 16cm \oddsidemargin 0cm \topmargin 0cm \pagestyle{empty} \begin{document} \noindent Scholarships office, Registry \\ University of Canterbury \vskip 2cm \centerline{Reference for Mark Dunlop} \vskip 1cm \noindent Re: Freemasons Postgraduate Scholarship \vskip 2cm I have known Mark Dunlop for several months only, since this is my first year at the University of Canterbury. He took COSC801 (Machine Learning), which is the Honours course I teach. He is a highly intelligent and hard-working student. Although the material we covered in the course was quite theoretical and abstract, he was able to focus on interesting points and contributed very much to class discussions. He showed great curiosity and motivation for learning. Furthermore, he presented his views very clearly and even developed very interesting ideas of how to improve some existing solutions and how to overcome some problems in the area. All his assignments were finished in time. One of the assessments in the course included writing and essay. His writing style is excellent. He was able to choose a suitable topic, explore it and present the conclusions in a good way. I was impressed with his performance in the course, which resulted in the highest grade. Apart from this, I have also been involved in his MSc project, which is still in an early stage, as a co-supervisor. Due to his health problems, we have not had the opportunity to spend a lot of time on it, but we had a few discussions on potential directions for his research which were very promising. To conclude, I have a very high opinion on his intellectual abilities and research skills. Therefore, I recommend Mark Dunlop for the Freemasons Postgraduate Scholarship wholeheartedly. I would be happy to provide additional comments if needed. \vskip 2cm \noindent Dr Antonija Mitrovi\'{c} \\ \\ Computer Science Department \\ University of Canterbury \\ phone 6348 \\ email tanja@cosc \end{document} From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:09:55.54 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:23:13 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152123.KAA03641@hoiho> Content-transfer-encoding: 7BIT %PUMA560 Load kinematic and dynamic data for a Puma 560 manipulator % % Defines the matrix 'p560' which describes the kinematic and dynamic % characterstics of a Unimation Puma 560 manipulator. % Specifies armature inertia and gear ratios. % % See also DH, DYN, TWOLINK, STANFORD. % % Notes: % - the value of m1 is given as 0 here. Armstrong found no value for it % and it does not appear in the equation for tau1 after the substituion % is made to inertia about link frame rather than COG frame. % updated: % 29/1/91 to agree with data from Armstrong etal. Due to their use % of modified D&H params, some of the offsets Ai, Di are % offset, and for links 3-5 swap Y and Z axes. % 14/2/91 to use Paul's value of link twist (alpha) to be consistant % with ARCL. This is the -ve of Lee's values, which means the % zero angle position is a righty for Paul, and lefty for Lee. % Note that gravity load torque is the motor torque necessary % to keep the joint static, and is thus -ve of the gravity % caused torque. % % Copright (C) Peter Corke 1990 % alpha A theta D sigma m rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm G B Tc+ Tc- p560 = [ pi/2 0 0 0 0 0 0 0 0 0 0.35 0 0 0 0 200e-6 -62.6111 1.48e-3 .395 -.435 0 .4318 0 0 0 17.4 -.3638 .006 .0774 .13 .524 .539 0 0 0 200e-6 107.815 .817e-3 .126 -.071 -pi/2 .0203 0 .1254 0 4.8 0 -.014 .070 .066 .086 .0125 0 0 0 200e-6 -53.7063 1.38e-3 .132 -.105 pi/2 0 0 .4318 0 0.82 0 .019 0 1.8e-3 1.3e-3 1.8e-3 0 0 0 33e-6 76.0364 71.2e-6 11.2e-3 -16.9e-3 -pi/2 0 0 0 0 0.34 0 0 0 .3e-3 .4e-3 .3e-3 0 0 0 33e-6 71.923 82.6e-6 9.26e-3 -14.5e-3 0 0 0 0 0 .09 0 0 .032 .15e-3 .15e-3 .04e-3 0 0 0 33e-6 76.686 36.7e-6 3.96e-3 -10.5e-3 ]; % % some useful poses % qz = [0 0 0 0 0 0]; % zero angles, L shaped pose qr = [0 pi/2 -pi/2 0 0 0]; % ready pose, arm up qstretch = [0 0 -pi/2 0 0 0]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:13:58.45 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:25:53 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152125.KAA03666@hoiho> Content-transfer-encoding: 7BIT %TRNORM Normalize a homogeneous transformation. % % TN = TRNORM(T) % % Returns a normalized homogeneous tranformation matrix in which the rotation % submatrix is a proper orthogonal matrix. % The O and V vectors are normalized and the normal vector is formed from % O x A. % % Finite word length arithmetic can cause transforms to become `unnormalized'. % % See also OA2TR % Copyright (C) 1993 Peter Corke function r = trnorm(t) n = cross(t(1:3,2), t(1:3,3)); % N = O x A r = [unit(n) unit(t(1:3,2)) unit(t(1:3,3)) t(1:3,4); 0 0 0 1]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:18:01.67 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:16:38 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152116.KAA03569@hoiho> Content-transfer-encoding: 7BIT %ROTZ Rotation about Z axis % % ROTZ(theta) returns a homogeneous transformation representing a % rotation of theta about the X axis. % % See also ROTX, ROTY, ROTVEC. % Copyright (C) Peter Corke 1990 function r = rotz(t) ct = cos(t); st = sin(t); r = [ct -st 0 0 st ct 0 0 0 0 1 0 0 0 0 1]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:21:34.90 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:28:13 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152128.KAA03703@hoiho> Content-transfer-encoding: 7BIT %ISHOMOG test if argument is a homogeneous transformation function h = ishomog(tr) h = all(size(tr) == [4 4]); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:25:00.94 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:23:37 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152123.KAA03647@hoiho> Content-transfer-encoding: 7BIT %ROTY Rotation about Y axis % % ROTY(theta) returns a homogeneous transformation representing a % rotation of theta about the Y axis. % % See also ROTX, ROTZ, ROTVEC. % Copyright (C) Peter Corke 1990 function r = roty(t) ct = cos(t); st = sin(t); r = [ct 0 st 0 0 1 0 0 -st 0 ct 0 0 0 0 1]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:28:35.48 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:23:58 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152123.KAA03653@hoiho> Content-transfer-encoding: 7BIT %OA2TR % % OA2TR(O, A) returns a homogeneous tranformation for the specified % orientation and approach vectors. % % See also RPY2TR, EUL2TR % Copyright (C) 1993 Peter Corke function r = oa2tr(o, a) n = cross(o, a); r = [unit(n(:)) unit(o(:)) unit(a(:)) zeros(3,1); 0 0 0 1]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:31:59.87 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:26:38 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152126.KAA03680@hoiho> Content-transfer-encoding: 7BIT %QINV Invert a unit-quaternion % % QI = qinv(Q) % % Return the inverse of the unit-quaternion Q. % % See also QQMUL % Copyright (C) 1993 Peter Corke function qi = qinv(q) qi = [q(1) -q(2:4)] / sum(q.^2); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:35:24.48 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:27:37 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152127.KAA03694@hoiho> Content-transfer-encoding: 7BIT %Q2TR Convert unit-quaternion to homogeneous transform % % T = q2tr(Q) % % Return the rotational homogeneous transform corresponding to the unit % quaternion Q. % % See also TR2Q % Copyright (C) 1993 Peter Corke function t = q2tr(q) s = q(1); x = q(2); y = q(3); z = q(4); r = [ 1-2*(y^2+z^2) 2*(x*y-s*z) 2*(x*z+s*y) 2*(x*y+s*z) 1-2*(x^2+z^2) 2*(y*z-s*x) 2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2) ]; t = eye(4,4); t(1:3,1:3) = r; t(4,4) = 1; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:38:59.89 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:26:58 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152126.KAA03684@hoiho> Content-transfer-encoding: 7BIT %RPY2TR Roll/pitch/yaw to homogenous transform % % RPY2TR([R P Y]) % RPY2TR(R,P,Y) returns a homogeneous tranformation for the specified % roll/pitch/yaw angles. These correspond to rotations about the % Z, X, Y axes respectively. % % See also TR2RPY, EUL2TR % Copright (C) Peter Corke 1993 function r = rpy2tr(roll, pitch, yaw) if length(roll) == 3, r = rotz(roll(1)) * roty(roll(2)) * rotx(roll(3)); else r = rotz(roll) * roty(pitch) * rotx(yaw); end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:46:09.87 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:29:05 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152129.KAA03716@hoiho> Content-transfer-encoding: 7BIT % % NUMROWS(m) % % Return the number of rows in the matrix m % function r = numrows(m) [r,x] = size(m); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:49:31.92 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:26:09 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152126.KAA03671@hoiho> Content-transfer-encoding: 7BIT %JACOB0 Compute manipulator Jacobian in world coordinates % % JACOB0(DH, Q) returns a Jacobian matrix for the current pose Q. % % The manipulator Jacobian matrix maps differential changes in joint space % to differential Cartesian motion (world coord frame) of the end-effector. % dX = J dQ % % For an n-axis manipulator the Jacobian is a 6 x n matrix. % % See also JACOBN, DIFF2TR, TR2DIFF, DIFF % Copyright (C) Peter Corke 1993 function J0 = jacob0(dh, q) % % dX_tn = Jn dq % Jn = jacobn(dh, q); % Jacobian from joint to wrist space % % convert to Jacobian in base coordinates % Tn = fkine(dh, q); % end-effector transformation J0 = [Tn(1:3,1:3) zeros(3,3); zeros(3,3) Tn(1:3,1:3)] * Jn; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:53:09.37 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:22:14 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152122.KAA03629@hoiho> Content-transfer-encoding: 7BIT %TR2Q Convert homogeneous transform to a unit-quaternion % % Q = tr2q(T) % % Return a unit quaternion corresponding to the rotational part of the % homogeneous transform T. % % See also Q2TR % Copyright (C) 1993 Peter Corke function q = tr2q(t) q = zeros(1,4); q(1) = sqrt(trace(t))/2; kx = t(3,2) - t(2,3); % Oz - Ay ky = t(1,3) - t(3,1); % Ax - Nz kz = t(2,1) - t(1,2); % Ny - Ox if (t(1,1) >= t(2,2)) & (t(1,1) >= t(3,3)) kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1 ky1 = t(2,1) + t(1,2); % Ny + Ox kz1 = t(3,1) + t(1,3); % Nz + Ax add = (kx >= 0); elseif (t(2,2) >= t(3,3)) kx1 = t(2,1) + t(1,2); % Ny + Ox ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1 kz1 = t(3,2) + t(2,3); % Oz + Ay add = (ky >= 0); else kx1 = t(3,1) + t(1,3); % Nz + Ax ky1 = t(3,2) + t(2,3); % Oz + Ay kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1 add = (kz >= 0); end if add kx = kx + kx1; ky = ky + ky1; kz = kz + kz1; else kx = kx - kx1; ky = ky - ky1; kz = kz - kz1; end nm = norm([kx ky kz]); s = sqrt(1 - q(1)^2) / nm; q(2:4) = s*[kx ky kz]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 20:56:47.70 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:32:20 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152132.KAA03772@hoiho> Content-transfer-encoding: 7BIT %DYN Matrix representation of manipulator kinematics and dynamics % % Many robot toolbox functions take a DYN matrix which describes the % kinematics and dynamics of a manipulator in a general way. % % For an n-axis manipulator, DYN is an nx17 matrix, whose rows comprise % % 1 alpha link twist angle % 2 A link length % 3 theta link rotation angle % 4 D link offset distance % 5 sigma joint type, 0 for revolute, non-zero for prismatic % 6 mass mass of the link % 7 rx link COG with respect to the link coordinate frame % 8 ry % 9 rz % 10 Ixx elements of link inertia tensor about the link COG % 11 Iyy % 12 Izz % 13 Ixy % 14 Iyz % 15 Ixz % 16 Jm armature inertia % 17 G reduction gear ratio. joint speed/link speed % 18 B viscous friction, motor refered % 19 Tc+ coulomb friction (positive rotation), motor refered % 20 Tc- coulomb friction (negative rotation), motor refered % % The first 5 columns of a DYN matrix contain the kinematic parameters % and maybe used anywhere that a DH kinematic matrix is required -- the % dynamic data is ignored. % % See also DH. % Copright (C) Peter Corke 1993 % MOD.HISTORY % 1/95 reverse labels on A & D From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 21:18:25.74 To: GORANDJ@efnis.elfak.ni.ac.yu CC: Subj: Re: Ponvovo Djole! Organization: Date: Fri, 16 Feb 1996 09:52:10 +1300 From: Antonija Mitrovic Subject: Re: Ponvovo Djole! To: GORANDJ@efnis.elfak.ni.ac.yu Message-id: <199602152052.JAA03506@hoiho> Content-transfer-encoding: 7BIT Zdravo Djole, to sto su stigle samo informacije, a ne programi, jeste zato sto janisam htela da skidam programe (ogromni fajlovi!) dok ne budes siguran da ti to treba. A posto sam videla da je to za Mac, mislim i da neces biti u stanju da ih koristis. Ono sto pise [Image] je zato sto sam ja to pamtila kao Text, a ne kao www source. Probacu da ti skinem jos nesto. Tanja From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 21:29:09.95 To: GORANDJ@efnis.elfak.ni.ac.yu CC: Subj: spisak Organization: Date: Thu, 15 Feb 1996 12:48:40 +1300 From: Antonija Mitrovic Subject: spisak To: GORANDJ@efnis.elfak.ni.ac.yu Message-id: <199602142348.MAA00525@kaka.cosc.canterbury.ac.nz> Content-transfer-encoding: 7BIT Djoko, poslala sam ti danas (avionskom postom) 3 rada sa WWW sa onog spiska. Prekjuce sam ti slala neke fajlove - javi mi sta si dobio i sta mislis o tome. Ne znam koliko cu jos stici da ti posaljem, zato javi ako imas neke preference. Ona prva referenca koju si dao nije u redu, ne mogu da doprem do tamo. Proveri. Tanja From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 21:36:17.95 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:29:23 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152129.KAA03720@hoiho> Content-transfer-encoding: 7BIT %**************************animation******************************************** echo on % % 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 function plotbot() animates a stick figure robot moving along a trajectory. plotbot(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. The axes X, Y and Z are represented by colors red, green % and blue respectively. pause % any key to continue % There are a number of options, one of which is to inhibit erasure, which leaves % a trail of robots behind plotbot(p560, q, 'l') pause % any key to continue echo off From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 21:50:47.02 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:15:43 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152115.KAA03560@hoiho> Content-transfer-encoding: 7BIT %IKINE Inverse manipulator kinematics % % Q = IKINE(DH, T) % Q = IKINE(DH, T, Q) % Q = IKINE(DH, T, Q, M) % % Returns the joint coordinates corresponding to the % end-effector transform T. Note that the inverse kinematic solution is % generally not unique, and depends on the initial guess Q (which % defaults to 0). % % Q = IKINE(DH, TG) % Q = IKINE(DH, TG, Q) % Q = IKINE(DH, TG, Q, M) % % Returns the joint coordinates corresponding to % each of the transforms in flattened form which are the rows of TG. % Returns one row of Q for each input transform. The initial estimate % of Q for each time step is taken as the solution from the previous % time step. % % If the manipulator has fewer than 6 DOF then this method of solution % will fail, since the solution space has more dimensions than can % be spanned by the manipulator joint coordinates. In such a case % it is necessary to provide a mask matrix, M, which specifies the % Cartesian DOF (in the wrist coordinate frame) that will be ignored % in reaching a solution. The mask matrix has six elements that % correspond to translation in X, Y and Z, and rotation about X, Y and % Z respectively. The value should be 0 (for ignore) or 1. The number % of non-zero elements should equal the number of manipulator DOF. % % Solution is computed iteratively using the pseudo-inverse of the % manipulator Jacobian. % % Such a solution is completely general, though much less efficient % than specific inverse kinematic solutions derived symbolically. % % This approach allows a solution to obtained at a singularity, but % the joint angles within the null space are arbitrarily assigned. % % For instance with a typical 5 DOF manipulator one would ignore % rotation about the wrist axis, that is, M = [1 1 1 1 1 0]. % % % See also FKINE, TR2DIFF, JACOB0. % Copyright (C) 1993 Peter Corke % MOD.HISTORY % 2/95 use new 2-argument version of tr2diff(), cleanup function qt = ikine(dh, tr, q, m) % % solution control parameters % ilimit = 1000; stol = 1e-12; n = numrows(dh); if nargin == 2, q = zeros(n, 1); else q = q(:); end if nargin == 4, m = m(:); if numrows(m) ~= 6 error('Mask matrix must have 6 elements') end else if numrows(dh) < 6, disp('For a manipulator with fewer than 6DOF a mask matrix argument should be specified'); end m = ones(6, 1); end tcount = 0; if ishomog(tr), % single xform case nm = 1; count = 0; while nm > stol, e = tr2diff(fkine(dh, q'), tr) .* m; dq = pinv( jacob0(dh, q) ) * e; q = q + dq; nm = norm(dq); count = count+1; if count > ilimit, error('Solution wouldn''t converge') end end qt = q'; else % trajectory case for i=1:numrows(tr), nm = 1; T = reshape(tr(i,:), 4,4); count = 0; while nm > stol, e = tr2diff(fkine(dh, q'), T) .* m; dq = pinv( jacob0(dh, q) ) * e; q = q + dq; nm = norm(dq); count = count+1; if count > ilimit, error('Solution wouldn''t converge') end end qt = [qt; q']; tcount = tcount + count; end end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 21:54:29.83 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:22:35 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152122.KAA03633@hoiho> Content-transfer-encoding: 7BIT echo on % % 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] 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' % % Compared with the original value q % % 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' % % which is not the same as the original joint angle qr pause % any key to continue % % However both result in the same end-effector position fkine(p560, qi) - fkine(p560, qr) pause % 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 point T2 = transl(0.4, 0.5, 0.2) % and destination T = ctraj(T1, T2, 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 % % 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)') pause % hit any key to continue % This joint space trajectory can now be animated plotbot(p560, q) pause % any key to continue From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:03:16.01 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:21:35 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152121.KAA03621@hoiho> Content-transfer-encoding: 7BIT echo on % % 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) pause % 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; DT' % % 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; % % 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 0] J = jacob0(p560, q) % % or the T6 coordinate frame J = jacobn(p560, q) % % 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 singular det(J) % % and may be inverted Ji = inv(J) pause % 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 direction qvel = Ji * vel; qvel' % % 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 qvel' % % 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 Jacobian rank( jacobn(p560, qr) ) % % and the singular values are svd( jacobn(p560, qr) ) pause % 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 % Yoshikawa maniplty(p560(:,1:5), q) % % 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) % % 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 From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:17:24.87 To: GORANDJ@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:10:14 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu Message-id: <199602152110.KAA01605@kaka.cosc.canterbury.ac.nz> Content-transfer-encoding: 7BIT %UNIT Unitize a vector % % UNIT(V) returns a unit vector aligned with V. % Copright (C) Peter Corke 1990 function u = unit(v) u = v / norm(v,'fro'); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:20:48.46 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:30:09 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152130.KAA03733@hoiho> Content-transfer-encoding: 7BIT %QQMUL Multiply unit-quaternion by unit-quaternion % % QQ = qqmul(Q1, Q2) % % Return a product of unit-quaternions. % % See also TR2Q % Copyright (C) 1993 Peter Corke function qq = qqmul(q1, q2) % decompose into scalar and vector components s1 = q1(1); v1 = q1(2:4); s2 = q2(1); v2 = q2(2:4); % form the product qq = [s1*s2-v1*v2' s1*v2+s2*v1+cross(v1,v2)']; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:24:20.87 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:32:56 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152132.KAA03786@hoiho> Content-transfer-encoding: 7BIT %EUL2TR % % EUL2TR([R P Y]) % EUL2TR(R,P,Y) returns a homogeneous tranformation for the specified % Euler angles. These correspond to rotations about the % Z, X, Z axes respectively. % % See also TR2EUL, RPY2TR % Copright (C) Peter Corke 1993 function r = eul2tr(phi, theta, psi) if length(phi) == 3, r = rotz(phi(1)) * roty(phi(2)) * rotz(phi(3)); else r = rotz(phi) * roty(theta) * rotz(psi); end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:31:29.88 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:33:59 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152133.KAA03813@hoiho> Content-transfer-encoding: 7BIT function xd = fdyn2(t, x) global FDYN_DYN FDYN_TORQFUN; t n = numrows(FDYN_DYN); if isstr(FDYN_TORQFUN) tau = feval(FDYN_TORQFUN, t, x); else tau = zeros(n,1); end qdd = accel(FDYN_DYN, x(1:n,1), x(n+1:2*n,1), tau); xd = [x(n+1:2*n,1); qdd]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:35:05.89 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:34:10 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152134.KAA03818@hoiho> Content-transfer-encoding: 7BIT %DIFF2TR Convert a differential to a homogeneous transform % % DIFF2TR(D) returns a homogeneous transform representing differential % translation and rotation. % % See also TR2DIFF, DIFF % Copyright (C) Peter Corke 1993 function t = diff2tr(d) t =[ 0 -d(6) d(5) d(1) d(6) 0 -d(4) d(2) -d(5) d(4) 0 d(3) 0 0 0 0 ]; From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:38:41.88 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:33:30 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152133.KAA03800@hoiho> Content-transfer-encoding: 7BIT %ITORQUE Compute the manipulator inertia torque % % ITORQUE(DYN, Q, QDD) for an n-axis manipulator returns the n element % inertia torque vector at the specified pose and acceleration, that is, % INERTIA(Q)*QDD % DYN describes the manipulator dynamics and kinematics. % If Q and QDD are row vectors, ITORQUE(DYN,Q,QDD) is a row vector % of joint torques. % If Q and QDD are matrices, each row is interpretted as a joint state % vector, and ITORQUE(DYN,Q,QDD) is a matrix each row being the % corresponding joint torques. % % See also DYN, RNE, CORIOLIS, INERTIA, GRAVLOAD. % Copright (C) Peter Corke 1993 function it = itorque(dyn, q, qdd) it = rne(dyn, q, zeros(size(q)), qdd, [0;0;0]); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:45:53.89 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:34:31 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152134.KAA03827@hoiho> Content-transfer-encoding: 7BIT %JACOBN Compute manipulator Jacobian in end-effector frame % % JACOBN(DH, Q) returns a Jacobian matrix for the current pose Q. % % The manipulator Jacobian matrix maps differential changes in joint space % to differential Cartesian motion of the end-effector. % dX = J dQ % % This function uses the technique of % Paul, Shimano, Mayer % Differential Kinematic Control Equations for Simple Manipulators % IEEE SMC 11(6) 1981 % pp. 456-460 % % For an n-axis manipulator the Jacobian is a 6 x n matrix. % % See also DIFF2TR, TR2DIFF, DIFF % Copyright (C) Peter Corke 1993 function J = jacobn(dh, q) J = []; n = numrows(dh); T = eye(4,4); for i=n:-1:1, T = linktran(dh(i,:), q(i)) * T; if dh(i,5) == 0, % revolute axis d = [ -T(1,1)*T(2,4)+T(2,1)*T(1,4) -T(1,2)*T(2,4)+T(2,2)*T(1,4) -T(1,3)*T(2,4)+T(2,3)*T(1,4)]; delta = T(3,1:3)'; % nz oz az else % prismatic axis d = T(3,1:3)'; % nz oz az delta = zeros(3,1); % 0 0 0 end J = [[d; delta] J]; end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:49:25.58 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:30:23 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152130.KAA03738@hoiho> Content-transfer-encoding: 7BIT %MANIPLTY Manipulability measure % % M = MANIPLTY(DH, Q) % M = MANIPLTY(DYN, Q) % % Computes the manipulability index for the manipulator % at the given pose. % % For an n-axis manipulator Q may be an n-element vector, or an m x n % joint space trajectory. % % If Q is a vector MANIPLTY returns a scalar manipulability index. % If Q is a matrix MANIPLTY returns a column vector of % manipulability indices for each pose specified by Q. % % The first form computes Yoshikawa's manipulability measure which % gives an indication of how far the manipulator is from singularities % and thus able to move and exert forces uniformly in all directions. % % The second form computes Asada's manipulability measure based on % the Cartesian manipulator inertia matrix. An n-dimensional % inertia ellipsoid % X' M(q) X = 1 % gives an indication of how well the manipulator can accelerate % in each of the Cartesian directions. The scalar measure computed % here is the ratio of the smallest/largest ellipsoid axis. Ideally % the ellipsoid would be spherical, giving a ratio of 1, but in % practice will be less than 1. % % See also INERTIA, JACOB0. % Copyright (C) 1993 Peter Corke function w = maniplty(dyn, q) n = numrows(dyn); if numcols(dyn) > 5, % use Asada's measure if length(q) == n, J = jacob0(dyn, q); Ji = inv(J); M = inertia(dyn, q); Mx = Ji' * M * Ji; e = eig(Mx); w = min(e) / max(e); else w = []; for Q = q', J = jacob0(dyn, Q); Ji = inv(J); M = inertia(dyn, Q); Mx = Ji' * M * Ji; e = sqrt(eig(Mx)); w = [w; min(e)/max(e)]; end end else % use Yoshikawa's measure if length(q) == n, J = jacob0(dyn, q); w = sqrt(det(J * J')); else w = []; for Q = q', J = jacob0(dyn, Q); w = [w; sqrt(det(J * J'))]; end end end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 22:53:08.02 To: GORANDJ@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:10:00 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu Message-id: <199602152110.KAA01600@kaka.cosc.canterbury.ac.nz> Content-transfer-encoding: 7BIT % % crude attempt at changing the viewing angles. Run this script after plotbot % and it pops up a couple of sliders to allow changing the viewpoint. Should % be an integral part of plotbot, along with rewind button... % fig = gcf; %clf sli_azm = uicontrol(fig, 'Style', 'slider', ... 'Position', [50 50 120 20], ... 'Min', -90, 'Max', 90, 'Value', 30, ... 'Callback', [ ... 'set(azm_cur,''String'',',... 'num2str(get(sli_azm,''Val''))),',... 'set(gca,''View'',',... '[get(sli_elv,''Val''),get(sli_azm,''Val'')])']); sli_elv = uicontrol(fig, 'Style', 'slider', ... 'Position', [240 50 120 20], ... 'Min', -90, 'Max', 90, 'Value', 30, ... 'Callback', [ ... 'set(elv_cur,''String'',',... 'num2str(get(sli_elv,''Val''))),',... 'set(gca,''View'',',... '[get(sli_azm,''Val''),get(sli_elv,''Val'')])']); azm_min = uicontrol(fig,... 'Style', 'text', ... 'Pos', [210 50 30 20], ... 'String', num2str(get(sli_elv, 'Min'))); elv_min = uicontrol(fig,... 'Style', 'text', ... 'Pos', [210 50 30 20], ... 'String', num2str(get(sli_elv, 'Min'))); azm_max = uicontrol(fig,... 'Style', 'text', ... 'Pos', [170 50 30 20], ... 'String', num2str(get(sli_elv, 'Max'))); elv_max = uicontrol(fig,... 'Style', 'text', ... 'Pos', [360 50 30 20], ... 'String', num2str(get(sli_elv, 'Max'))); azm_label = uicontrol(fig, ... 'Style', 'text', ... 'Pos', [50 80 65 20], ... 'String', 'Azimuth'); elv_label = uicontrol(fig, ... 'Style', 'text', ... 'Pos', [240 80 65 20], ... 'String', 'Elevation'); azm_cur = uicontrol(fig, ... 'Style', 'text', ... 'Pos', [120 80 50 20], ... 'String', num2str(get(sli_azm, 'Value'))); elv_cur = uicontrol(fig, ... 'Style', 'text', ... 'Pos', [310 80 50 20], ... 'String', num2str(get(sli_elv, 'Value'))); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 19-FEB-1996 23:00:26.19 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:15:58 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152115.KAA03564@hoiho> Content-transfer-encoding: 7BIT %IKINE560 Inverse kinematics for Puma 560 % % % Q = IKINE560(DH, T, CONFIG) % % This routine allows the caller to choose from a variety of outputs % If the user only passes dh and Homogeneous_Transformation, the inverse % kinematics will default to a configuration of n1 = 1, n2 = -1, n4= -1. % The user may opt to pass a third argument which specifies the configuration % of the arm. % He may either specify the values of n1, n2, n4 using a (1x3) % vector, [n1, n2, n4], % or the user may opt to use the right-left, up-down, flip-no flip conventions. % In that case, a (1x3) vector with the characters will be passed ['r','u','f'] % Any combination of uppercase and lowercase is allowed. % However, the user must pass the parameters as separate elements in the % proper order: right or left first, followed by up or down and finally flip % or no-flip. % There is an additional option to return all eight solutions- done by % sending the scalar 8. % If the users passes anyvalue other than a (3x1) vector or the scalar 8, % the default will be used. % % The Denavit-Hartenberg parameters from the dh matrix are passed in. % Therefore, puma560.m must be run first. % The DH parameters are then taken out of the dh matrix % % REFERENCE: % % Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang % From The International Journal of Robotics Research % Vol. 5, No. 2, Summer 1986, p. 32-44 % % % AUTHOR: % Robert Biro gt2231a@prism.gatech.edu % with Gary Von McMurray % % GTRI/ATRP/IIMB % Georgia Institute of Technology % 2/13/95 function invkine= ikine560(p560, Homogeneous_Transformation,configuration) a1 = p560(1,2); a2 = p560(2,2); a3 = p560(3,2); a4 = p560(4,2); a5 = p560(5,2); a6 = p560(6,2); d1 = p560(1,4); d2 = p560(2,4); d3 = p560(3,4); d4 = p560(4,4); d5 = p560(5,4); d6 = p560(6,4); % The Homogeneous Transformation may be passed as either a row vector % or a 4x4 matrix size_Homogeneous_Transformation=size(Homogeneous_Transformation); if (size_Homogeneous_Transformation(1) == 1), Homogeneous_Transformation= [Homogeneous_Transformation(1:4); ... Homogeneous_Transformation(5:8); ... Homogeneous_Transformation(9:12); ... Homogeneous_Transformation(13:16)]'; end % The following parameters are extracted from the Homogeneous Transformation % as defined in equation 1, p. 34 Ox = Homogeneous_Transformation(1,2); Oy = Homogeneous_Transformation(2,2); Oz = Homogeneous_Transformation(3,2); Ax = Homogeneous_Transformation(1,3); Ay = Homogeneous_Transformation(2,3); Az = Homogeneous_Transformation(3,3); Px = Homogeneous_Transformation(1,4); Py = Homogeneous_Transformation(2,4); Pz = Homogeneous_Transformation(3,4); % The configuration parameter determines what n1,n2,n4 values are used % and how many solutions are determined. % If a 1x3 vector is passed, it is assumed that a specific configuration is desired % If the scalar 8 is passed, all eight configurations are returned in a (8x6) matrix % Any other passed value defaults to the 1, -1, -1 configuration % if (nargin < 3), configuration = 1; end if (size(configuration,2) ~= 1), if ((configuration(1) == 'R') | (configuration(1) == 'r')), n1_lower = 1; n1_upper = 1; elseif ((configuration(1) == 'L') | (configuration(1) == 'l')), n1_lower = -1; n1_upper = -1; else n1_lower = sign(configuration(1)); n1_upper = sign(configuration(1)); end if ((configuration(2) == 'U') | (configuration(1) == 'u')), if (n1_lower == 1), n2_lower = 1; n2_upper = 1; else n2_lower = -1; n2_upper = -1; end elseif ((configuration(2) == 'D') | (configuration(2) == 'd')), if (n1_lower == -1), n2_lower = 1; n2_upper = 1; else n2_lower = -1; n2_upper = -1; end else n2_lower = sign(configuration(2)); n2_upper = sign(configuration(2)); end if ((configuration(3) == 'F') | (configuration(3) == 'f')), n4_lower = -1; n4_upper = -1; elseif ((configuration(3) == 'N') | (configuration(3) == 'n')), n4_lower = 1; n4_upper = 1; else n4_lower = sign(configuration(3)); n4_upper = sign(configuration(3)); end else if (configuration == 8), n1_lower = -1; n1_upper = 1; n2_lower = -1; n2_upper = 1; n4_lower = -1; n4_upper = 1; else n1_lower = 1; n1_upper = 1; n2_lower = -1; n2_upper = -1; n4_lower = -1; n4_upper = -1; end end % % Initialize some variables % b = 0; for n1=n1_lower:2:n1_upper, for n2=n2_lower:2:n2_upper, for n4=n4_lower:2:n4_upper, % % Solve for theta(1) % % r is defined in equation 38, p. 39. % theta(1) uses equations 40 and 41, p.39, % based on the configuration parameter n1 % r=sqrt(Px^2 + Py^2); if (n1 == 1), theta(1)= atan2(Py,Px) + asin(d3/r); else theta(1)= atan2(Py,Px) + pi - asin(d3/r); end % % Solve for theta(2) % % V114 is defined in equation 43, p.39. % r is defined in equation 47, p.39. % Psi is defined in equation 49, p.40. % theta(2) uses equations 50 and 51, p.40, based on the configuration parameter n2 % V114= Px*cos(theta(1)) + Py*sin(theta(1)); r=sqrt(V114^2 + Pz^2); Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r)); theta(2) = atan2(Pz,V114) + n2*Psi; % % Solve for theta(3) % % theta(3) uses equation 57, p. 40. % num = cos(theta(2))*V114+sin(theta(2))*Pz-a2; den = cos(theta(2))*Pz - sin(theta(2))*V114; theta(3) = atan2(a3,d4) - atan2(num, den); % % Solve for theta(4) % % V113 is defined in equation 62, p. 41. % V323 is defined in equation 62, p. 41. % V313 is defined in equation 62, p. 41. % theta(4) uses equation 61, p.40, based on the configuration parameter n4 % V113 = cos(theta(1))*Ax + sin(theta(1))*Ay; V323 = cos(theta(1))*Ay - sin(theta(1))*Ax; V313 = cos(theta(2)+theta(3))*V113 + sin(theta(2)+theta(3))*Az; theta(4) = atan2((n4*V323),(n4*V313)); % % Solve for theta(5) % % num is defined in equation 65, p. 41. % den is defined in equation 65, p. 41. % theta(5) uses equation 66, p. 41. % num = -cos(theta(4))*V313 - V323*sin(theta(4)); den = -V113*sin(theta(2)+theta(3)) + Az*cos(theta(2)+theta(3)); theta(5) = atan2(num,den); % % Solve for theta(6) % % V112 is defined in equation 69, p. 41. % V122 is defined in equation 69, p. 41. % V312 is defined in equation 69, p. 41. % V332 is defined in equation 69, p. 41. % V412 is defined in equation 69, p. 41. % V432 is defined in equation 69, p. 41. % num is defined in equation 68, p. 41. % den is defined in equation 68, p. 41. % theta(6) uses equation 70, p. 41. % V112 = cos(theta(1))*Ox + sin(theta(1))*Oy; V132 = sin(theta(1))*Ox - cos(theta(1))*Oy; V312 = V112*cos(theta(2)+theta(3)) + Oz*sin(theta(2)+theta(3)); V332 = -V112*sin(theta(2)+theta(3)) + Oz*cos(theta(2)+theta(3)); V412 = V312*cos(theta(4)) - V132*sin(theta(4)); V432 = V312*sin(theta(4)) + V132*cos(theta(4)); num = -V412*cos(theta(5)) - V332*sin(theta(5)); den = - V432; theta(6) = atan2(num,den); b = b + 1; invkine(b,:)=theta(:)'; end end end return From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 20-FEB-1996 03:45:22.92 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:27:50 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152127.KAA03698@hoiho> Content-transfer-encoding: 7BIT %INERTIA Compute the manipulator inertia matrix % % INERTIA(DYN, Q) for an n-axis manipulator returns the nxn symmetric % inertia matrix which relates joint torque to joint acceleration. % DYN describes the manipulator dynamics and kinematics, and Q is % an n element vector of joint state. % % See also DYN, RNE, ITORQUE, CORIOLIS, GRAVLOAD. % Copright (C) Peter Corke 1993 function M = inertia(dyn, q) n = numrows(dyn); M = rne(dyn, ones(n,1)*q, zeros(n,n), eye(n), [0;0;0]); From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 20-FEB-1996 03:48:29.90 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:32:08 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152132.KAA03767@hoiho> Content-transfer-encoding: 7BIT %DH Matrix representation of manipulator kinematics % % Many robot toolbox functions take a DH matrix which describes the % kinematics of a manipulator in a general way. % % For an n-axis manipulator, DH is an nx4 or nx5 matrix, whose rows % comprise % % 1 alpha link twist angle % 2 A link length % 3 theta link rotation angle % 4 D link offset distance % 5 sigma joint type, 0 for revolute, non-zero for prismatic % % If the last column is not given, most toolbox functions assume that % the manipulator is all-revolute. % % The first 5 columns of a DYN matrix contain the kinematic parameters % and maybe used anywhere that a DH kinematic matrix is required -- the % dynamic data is ignored. % % See also DYN. % MOD.HISTORY % 1/95 reverse labels on A & D% Copright (C) Peter Corke 1993 From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 20-FEB-1996 03:51:36.90 To: GORANDJ@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:10:47 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu Message-id: <199602152110.KAA01612@kaka.cosc.canterbury.ac.nz> Content-transfer-encoding: 7BIT echo off %RTDEMO Robot toolbox demonstrations % % % Copyright (C) Peter Corke 1993 puma560 while 1, clg clc which = menu('Robot Toolbox demonstrations', ... 'Transformations', ... 'Trajectory', ... 'Forward kinematics', ... 'Animation', ... 'Inverse kinematics', ... 'Jacobians', ... 'Inverse dynamics', ... 'Forward dynamics', ... 'Exit'); if which == 1, rttrdemo elseif which == 2, rttgdemo elseif which == 3, rtfkdemo elseif which == 4, rtandemo elseif which == 5, rtikdemo elseif which == 6, rtjademo elseif which == 7, rtdydemo elseif which == 8, rtfddemo % chaotic 2 link elseif which == 9, break; end end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 20-FEB-1996 04:25:01.06 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:29:57 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152129.KAA03729@hoiho> Content-transfer-encoding: 7BIT %JTRAJ Compute a joint space trajectory between two points % % [Q QD QDD] = JTRAJ(Q0, Q1, N) % [Q QD QDD] = JTRAJ(Q0, Q1, N, QD0, QD1) % [Q QD QDD] = JTRAJ(Q0, Q1, T) % [Q QD QDD] = JTRAJ(Q0, Q1, T, QD0, QD1) % % Returns a joint space trajectory Q from state Q0 to Q1. The number % of points is N or the length of the given time vector T. A 7th % order polynomial is used with default zero boundary conditions for % velocity and acceleration. Non-zero boundary velocities can be % optionally specified as QD0 and QD1. % % The function can optionally return a velocity and acceleration % trajectories as QD and QDD. % % Each trajectory is an mxn matrix, with one row per time step, and % one column per joint parameter. % % Copright (C) Peter Corke 1993 % MOD.HISTORY % 1/95 add support for initial velocities as advertised function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1) if length(tv) > 1, tscal = max(tv); t = tv(:)/tscal; else tscal = 1; t = [0:(tv-1)]'/(tv-1); % normalized time from 0 -> 1 end q0 = q0(:); q1 = q1(:); if nargin == 3, qd0 = zeros(size(q0)); qd1 = qd0; end % compute the polynomial coefficients A = 6*(q1 - q0) - 3*(qd1+qd0); B = -15*(q1 - q0) + 8*qd0 + 7*qd1; C = 10*(q1 - q0) - 6*qd0 - 4*qd1; F = q0; tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))]; c = [A B C zeros(size(A)) zeros(size(A)) F]'; qt = tt*c; % compute optional velocity if nargout >= 2, c = [ zeros(size(A)) 5*A 4*B 3*C zeros(size(A)) zeros(size(A))]'; qdt = tt*c/tscal; end % compute optional acceleration if nargout == 3, c = [ zeros(size(A)) zeros(size(A)) 20*A 12*B 6*C zeros(size(A))]'; qddt = tt*c/tscal*2; end From: SMTP%"ban.junis.ni.ac.yu!info!cosc.canterbury.ac.nz!tanja" 20-FEB-1996 04:28:21.82 To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu CC: Subj: Organization: Date: Fri, 16 Feb 1996 10:29:38 +1300 From: Antonija Mitrovic To: GORANDJ@efnis.elfak.ni.ac.yu, gorandj@efnis.elfak.ni.ac.yu Message-id: <199602152129.KAA03724@hoiho> Content-transfer-encoding: 7BIT echo on % % 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)) pause % 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 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)') 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 compare the gravity torque with 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)') 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 M11 = []; for Q = q', M = inertia(p560, Q'); M11 = [M11; M(1,1)]; end subplot(1,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) pause % any key to continue