function  robot = vee( L2 )

% VEE  free-floating 1R robot consisting of two rods
% vee(L2) creates a free-floating robot consisting of two thin rods
% connected by a revolute joint with an axis at 45 degrees to the axis of
% each rod.  When the joint variables are zero, rod 1 lies on the X axis,
% rod 2 lies on the Y axis, and the connecting joint lies on the line x=y,
% z=0.  Rod 1 has a length of 1, and rod 2 has a length of L2.  If the
% length argument is omitted then it defaults to 1.  The rods have a radius
% of 0.01, and their masses equal their lengths.

if nargin < 1
  L2 = 1;
end

% For efficient use with Simulink, this function builds a new model the
% first time it is called, and stores it in the persistent variable
% 'memory'; and it stores the corresponding length argument in 'memory_L2'.
% On each subsequent occasion that it is called, it compares the L2 value
% with memory_L2, and if they are the same then it simply returns the
% stored model instead of creating a new one.

persistent memory memory_L2;

if length(memory) ~= 0 && memory_L2 == L2
  robot = memory;
  return
end

% To make a new vee model, we first create a two-link robot with two
% revolute joints.  The first joint is just a place-holder, as it will be
% replaced with a chain of six joints when floatbase is called near the end
% of this function.  When that happens, link 1 (=rod 1) will become link 6,
% link 2 will become link 7, and joint 2 (the real joint) will become joint
% 7.  For now, the first step is to define the connectivity and kinematics
% of a two-link robot.

robot.NB = 2;
robot.parent = [0 1];
robot.jtype = {'R', 'R'};

robot.Xtree = { eye(6), roty(pi/2) * rotz(pi/4) };

robot.gravity = [0 0 0];		% zero gravity is not the default,
                                        % so it must be stated explicitly

% The next step is to calculate the inertias of the two links, assuming
% they are uniform solid cylinders of radius 0.01.  Note that L2 is both
% the length and mass of link 2.  One little complication here is that rod
% 1 lies on the X axis of its local coordinate system, but rod 2 lies on
% the line x=0, y=z of its local coordinate system.  We get over this by
% first defining the inertia of rod 2 as if it were lying on the Y axis,
% and then rotate it by pi/4 about the X axis.

Ibig1 = 1.0003/12;
Ibig2 = L2*(L2^2+0.0003)/12;
Ismall1 = 0.00005;
Ismall2 = L2*0.00005;
rod1 = mcI( 1, [0.5,0,0], diag([Ismall1,Ibig1,Ibig1]) );
rod2 = mcI( L2, [0,L2/2,0], diag([Ibig2,Ismall2,Ibig2]) );

% The next two lines rotate rod 2 by pi/4 about the X axis.  Note that X2
% is actually a cordinate transform, but it is being used as a rotation
% operator on this occasion.

X2 = rotx(pi/4);
rod2 = X2' * rod2 * X2;

robot.I = { rod1, rod2 };

% The next step is to draw some visual guide lines in the fixed coordinate
% system, to help the viewer understand the motion.  Lines are drawn on the
% X axis, the Y axis, a line at 45 degrees to these axes, and a line
% joining the two centres of mass.

robot.appearance.base = ...
  { 'line', [1.1 0 0; 0 0 0; 0 1.1 0], ...
    'line', [0 0 0; 0.8 0.8 0], ...
    'line', [0.55 -L2*0.05 0; -0.05 L2*0.55 0] };

% The next step is to draw the two bodies.  A small red sphere marks each
% centre of mass.  Observe that rod 2 lies at 45 degrees to the Y and Z
% axes in its local coordinate system, so the cylinder representing it is
% drawn at [0 0 0; 0 L2 L2]/sqrt(2).  Similar comments apply to the
% cylinder in link 1 that models the appearance of the revolute joint.

robot.appearance.body{1} = ...
  { 'cyl', [0 0 0; 1 0 0], 0.01, ...
    'cyl', [0.02 0.02 0; -0.02 -0.02 0]/sqrt(2), 0.02, ...
    'colour', [0.9 0 0], ...
    'sphere', [0.5 0 0], 0.015 };

robot.appearance.body{2} = ...
  { 'cyl', [0 0 0; 0 L2 L2]/sqrt(2), 0.01, ...
    'cyl', [0 0 0.02; 0 0 0.06], 0.02, ...
    'colour', [0.9 0 0], ...
    'sphere', [0 L2/2 L2/2]/sqrt(2), 0.015 };

% To further assist the viewer, if L2 takes a value other than 1 then three
% small orange spheres are added to the drawing instructions: one at the
% system centre of mass, and one embedded in each rod at the point where
% that rod intersects with the system CoM.

if L2 ~= 1

  Lcm = (1+L2^2) / (1+L2) / 2;
  robot.appearance.body{1} = ...
    [ robot.appearance.body{1}, ...
      { 'colour', [0.9 0.5 0], 'sphere', [Lcm 0 0], 0.015 } ];

  robot.appearance.body{2} = ...
    [ robot.appearance.body{2}, ...
      { 'colour', [0.9 0.5 0], ...
	'sphere', [0 sqrt(0.5) sqrt(0.5)]*Lcm, 0.015 } ];

  sys_CoM = [0.5 L2^2/2 0] / (1+L2);	% system centre of mass
  robot.appearance.base = ...
    [ robot.appearance.base, ...
      { 'colour', [0.9 0.5 0], 'sphere', sys_CoM, 0.015 } ];
end

% And finally, we call floatbase to replace joint 1 with a chain of six
% joints (three prismatic and three revolute), and store a copy of the new
% robot in 'memory'.

robot = floatbase(robot);

memory = robot;
memory_L2 = L2;