RHSRobotInitialize:

--------------------------------------------------------------------------
   Robot dynamics model initialization. 

   The initialization function uses parameter pairs. The ones starting with
   name are links. Entering name creates a new link. All link parameters
   for that link must be specified before the next name

   Each hinge has one degree-of-freedom which may be either revolute or 
   prismatic. The rotation or translation axis for each hinge is always z. 
   Additional transformation matrices are defined in fileName in cell
   array R.

   This routine is for a general topological tree with no closed loops.

   Branching is allowed. 

   RHSRobot can take a function for external torques and forces

   [force, torque] = forceFun( k, v, d )

   where k is the link id

   The name of forceFun is the pointer forceFun. forceFun may be empty.

   u = frictionFun( v, cFriction ) is the friction function. u is
   the joint friction and may be sliding or rotational. cFriction
   is an array of data structures for the friction function. u is an 
   array of friction forces and torques.

--------------------------------------------------------------------------
   Form:
   h = RHSRobotInitialize( varargin )
--------------------------------------------------------------------------

   ------
   Inputs
   ------
                       
   -------
   Outputs
   -------
   h       (:,1)   Data structure (n links)
                   .robotName      (1)     Robot name
                   .forceFun       (1,:)   Pointer to the link force and torque function.
                   .forceData      (1,1)   Data structure for forceFun
                   .jointFun       (1,:)   Pointer to the jointFun
                   .jointData      (1,1)   Data structure for the joint function
                   .frictionFun	(1,:)   Pointer to the friction function.
                   .frictionData	(1,1)   Friction data structure.
                   .actuatorMap    (1,:)   Maps the u vector to the links.
                   .u              (:,1)   Control vector (of length(h.actuatorMap)
                   .g0             (3,1)   Gravity vector
                   .name           {n}     Link name 
                   .inertia        {(3,3)} 3-by-3 inertia matrices.                 
                   .R              {(3,3)} transformation matrix that rotates the hinge axis into z.
                   .c              {(3,1)} vector from the i hinge to the cm in a link.
                   .r              {(3,1)} vector from the i-1 hinge to the i hinge in a link.
                   .parent         {n}     parent of current link  
                   .rParent          
                   .hinge          (1,n)   Type of hinge 0 for prismatic 1 for revolute
                   .radius         
                   .mass
                   .stateLinkIndex	(1,n)   Maps the links to the state vector.
                   .windVel        {(3,1)} Wind velocity vector
                   .cP             {(3,1}) vector from the cm to center of pressure
                   .fname          (1)     Name of .mat file to load data structure from     
  
       The remaining entries in the data structure h are autogenerated by
       the function RHSRobotInitialize
                   .path           {(:)}   The link path to each end point
                   .link           (1,n)   Previous link.
                   .linkV          (1,n)   Velocity source for link.
                   .linkOrder      (1,n)   The order of links in the topological tree.
                   .rMap           {(:)}   Maps the r vectors to the links
                   .nB             (1,1)   Number of branches
                   .branches       {k}     Branches per link
                   .nBranches      (:)     Number of branches per link


--------------------------------------------------------------------------