%PUMAMRCNTRL Puma Motion Rate Control % Function for performing motion rate control of a puma 6 dof arm % along a series of cubic Bezier curves. % % Syntax: [theta, T] = pumamrcntrl(theta, param, CP, Tf, dt, drawmode) % % Inputs: % theta - a 6 element array of the initial joint angles. % param - [d1 l2 d4 d6] (The non-zero lengths and offsets.) % CP - a nx3 vector of n Bezier control points in 3-space. % Tf - 4x4 homogeneous homogeneous rotation matrix specifying the final % desired orientation of the end effector. % dt - increment in the parameter along the Bezier curve that is made % between successive movements. % drawmode - same as puma6dof. % % Outputs: % finaltheta - a 6 element array of the joint angles at the final % position of the arm. % Taf - The actual transformation matrix describing the final end % effector pose that was achieved. % % Other m-files required: puma6dof.m, startCubicBez.m, invht.m, % startCubicBez.m, invrotaxis.m, bezier.m, rot.m, % twitch.m % % See also: PUMA6DOF % Author: Travis Hydzik % Last revision: 20 October 2004 function [theta, T] = pumamrcntrl(theta, param, CP, Tf, dt, drawmode) [T, J] = puma6dof(theta, param, drawmode); CP = [ startCubicBez(T(1:3,4)', [0 0 0]); CP ] [N, M] = size(CP); Ti = T; S = invht(Ti)*Tf; %find rotation axis and angle [axis, omega] = invrotaxis(S); domega = omega/(N/(4*dt)); omega = 0; for i = 1:4:N for t = dt:dt:1 omega = omega + domega; dO = twitch(T, Ti*rot(axis, omega)); dx = [ (bezier(t, CP(i:i+3,:)) - T(1:3,4)') (T(1:3,1:3)*dO)' ]'; dtheta = J\dx; theta = theta + dtheta'; [T, J] = puma6dof(theta, param, drawmode); end end