function [o1, o2, o3] = fbkin( i1, i2, i3 ) % fbkin Forward and Inverse Kinematics of Floating Base % [x,xd]=fbkin(q,qd,qdd) calculates the forward kinematics, and % [q,qd,qdd]=fbkin(x,xd) calculates the inverse kinematics, of a spatial % floating base constructed by floatbase. The vectors q, qd and qdd are % the joint position, velocity and acceleration variables for the six % joints forming the floating base; x is the 13-element singularity-free % state vector used by FDfb and IDfb; and xd is its derivative. The % component parts of x are: a unit quaternion specifying the orientation of % the floating-base frame relative to the fixed base, a 3D vector giving % the position of the origin of the floating-base frame, and the spatial % velocity of the floating base expressed in fixed-base coordinates. If % the acceleration calculation is not needed then use the simpler calls % x=fbkin(q,qd) and [q,qd]=fbkin(x); and if the velocity calculation is % also not needed then use p=fbkin(q) and q=fbkin(p) (or q=fbkin(x)), where % p contains the first 7 elements of x. If the length of the first % argument is 7 or 13 then it is assumed to be p or x, otherwise it is % assumed to be q. All vectors are expected to be column vectors. The % returned value of q(5) is normalized to the range -pi/2 to pi/2; and the % returned values of q(4) and q(6) are normalized to the range -pi to pi. % The calculation of qd and qdd fails if the floating base is in one of its % two kinematic singularities, which occur when q(5)==+/-pi/2. However, % the calculation of q is robust, and remains accurate both in and close to % the singularities. if length(i1) == 13 || length(i1) == 7 if nargout == 3 [o1, o2, o3] = invkin( i1, i2 ); elseif nargout == 2 [o1, o2] = invkin( i1 ); else o1 = invkin( i1 ); end else if nargout == 2 [o1, o2] = fwdkin( i1, i2, i3 ); elseif nargin == 2 o1 = fwdkin( i1, i2 ); else o1 = fwdkin( i1 ); end end function [x, xd] = fwdkin( q, qd, qdd ) c4 = cos(q(4)); s4 = sin(q(4)); c5 = cos(q(5)); s5 = sin(q(5)); c6 = cos(q(6)); s6 = sin(q(6)); E = [ c5*c6, c4*s6+s4*s5*c6, s4*s6-c4*s5*c6; -c5*s6, c4*c6-s4*s5*s6, s4*c6+c4*s5*s6; s5, -s4*c5, c4*c5 ]; qn = rq(E); % unit quaternion fixed-->floating r = q(1:3); % position of floating-base origin x(1:4,1) = qn; x(5:7) = r; if nargin > 1 % do velocity calculation S = [ 1 0 s5; 0 c4 -s4*c5; 0 s4 c4*c5 ]; omega = S*qd(4:6); rd = qd(1:3); % lin vel of floating-base origin v = [ omega; rd+cross(r,omega) ]; % spatial vel in fixed-base coords x(8:13) = v; end if nargout == 2 % calculate xd c4d = -s4*qd(4); s4d = c4*qd(4); c5d = -s5*qd(5); s5d = c5*qd(5); Sd = [ 0 0 s5d; 0 c4d -s4d*c5-s4*c5d; 0 s4d c4d*c5+c4*c5d ]; omegad = S*qdd(4:6) + Sd*qd(4:6); rdd = qdd(1:3); % spatial accn in fixed-base coords a = [ omegad; rdd+cross(rd,omega)+cross(r,omegad) ]; xd(1:4,1) = rqd( omega, rq(E) ); xd(5:7) = rd; xd(8:13) = a; end function [q, qd, qdd] = invkin( x, xd ) E = rq(x(1:4)); % coord xfm fixed-->floating r = x(5:7); % position of floating-base origin q(1:3,1) = r; q(5) = atan2( E(3,1), sqrt(E(1,1)*E(1,1)+E(2,1)*E(2,1)) ); q(6) = atan2( -E(2,1), E(1,1) ); if E(3,1) > 0 q(4) = atan2( E(2,3)+E(1,2), E(2,2)-E(1,3) ) - q(6); else q(4) = atan2( E(2,3)-E(1,2), E(2,2)+E(1,3) ) + q(6); end if q(4) > pi q(4) = q(4) - 2*pi; elseif q(4) < -pi q(4) = q(4) + 2*pi; end if nargout > 1 % calculate qd c4 = cos(q(4)); s4 = sin(q(4)); c5 = cos(q(5)); s5 = sin(q(5)); S = [ 1 0 s5; 0 c4 -s4*c5; 0 s4 c4*c5 ]; omega = x(8:10); rd = x(11:13) - cross(r,omega); % lin vel of floating-base origin qd(1:3,1) = rd; qd(4:6) = S \ omega; % this will fail at a singularity end if nargout == 3 % calculate qdd c4d = -s4*qd(4); s4d = c4*qd(4); c5d = -s5*qd(5); s5d = c5*qd(5); Sd = [ 0 0 s5d; 0 c4d -s4d*c5-s4*c5d; 0 s4d c4d*c5+c4*c5d ]; omegad = xd(8:10); rdd = xd(11:13) - cross(rd,omega) - cross(r,omegad); qdd(1:3,1) = rdd; qdd(4:6) = S \ (omegad - Sd*qd(4:6)); % this will fail at a singularity end