function Q = fbanim( X, Qr )
% fbanim Floating Base Inverse Kinematics for Animation
% Q=fbanim(X) and Q=fbanim(X,Qr) calculate matrices of joint position data
% for use in showmotion animations. Q=fbanim(X) calculates a 6xN matrix Q
% from a 13x1xN or 13xN or 7x1xN or 7xN matrix X, such that each column of
% X contains at least the first 7 elements of a 13-element singularity-free
% state vector used by FDfb and IDfb, and the corresponding column of Q
% contains the position variables of the three prismatic and three revolute
% joints that form the floating base. Each column of Q is calculated using
% fbkin; but fbanim then adjusts the three revolute joint variables to
% remove the pi and 2*pi jumps that occur when the angles wrap around or
% pass through a kinematic singularity. As a result, some or all of the
% revolute joint angles may grow without limit. This is essential for a
% smooth animation. Q=fbanim(X,Qr) performs the same calculation as just
% described, but then appends Qr to Q. Qr must be an MxN or Mx1xN matrix
% of joint position data for the real joints in a floating-base mechanism,
% and the resulting (6+M)xN matrix Q contains the complete set of joint
% data required by showmotion. Note: the algorithm used to remove the
% jumps makes continuity assumptions (specifically, less than pi/2 changes
% from one column to the next, except when passing through a singularity)
% that are not guaranteed to be true. Therefore, visible glitches in an
% animation are still possible.
if length(size(X)) == 3 % collapse 3D -> 2D array
tmp(:,:) = X(:,1,:);
X = tmp;
end
% apply kinematic transform using fbkin
for i = 1 : size(X,2)
Q(:,i) = fbkin( X(1:7,i) );
end
% This code removes wrap-arounds and step-changes on passing through a
% singularity. Whenever q6 or q4 wrap, they jump by 2*pi. However, when
% q5 hits pi/2 (or -pi/2), q4 and q6 both jump by pi, and q5 turns around.
% To undo this, the code looks to see whether n is an even or odd number,
% and replaces q5 with pi-q5 whenever n is odd. q4 is calculated via the
% sum or difference of q4 and q6 on the grounds that the sum well defined
% at the singularity at q5=pi/2 and the difference is well defined at
% q5=-pi/2.
for i = 2 : size(X,2)
n = round( (Q(6,i-1) - Q(6,i)) / pi );
q6 = Q(6,i) + n*pi;
if Q(5,i) >= 0
q46 = Q(4,i) + Q(6,i);
q46 = q46 + 2*pi * round( (Q(4,i-1)+Q(6,i-1) - q46) / (2*pi) );
Q(4,i) = q46 - q6;
else
q46 = Q(4,i) - Q(6,i);
q46 = q46 + 2*pi * round( (Q(4,i-1)-Q(6,i-1) - q46) / (2*pi) );
Q(4,i) = q46 + q6;
end
Q(6,i) = q6;
if mod(n,2) == 0
q5 = Q(5,i);
else
q5 = pi - Q(5,i);
end
Q(5,i) = q5 + 2*pi * round( (Q(5,i-1) - q5) / (2*pi) );
end
% add the rest of the joint data, if argument Qr has been supplied
if nargin == 2
if length(size(Qr)) == 3 % collapse 3D -> 2D array
tmp = zeros(size(Qr,1),size(Qr,3));
tmp(:,:) = Qr(:,1,:);
Qr = tmp;
end
Q = [ Q; Qr ];
end