Matlab Source Code for Constructing Robot Models
(c) 2004 Roy Featherstone
Cut and paste the source code below into files as indicated

----------- planarN.m --------------------------------------------------------

function robot = planarN( n )
% PLANARN  create an n-link planar robot.
% planarN(n) creates an all-revolute n-link planar robot with identical links.

robot.NB = n;
robot.parent = [0:n-1];
robot.pitch = zeros(1,n);

robot.Xlink = cell(n,1);
robot.Ilink = cell(n,1);
for i = 1:n
  robot.Xlink{i} = Xtrans([1 0 0]);
  robot.Ilink{i} = RBmci( 1, [0.5 0 0], diag([0,1/12,1/12]) );
end

robot.Xlink{1} = Xtrans([0 0 0]);

----------- hydra.m ----------------------------------------------------------

function robot = hydra( nb, bf, skew, taper )
% HYDRA  create a branched, tapering spatial robot.
% hydra(nb,bf,skew,taper) creates an all-revolute nb-link spatial robot with
% a branching factor of bf, which must be a number >= 1.  bf=1 produces an
% unbranched chain, bf=2 produces a binary tree, bf=1.5 produces a tree where
% the average branching factor is 1.5, and so on.  Trees are constructed
% breadth-first, and therefore have minimum depth.  Link 1 is a thin-walled
% cylindrical tube of length 1m, radius 0.05m and mass 1kg, lying between 0
% and 1 on the x axis of its local coordinate system.  Its inboard joint axis
% lies on the local z axis, and its outboard axis passes through (1,0,0) at an
% angle of rotx(skew) radians relative to the inboard axis.  (If the link has
% more than one outboard joint then they all have the same axis.)  If skew=0
% then the robot is planar.  Link i is identical to link 1 scaled by
% taper^(i-1).  Its length, mass and inertia properties scale in proportion to
% size^1, size^3 and size^5, respectively.  If taper=1 then all links are
% identical.

robot.NB = nb;
robot.pitch = zeros(1,nb);
robot.parent = zeros(1,nb);
robot.Xlink = cell(1,nb);
robot.Ilink = cell(1,nb);

for i = 1:nb
  robot.parent(i) = floor((i-2+ceil(bf))/bf);
  if robot.parent(i) == 0
    robot.Xlink{1} = Xtrans([0 0 0]);
  else
    robot.Xlink{i} = Xrotx(skew) * Xtrans([taper^(robot.parent(i)-1),0,0]);
  end
  robot.Ilink{i} = RBmci( taper^(3*(i-1)), ...
			  taper^(i-1)     * [0.5,0,0], ...
			  taper^(5*(i-1)) * diag([0.0025,1.015/12,1.015/12]) );
end