function  fbmodel = floatbase( model )

% floatbase  construct the floating-base equivalent of a fixed-base model
% floatbase(model)  converts a fixed-base spatial 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.  If this is a
% problem then use the special functions FDfb and IDfb instead of the
% standard dynamics functions.  floatbase requires that old joint 1 is the
% only joint connected to the fixed base, and that Xtree{1} is the
% identity.

if size(model.Xtree{1},1) == 3
  error('floatbase applies to spatial models only');
end

if any( model.parent(2:model.NB) == 0 )
  error('only one connection to a fixed base allowed');
end

if isfield( model, 'gamma_q' )
  warning('floating a model with gamma_q (joint numbers will change)');
end

if ~isequal( model.Xtree{1}, eye(6) )
  warning('Xtree{1} not identity');
end

fbmodel = model;

fbmodel.NB = model.NB + 5;

fbmodel.jtype = ['Px' 'Py' 'Pz' 'Rx' 'Ry' 'Rz' model.jtype(2:end)];

fbmodel.parent = [0 1 2 3 4 model.parent+5];

fbmodel.Xtree = [eye(6) eye(6) eye(6) eye(6) eye(6) model.Xtree];

fbmodel.I = [zeros(6) zeros(6) zeros(6) zeros(6) zeros(6) model.I];

if isfield( model, 'appearance' )
  fbmodel.appearance.body = {{}, {}, {}, {}, {}, model.appearance.body{:}};
end

if isfield( model, 'camera' ) && isfield( model.camera, 'body' )
  if model.camera.body > 0
    fbmodel.camera.body = model.camera.body + 5;
  end
end

if isfield( model, 'gc' )
  fbmodel.gc.body = model.gc.body + 5;
end