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