function fbmodel = floatbase( model )
% floatbase construct the floating-base equivalent of a fixed-base model
% floatbase(model) converts a fixed-base kinematic tree to a floating-base
% kinematic tree as follows: old body 1 becomes new body 6, and is regarded
% as the floating base; old joint 1 is discarded; six new joints are added
% (three prismatic and three revolute, in that order, arranged along the x,
% y and z axes, in that order); and five new zero-mass bodies are added
% (numbered 1 to 5), to connect the new joints. All other bodies and
% joints in the given model are preserved, but their identification numbers
% are incremented by 5. The end result is that body 6 has a full 6 degrees
% of motion freedom relative to the fixed base, with joint variables 1 to 6
% serving as a set of 3 Cartesian coordinates and 3 rotation angles (about
% x, y and z axes) specifying the position and orientation of body 6's
% coordinate frame relative to the base coordinate frame. CAUTION: A
% singularity occurs when q(5)=+-pi/2, and these orientations must be
% avoided. Note: this function requires that old joint 1 is the only joint
% connected to the fixed base, and that Xlink{1} is the identity.
if any( model.parent(2:model.NB) == 0 )
error('only one connection to a fixed base allowed');
end
if ~isequal( model.Xtree{1}, Xtrans([0,0,0]) )
warning('Xtree{1} not identity');
end
fbmodel.NB = model.NB + 5;
fbmodel.pitch(1:6) = [inf,inf,inf,0,0,0];
fbmodel.pitch(7:fbmodel.NB) = model.pitch(2:model.NB);
fbmodel.parent(1:6) = [0 1 2 3 4 5];
fbmodel.parent(7:fbmodel.NB) = model.parent(2:model.NB) + 5;
fbmodel.Xtree{1} = Xroty(pi/2);
fbmodel.Xtree{2} = Xrotx(-pi/2) * Xroty(-pi/2);
fbmodel.Xtree{3} = Xrotx(pi/2);
fbmodel.Xtree{4} = Xroty(pi/2);
fbmodel.Xtree{5} = Xrotx(-pi/2) * Xroty(-pi/2);
fbmodel.Xtree{6} = Xrotx(pi/2);
for i = 7:fbmodel.NB
fbmodel.Xtree{i} = model.Xtree{i-5};
end
for i = 1:fbmodel.NB
if i < 6
fbmodel.I{i} = mcI( 0, [0,0,0], zeros(3) );
else
fbmodel.I{i} = model.I{i-5};
end
end
fbmodel.appearance{1} = model.appearance{1};
for i = 6:fbmodel.NB
fbmodel.appearance{i+1} = model.appearance{i-4};
end