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