The functions described in this document are intended as an accompaniment to the book Rigid Body Dynamics Algorithms (RBDA), and will likely be easiest to understand if you happen to have access to a copy of this book. Nevertheless, is should still be possible to understand and make good use of the majority of these functions even without access to this book. Some of the materials on the spatial vectors page may be helpful in this regard. The complete source code is available in this zip file. (It will create a directory called spatial_v1 containing all of the code.)
Spatial
Vector Arithmetic 

crf 
cross product operator (force) 
crm 
cross product operator (motion) 
mcI 
construct rigidbody inertia
from
mass, centre of mass and rotational inertia 
Xrotx 
coordinate transform (rotation
about x axis) 
Xroty 
coordinate transform (rotation
about y axis) 
Xrotz 
coordinate transform (rotation
about z axis) 
XtoV 
compute a displacement vector
from a coordinate transform 
Xtrans 
coordinate transform
(translation) 
Dynamics
Functions 

fbKin 
forward and inverse kinematics
of floating base 
FDab 
forward dynamics,
articulatedbody algorithm 
FDcrb 
forward dynamics,
compositerigidbody algorithm 
FDf 
floatingbase forward dynamics
(articulatedbody algorithm) 
HandC 
coefficients of jointspace
equation of motion 
HD 
hybrid dynamics
(articulatedbody algorithm) 
ID 
inverse dynamics (recursive
NewtonEuler algorithm) 
IDf 
floatingbase inverse dynamics 
jcalc 
joint data calculation function
used by dynamics functions 
Model Constructors 

system model data
structure 

autoTree 
construct a kinematic tree 
floatbase 
construct a floatingbase model
from a given fixedbase model 
planar2 
efficient version of planarN(2) 
planar3 
efficient version of planarN(3) 
planarN 
construct a simple planar robot
with revolute joints and identical links 
Planar Vector
Arithmetic 

crfp 
cross product operator (force) 
crmp 
cross product operator (motion) 
mcIp 
construct rigidbody inertia from mass, centre of mass and rotational inertia 
Xpln 
general planar coordinate
transform 
XtoVp 
compute a displacement vector from a coordinate transform 
Planar
Vector Dynamics Functions 

FDabp 
forward dynamics,
articulatedbody algorithm 
FDcrbp 
forward dynamics,
compositerigidbody algorithm 
HandCp  coefficients of jointspace equation of motion 
IDp 
inverse dynamics (recursive
NewtonEuler algorithm) 
jcalcp 
joint data calculation function
used by dynamics functions 
Planar Vector Model
Constructors 

system model data
structure 

autoTreep 
construct a kinematic tree 
Graphics 

drawmodel 
display kinematic models and
animations 
Examples (Demos) 

planar2ctrl.mdl 
Simulink model of a planar robot
and a PD controller 
test_FD 
tests mutual consistency of
FDab, FDcrb and ID 
test_HD 
tests mutual consistency of HD
and ID 
test_fb1 
tests mutual consistency of FDf
and IDf 
test_fb2 
compare floatingbase dynamics
with equivalent fixedbase dynamics 
test_p 
tests correctness/consistency of
FDabp, FDcrbp, IDp and HandCp 
Branch Induced
Sparsity 

expandLambda 
calculate expanded parent array 
LTDL 
LTDL factorization (given H,
calculate L and D such that L'*D*L==H) 
LTL 
LTL factorization (given H,
calculate L such that L'*L==H) 
mpyH 
multiply a vector by H 
mpyL 
multiply a vector by L 
mpyLt 
multiply a vector by the
transpose of L 
mpyLi 
multiply a vector by the inverse
of L 
mpyLit 
multiply a vector by the
transpose of the inverse of L 
crf(v) and crm(v) calculate the 6x6 matrices such that the expressions crf(v)*f and crm(v)*m are the cross products of the motion vector v with the force vector f and motion vector m, respectively. If f and m are fixed in a rigid body that is moving with a velocity of v, then these expressions give the time derivatives of f and m due to the motion of the body.
Construct the 6x6 spatial inertia matrix of a rigid body from its mass m, its centre of mass c (3D vector) and its rotational inertia I (3x3 matrix) about its centre of mass.
These functions calculate the 6x6 coordinate transform matrix that transforms a motion vector from A coordinates to B coordinates, where the Cartesian frame that defines B coordinates (frame B) is rotated relative to frame A by an angle theta (radians) about their common x, y or z axis, respectively. (Positive rotation of a Cartesian frame about its x/y/z axis causes the y/z/x axis to rotate towards the z/x/y axis.) See also general transform properties.
Calculate a motion vector v that
approximates to the displacement defined by the coordinate transform X.
Specifically, if X is the
transform from A
coordinates to B
coordinates, then the location of
frame B
relative to frame A can be
deduced from the value of X.
Thus, X
can supply a description of the displacement that carries frame
A to
frame B.
If the angle between these frames is small, then
the displacement can also be described by means of the velocity vector,
u, that
carries frame A
to frame B
in one time unit. The function XtoV
calculates a vector v that
approximates to u,
and that converges to the exact value as the angle between A and B
approaches zero. v also has
the special property of being an invariant of X, meaning
that v == X*v.
Thus, v
can be regarded as being expressed both in A coordinates
and in B
coordinates.
Calculate the 6x6 coordinate transform matrix that transforms a motion vector from A coordinates to B coordinates, where the Cartesian frame that defines B coordinates (frame B) is translated relative to frame A by an amount r, which is a 3D vector giving the coordinates of the origin of frame B expressed in A coordinates. See also general transform properties.
This function calculates the forward and inverse kinematics of the floating base (body 6) in a system model created by floatbase. X is the coordinate transform from fixedbase to floatingbase coordinates; v and a are the spatial velocity and acceleration of the floating base, expressed in fixedbase coordinates; and q, qd and qdd are vectors containing the position, velocity and acceleration variables of the first 6 joints in the system. If the first argument is a 6x6 matrix, then it is assumed to be X; otherwise, it is assumed to be q. In effect, fbKin calculates the forward kinematics (X, v and a from q, qd and qdd) and inverse kinematics (q, qd and qdd from X, v and a) of a 6DoF Cartesian robot in which the first three joints are translations in the x, y and z directions, and the second three are successive rotations about the x, y and z axis, in that order. The returned value of q(5) is normalized to the range pi/2 to pi/2; and the returned values of q(4) and q(6) are normalized to the range pi to pi. This robot has a singularity when joint 5 is at +pi/2 or pi/2; and the inverse kinematics calculation fails (without warning) if the configuration is too close to a singularity.
The main purpose of fbKin is to act as a bridge between the floatingbase dynamics functions, FDf and IDf, and other dynamics functions. Both FDf and IDf require X and v as inputs, and return a as a result.
Examples:Calculate the forward dynamics of a kinematic tree using the
articulatedbody algorithm. model is a
system model data
structure that specifies the number of bodies and joints, the
connectivity, and so on. q, qd, qdd and tau are
vectors of joint
position, velocity, acceleration and force variables, respectively,
having dimensions of model.NB x 1
(i.e., they are column
vectors). f_ext
is a cell array of spatial force vectors
specifying the external forces acting on the bodies. If there are
no external forces, then f_ext can
take the value {}.
Otherwise,
f_ext
must contain model.NB
elements, and each element f_ext{i} must
either be empty, to indicate that there is no external force acting on
body i,
or else it must be a spatial force vector specifying the force
acting on body i,
expressed in body i
coordinates. grav_accn is
a
3D vector specifying the acceleration due to a uniform gravitational
field, expressed in base coordinates. Both f_ext and grav_accn
are optional arguments. If they are omitted, then they default to
the values {}
and [0,0,9.81],
respectively.
Calculate the forward dynamics of a kinematic tree using the
compositerigidbody algorithm. model is a
system model data
structure that specifies the number of bodies and joints, the
connectivity, and so on. q, qd, qdd and tau are
vectors of joint
position, velocity, acceleration and force variables, respectively,
having dimensions of model.NB x 1
(i.e., they are column
vectors). f_ext
is a cell array of spatial force vectors
specifying the external
forces acting on the bodies. If there are no external forces,
then
f_ext
can take the value {}.
Otherwise, f_ext
must contain
model.NB
elements, and each element f_ext{i} must
either be empty, to indicate
that there is no external force acting on body i, or else it
must be a
spatial force vector specifying the force acting on body i, expressed
in body i
coordinates. grav_accn is
a 3D vector specifying the
acceleration due to a uniform gravitational field, expressed in base
coordinates. Both f_ext and grav_accn are
optional
arguments. If they
are omitted, then they default to the values {} and [0,0,9.81],
respectively. This function calls HandC to
calculate the
coefficients of
the equation of motion, and then solves the resulting linear equation
for
qdd.
Calculate the forward dynamics of a floatingbase kinematic tree using the articulatedbody algorithm. model is a system model data structure created by floatbase, or any other system model that adheres to the same conventions. Xfb is the coordinate transform from fixedbase to floatingbase coordinates; vfb and afb are the spatial velocity and acceleration of the floating base, expressed in fixedbase coordinates; and q, qd, qdd and tau contain the joint position, velocity, acceleration and force variables for the real joints in the system (i.e., they exclude the fictitious 6DoF joint between the fixed and floating bases). Note that q, qd, qdd and tau are all (model.NB  6)dimensional column vectors. f_ext is a cell array of spatial force vectors specifying the external forces acting on the bodies. If there are no external forces, then f_ext can take the value {}. Otherwise, f_ext must contain model.NB elements, and each element f_ext{i} must either be empty, to indicate that there is no external force acting on body i, or else it must be a spatial force vector specifying the force acting on body i, expressed in body i coordinates. grav_accn is a 3D vector specifying the acceleration due to a uniform gravitational field, expressed in fixedbase coordinates. Both f_ext and grav_accn are optional arguments. If they are omitted, then they default to the values {} and [0,0,9.81], respectively.
In a system model created by floatbase, body 6 is the floating base, and it is connected to the fixed base by a chain of three prismatic and three revolute joints. However, FDf ignores these six joints, using instead the arguments Xfb and vfb to set the position and velocity of body 6. One consequence of this behaviour is that the kinematic singularity that occurs when q(5) == +pi/2 or pi/2 is irrelevant, and has no effect on the performance of FDf. Because body 6 is the floating base, element i in the vectors q, qd, qdd and tau refers to joint i+6 in the system model.
Calculate the coefficients H and C of the
equation of motion
H*qdd+C=tau
for the kinematic tree described by model. H is the
jointspace inertia matrix,
and is a symmetric, positivedefinite
matrix. C
is the jointspace bias force, and has the same value
as the vector calculated by
ID(model,q,qd,zeros(model.NB,1),f_ext,grav_accn).
H and C are
calculated using the compositerigidbody and recursive NewtonEuler
algorithms, respectively. model is a
system
model data structure that specifies the number of
bodies and joints, the connectivity, and so on; and q and qd are
vectors of joint position and velocity variables having dimensions of
model.NB x 1
(i.e., they are column
vectors). f_ext
is a cell array of spatial force vectors
specifying the external
forces acting on the bodies. If there are no external forces,
then
f_ext
can take the value {}.
Otherwise, f_ext
must contain
model.NB
elements, and each element f_ext{i} must
either be empty, to indicate
that there is no external force acting on body i, or else it
must be a
spatial force vector specifying the force acting on body i, expressed
in body i
coordinates. grav_accn is
a 3D vector specifying the
acceleration due to a uniform gravitational field, expressed in base
coordinates. Both f_ext and grav_accn are
optional
arguments. If they
are omitted, then they default to the values {} and [0,0,9.81],
respectively.
Calculate a hybrid of forward and inverse dynamics using the articulatedbody algorithm. model is a system model data structure describing the kinematic tree; fd is an array of boolean values identifying the forwarddynamics joints; q and qd are vectors of joint position and velocity variables; qdd and tau are vectors of joint acceleration and force input variables; and qdd_out and tau_out are vectors of joint acceleration and force output variables. f_ext is a cell array of spatial force vectors specifying the external forces acting on the bodies. If there are no external forces, then f_ext can take the value {}. Otherwise, f_ext must contain model.NB elements, and each element f_ext{i} must either be empty, to indicate that there is no external force acting on body i, or else it must be a spatial force vector specifying the force acting on body i, expressed in body i coordinates. grav_accn is a 3D vector specifying the acceleration due to a uniform gravitational field, expressed in base coordinates. Both f_ext and grav_accn are optional arguments. If they are omitted, then they default to the values {} and [0,0,9.81], respectively.
In hybrid dynamics, each joint is identified as being either a forwarddynamics joint or an inversedynamics joint. The task of the hybrid dynamics function is then to calculate the unknown acceleration from the given force at each forwarddynamics joint, and the unknown force from the given acceleration at each inversedynamics joint. HD uses the argument fd for this purpose. Specifically, fd(i)==1 for each joint i that is a forwarddynamics joint, and fd(i)==0 for each joint i that is an inversedynamics joint. If fd(i)==1 then tau(i) contains the given force at joint i, and the value of qdd(i) is ignored; and if fd(i)==0 then qdd(i) contains the given acceleration at joint i, and the value of tau(i) is ignored. Likewise, if fd(i)==1 then qdd_out(i) contains the calculated acceleration at joint i, and tau_out(i) contains the given force copied from tau(i); and if fd(i)==0 then tau_out(i) contains the calculated force, and qdd_out(i) the given acceleration copied from qdd(i). Thus, the two output vectors are always fully instantiated. Note that all input and output vectors have dimensions of model.NB x 1.
Calculate the inverse dynamics of a kinematic tree using the recursive NewtonEuler algorithm. model is a system model data structure that specifies the number of bodies and joints, the connectivity, and so on. q, qd, qdd and tau are vectors of joint position, velocity, acceleration and force variables, respectively, having dimensions of model.NB x 1 (i.e., they are column vectors). f_ext is a cell array of spatial force vectors specifying the external forces acting on the bodies. If there are no external forces, then f_ext can take the value {}. Otherwise, f_ext must contain model.NB elements, and each element f_ext{i} must either be empty, to indicate that there is no external force acting on body i, or else it must be a spatial force vector specifying the force acting on body i, expressed in body i coordinates. grav_accn is a 3D vector specifying the acceleration due to a uniform gravitational field, expressed in base coordinates. Both f_ext and grav_accn are optional arguments. If they are omitted, then they default to the values {} and [0,0,9.81], respectively.
Calculate the inverse dynamics of a floatingbase kinematic tree using the algorithm described in the books Robot Dynamics Algorithms and Rigid Body Dynamics Algorithms. model is a system model data structure created by floatbase, or any other system model that adheres to the same conventions. Xfb is the coordinate transform from fixedbase to floatingbase coordinates; vfb and afb are the spatial velocity and acceleration of the floating base, expressed in fixedbase coordinates; and q, qd, qdd and tau contain the joint position, velocity, acceleration and force variables for the real joints in the system (i.e., they exclude the fictitious 6DoF joint between the fixed and floating bases). Note that q, qd, qdd and tau are all (model.NB  6)dimensional column vectors. f_ext is a cell array of spatial force vectors specifying the external forces acting on the bodies. If there are no external forces, then f_ext can take the value {}. Otherwise, f_ext must contain model.NB elements, and each element f_ext{i} must either be empty, to indicate that there is no external force acting on body i, or else it must be a spatial force vector specifying the force acting on body i, expressed in body i coordinates. grav_accn is a 3D vector specifying the acceleration due to a uniform gravitational field, expressed in fixedbase coordinates. Both f_ext and grav_accn are optional arguments. If they are omitted, then they default to the values {} and [0,0,9.81], respectively.
In a system model created by floatbase, body 6 is the floating base, and it is connected to the fixed base by a chain of three prismatic and three revolute joints. However, IDf ignores these six joints, using instead the arguments Xfb and vfb to set the position and velocity of body 6. One consequence of this behaviour is that the kinematic singularity that occurs when q(5) == +pi/2 or pi/2 is irrelevant, and has no effect on the performance of IDf. Because body 6 is the floating base, element i in the vectors q, qd, qdd and tau refers to joint i+6 in the system model.
This function is used by the dynamics algorithms to calculate the
joint transform (Xj)
and motion subspace matrix (S) for a
revolute (pitch==0),
prismatic (pitch==inf)
or helical (pitch==any
other value) joint. Xj is the
coordinate transform from the joint's predecessor frame to its
successor frame; and S is
expressed in successorframe coordinates. In a kinematic tree,
the successor frame of joint i is also the
body coordinate frame of body i, and the
joint's predecessor frame is fixed in the parent body of body i. For
revolute and helical joints, q is the
joint angle in radians. For prismatic joints, q is the
linear displacement.
Describes fixedbase kinematic trees containing revolute, prismatic and helical joints only.
Field 
Type 
Description 
NB 
int 
the number of bodies in the
tree, excluding the fixed base. NB is also
the number of joints. 
parent 
int[NB] 
parent(i) is
the body number of the parent of body i in the
tree. The fixed base is defined to be body number 0; and the
remaining bodies are numbered from 1 to NB in any order such that each
body has a higher number than its parent (so parent(i) < i
for all i).
Joint i
connects from body parent(i) to
body i. 
pitch 
num[NB] 
pitch(i) is
the pitch of joint i, expressed
in length units (e.g. metres) per radian. The pitch of a revolute
joint is zero, and the pitch of a prismatic joint is infinity (inf).
Any other value implies a helical (screw) joint. Positive values
indicate a righthanded screw and negative values a lefthanded
screw. The joint variable for a revolute or helical joint is the
joint angle in radians. The joint variable for a prismatic joint
is the linear displacement. 
Xtree 
spx{NB} 
Xtree{i} is
the coordinate transform from body parent(i)
coordinates to the predecessor frame of joint i. The
product of this transform with the joint transform for joint i, as
calculated by jcalc, is the transform from body parent(i)
coordinates to body i coordinates. 
I 
spI{NB} 
I{i} is the
spatial inertia of body i, expressed
in body i
coordinates. 
appearance 
{NB+1} 
drawing instructions for use by drawmodel. This field is
optional. If it is present, then appearance{i+1}
is either an empty cell array, indicating that body i is
invisible, or else it is a cell array containing a list of drawing
primitives which together form the drawing instructions for body i.
Thus, appearance{1}
is the list of drawing instructions for body zero (the fixed base), appearance{2}{1}
is the first drawing primitive in the list of drawing instructions for
body 1, and so on. The following drawing primitives are
recognised:

codes appearing in
the 'Type' column:
int: integervalued number; num: number; spx: spatial
coordinate transform (6x6 matrix); spI: spatial inertia (6x6
matrix); [...]: array of specified dimension; {...}: cell
array of specified dimension 
Generate system models of kinematic trees having revolute joints. nb specifies the number of bodies in the tree, and must be an integer >=1. bf specifies the branching factor of the tree, which is a number >=1 indicating the average number of children of a nonterminal node. bf==1 produces an unbranched tree; bf==2 produces a binary tree; and noninteger values produce trees in which the number of children alternate between floor(bf) and ceil(bf) in such a way that the average is bf. However, the tree is designed so that the fixed base always has exactly one child, regardless of the value of bf. Trees are constructed in breadthfirst order, so that the depth of the finished tree is a minimum for the given values of nb and bf.
Body i is taken to be a thinwalled cylinder of length l(i), radius l(i)/20 and mass m(i), lying between 0 and l(i) on the x axis of body i's local coordinate system. The values of l(i) and m(i) are determined by the argument taper, which specifies the size ratio of consecutivelynumbered bodies (i.e., body i is taper times larger than body i1). The formulae are l(i)=taper^(i1) and m(i)=taper^(3*(i1)). Thus, if taper==1 then l(i)==1 and m(i)==1 for all i.
Two axes are embedded in body i: an inboard axis, which is aligned with the z axis of body i's coordinate frame, and an outboard axis, which passes through the point (l(i),0,0) and is rotated relative to the inboard axis by an angle skew about the x axis. Thus, if skew==0 then the two axes are parallel, and if skew==pi/2 then the outboard axis is parallel to the negative y axis. The inboard axis serves as the axis for joint i; and the outboard axis serves as the axis for each joint connecting body i to a child. If skew==0 then every joint axis is parallel, and the kinematic tree is a planar mechanism.
autoTree
also creates a set of drawing instructions, so that the resulting model
can be viewed using drawmodel.
The arguments bf, skew and taper are all
optional. If omitted, they default to the values bf==1, skew==0 and taper==1.
This function constructs a floatingbase kinematic tree from a given fixedbase kinematic tree. It does this by replacing joint 1 in the given tree with a chain of 5 massless bodies and 6 joints, which is designed to simulate a 6DoF joint. As a result, body i in model becomes body i+5 in fbmodel, and fbmodel.NB == model.NB+5. Body 6 in fbmodel is the floating base; joints 1 to 6 simulate a 6DoF joint; and joints 7 to fbmodel.NB are the real joints in the floatingbase system.
The joints added by floatbase
are: three prismatic joints in the x, y and z directions,
followed by three revolute joints about the x, y and z axes.
This chain has a kinematic singularity when the angle of joint 5 is
either pi/2
or pi/2.
If you use standard forward or hybrid dynamics algorithms on this
model, then they will fail when this chain is in a singularity, and
they will lose accuracy as the chain approaches a singularity.
However, the floatingbase dynamics algorithms ignore this chain, and
are therefore immune to its singularity.
planarN
constructs simple planar serial robots in which all the joints are
revolute and all the links are identical. The robot moves in the xy plane, and
it lies on the x
axis when the joint angles are all zero. Each link has unit
length, unit mass, and a rotational inertia of 1/12, which is the
inertia of a thin rod. The model includes drawing instructions
for drawmodel. planar2 and planar3
provide efficient access to the robots planarN(2)
and planarN(3).
Each function works by returning the value of a stored copy of a model
calculated by planarN.
The stored copy is initialized on the first call. These functions
are useful in Simulink models (e.g. planar2ctrl).
These functions calculate the 3x3 matrices such that the expressions crf(v)*f and crm(v)*m are the cross products of the planar motion vector v with the planar force vector f and planar motion vector m, respectively. If f and m are fixed in a rigid body that is moving with a velocity of v, then these expressions give the time derivatives of f and m due to the motion of the body.
Construct the 3x3 planar inertia matrix of a rigid body from its mass m, its centre of mass c (2D vector) and its rotational inertia I (scalar) about its centre of mass.
Calculate the 3x3 coordinate transform matrix that transforms a planar motion vector from A coordinates to B coordinates, where the (2D) Cartesian frame that defines B coordinates is both translated and rotated relative to the frame that defines A coordinates. The 2D vector r contains the coordinates of frame B's origin, expressed in A coordinates, and theta is the angle (in radians) by which frame B is rotated relative to frame A. See also general transform properties.
Example:Calculate a planar motion vector v that
approximates to the displacement defined by the planar coordinate
transform X.
Specifically, if X is the
transform from A coordinates to B coordinates, then the location of
frame B relative to frame A can be deduced from the value of X.
Thus, X
can supply a description of the displacement that carries frame
A to frame B. If the angle between these frames is small, then
the displacement can also be described by means of the velocity vector,
u, that
carries frame A to frame B in one time unit. The function XtoVp
calculates a vector v that
approximates to u,
and that converges to the exact value as the angle between A and B
approaches zero. v also has
the special property of being an invariant of X, meaning that v ==
X*v.
Thus, v can be regarded as being expressed both in A coordinates and in
B coordinates.
Let X denote the coordinate transform from A to B coordinates for motion vectors; and let mA, mB, fA, fB, IA and IB denote motion vectors, force vectors and inertia matrices expressed in A and B coordinates, respectively; then X has the following properties. (Watch out for transpose operators and minus signs.)
Caution: in this software library, phrases like "rotation about the x axis" are associated with the coordinate transform from the current coordinate system to the rotated coordinate system. Readers are warned that many authors use a different convention. In particular, phrases like "rotation about the x axis" are sometimes associated with the transform from the rotated coordinate system back to the current coordinate system, which is the exact opposite of the association used here.
Calculate the forward dynamics of a kinematic tree using a
planarvector version of the
articulatedbody algorithm. model is a
planarvector system model data
structure that specifies the number of bodies and joints, the
connectivity, and so on. q, qd, qdd and tau are
vectors of joint
position, velocity, acceleration and force variables, respectively,
having dimensions of model.NB x 1
(i.e., they are column
vectors). f_ext
is a cell array of planar force vectors
specifying the external forces acting on the bodies. If there are
no external forces, then f_ext can
take the value {}.
Otherwise,
f_ext
must contain model.NB
elements, and each element f_ext{i} must
either be empty, to indicate that there is no external force acting on
body i,
or else it must be a planar force vector specifying the force
acting on body i,
expressed in body i
coordinates. grav_accn is
a 2D vector specifying the acceleration in the xy plane due
to a uniform gravitational
field, expressed in base coordinates. Both f_ext and grav_accn
are optional arguments. If they are omitted, then they default to
the values {}
and [0,0],
respectively.
Calculate the forward dynamics of a kinematic tree using a planarvector version of the compositerigidbody algorithm. model is a planarvector system model data structure that specifies the number of bodies and joints, the connectivity, and so on. q, qd, qdd and tau are vectors of joint position, velocity, acceleration and force variables, respectively, having dimensions of model.NB x 1 (i.e., they are column vectors). f_ext is a cell array of planar force vectors specifying the external forces acting on the bodies. If there are no external forces, then f_ext can take the value {}. Otherwise, f_ext must contain model.NB elements, and each element f_ext{i} must either be empty, to indicate that there is no external force acting on body i, or else it must be a planar force vector specifying the force acting on body i, expressed in body i coordinates. grav_accn is a 2D vector specifying the acceleration in the xy plane due to a uniform gravitational field, expressed in base coordinates. Both f_ext and grav_accn are optional arguments. If they are omitted, then they default to the values {} and [0,0], respectively. This function calls HandCp to calculate the coefficients of the equation of motion, and then solves the resulting linear equation for qdd.
Calculate the coefficients H and C of the equation of motion H*qdd+C=tau for the kinematic tree described by model. H is the jointspace inertia matrix, and is a symmetric, positivedefinite matrix. C is the jointspace bias force, and has the same value as the vector calculated by IDp(model,q,qd,zeros(model.NB,1),f_ext,grav_accn). H and C are calculated using planarvector versions of the compositerigidbody and recursive NewtonEuler algorithms, respectively. model is a planarvector system model data structure that specifies the number of bodies and joints, the connectivity, and so on; and q and qd are vectors of joint position and velocity variables having dimensions of model.NB x 1 (i.e., they are column vectors). f_ext is a cell array of planar force vectors specifying the external forces acting on the bodies. If there are no external forces, then f_ext can take the value {}. Otherwise, f_ext must contain model.NB elements, and each element f_ext{i} must either be empty, to indicate that there is no external force acting on body i, or else it must be a planar force vector specifying the force acting on body i, expressed in body i coordinates. grav_accn is a 2D vector specifying the acceleration in the xy plane due to a uniform gravitational field, expressed in base coordinates. Both f_ext and grav_accn are optional arguments. If they are omitted, then they default to the values {} and [0,0], respectively.
Calculate the inverse dynamics of a kinematic tree using a planarvector version of the recursive NewtonEuler algorithm. model is a planarvector system model data structure that specifies the number of bodies and joints, the connectivity, and so on. q, qd, qdd and tau are vectors of joint position, velocity, acceleration and force variables, respectively, having dimensions of model.NB x 1 (i.e., they are column vectors). f_ext is a cell array of planar force vectors specifying the external forces acting on the bodies. If there are no external forces, then f_ext can take the value {}. Otherwise, f_ext must contain model.NB elements, and each element f_ext{i} must either be empty, to indicate that there is no external force acting on body i, or else it must be a planar force vector specifying the force acting on body i, expressed in body i coordinates. grav_accn is a 2D vector specifying the acceleration in the xy plane due to a uniform gravitational field, expressed in base coordinates. Both f_ext and grav_accn are optional arguments. If they are omitted, then they default to the values {} and [0,0], respectively.
This function is used by the planarvector dynamics algorithms to calculate the joint transform (Xj) and motion subspace matrix (S) for a revolute joint (code==1), a prismatic joint sliding in the x direction (code==2), or a prismatic joint sliding in the y direction (code==3). Xj is the coordinate transform from the joint's predecessor frame to its successor frame; and S is expressed in successorframe coordinates. In a kinematic tree, the successor frame of joint i is also the body coordinate frame of body i, and the joint's predecessor frame is fixed in the parent body of body i. For revolute joints, q is the joint angle in radians. For prismatic joints, q is the linear displacement.
The planarvector version of the system model data structure is identical to the spatialvector version, except for the following differences:
Generate system models of kinematic trees having revolute joints. nb specifies the number of bodies in the tree, and must be an integer >=1. bf specifies the branching factor of the tree, which is a number >=1 indicating the average number of children of a nonterminal node. bf==1 produces an unbranched tree; bf==2 produces a binary tree; and noninteger values produce trees in which the number of children alternate between floor(bf) and ceil(bf) in such a way that the average is bf. However, the tree is designed so that the fixed base always has exactly one child, regardless of the value of bf. Trees are constructed in breadthfirst order, so that the depth of the finished tree is a minimum for the given values of nb and bf.
Body i is taken to be a thinwalled cylinder of length l(i), radius l(i)/20 and mass m(i), lying between 0 and l(i) on the x axis of body i's local coordinate system. The values of l(i) and m(i) are determined by the argument taper, which specifies the size ratio of consecutivelynumbered bodies (i.e., body i is taper times larger than body i1). The formulae are l(i)=taper^(i1) and m(i)=taper^(3*(i1)). Thus, if taper==1 then l(i)==1 and m(i)==1 for all i.
Two axes are embedded in body i: an inboard axis, which is aligned with the z axis of body i's coordinate frame, and an outboard axis, which passes through the point (l(i),0,0) and is rotated relative to the inboard axis by an angle skew about the x axis. Thus, if skew==0 then the two axes are parallel, and if skew==pi/2 then the outboard axis is parallel to the negative y axis. The inboard axis serves as the axis for joint i; and the outboard axis serves as the axis for each joint connecting body i to a child. If skew==0 then every joint axis is parallel, and the kinematic tree is a planar mechanism.
The arguments bf and taper are optional. If omitted, they default to the values bf==1 and taper==1.
This function creates a graphics data file in the current directory,
depicting either a system model or an animation, and attempts to
display it using either the program anim8
(1st choice) or SceneViewer
(2nd choice). (Both programs use the Open Inventor
graphics
library, which is an objectoriented library built on top of
OpenGL.) model
is a spatialvector system model data structure containing an appearance field; delta_t is
the time interval between frames in an animation; and jnt_vals is
array containing one row for each frame in an animation and one column
for each joint variable, and jnt_vals(i,j)
is the value of joint variable j in frame i.
Linear interpolation is used to work out the values of the joint
variables in between frames.
If only one argument is supplied, then drawmodel creates a file called data_km.iv, written in Open Inventor text format, that depicts the given system model. SceneViewer displays this file as a still image of the model in its home position (all joint variables zero). anim8 initially displays the same scene, but also presents a control panel (in a separate window) with one slider per joint variable, so that the user can interactively modify joint variables and see the result.
If all three arguments are supplied, then drawmodel creates a file called data_mv.iv, also in Open Inventor text format, that depicts an animation of the given system model. SceneViewer displays this animation in an infinite loop, with a short pause both at the beginning and at the end, so that the user has a chance to study the initial and final configurations as well as the motion in between. anim8 displays the initial configuration, and presents a control panel (in a separate window) with a slider for the time line, a slider for the playing speed, a play/pause button and an autorepeat on/off button. Both programs allow you to pan, zoom and rotate the view while the animation is playing.
Availability of SceneViewer and anim8: SceneViewer is the standard viewer that comes with the Open Inventor library. anim8 is a program written many years ago by me (Roy Featherstone), and subsequently modified by students who did not properly understand how to write portable graphics software. The current version runs only on an Intel machine under Linux, with all the right graphics and window libraries (whatever they are). If you are very lucky, then the binary file might just work on your machine. If not, then the only way you can get to use anim8 is by porting it yourself from the source code. If you really want to do this, then contact me, and I'll give you the source code and what little advice I can.
This is a Simulink model of a planar twolink robot (constructed by
planar2) under the control of a simple PD
(proportional plus
derivative) control system. The parameters of the control system
have been chosen so that the robot does not follow the commanded
trajectory very well, and you can see the effects of the robot's
dynamics on the overall behaviour of the system. The simulation
is ready to go: just click on Start
in the simulation menu.
These are all matlab scripts that test the various dynamics
functions by demonstrating their mutual consistency. They also
serve as examples of how to use the dynamics routines. The best
way to use them is to read the source code, and then run them under
matlab. test_FD
demonstrates the mutual consistency of FDab, FDcrb and ID by showing that both
FDab and FDcrb are inverses
of ID; test_HD
demonstrates the correctness of HD by comparing it
with complete dynamics data prepared using ID; test_fb1
shows that IDf is the inverse of FDf;
test_fb2
compares the data calculated by IDf and FDf with the data calculated by FDab
on a fixedbase equivalent of a floatingbase system; and test_p tests
the functions IDp, FDabp, FDcrbp and HandCp by showing
that FDabp and FDcrbp are
inverses of IDp (i.e., the planar equivalent of test_FD), and
by showing that both HandC and HandCp
produce the same equation of motion for the same planar kinematic tree.
This function calculates the `expanded parent array', which is the parent array used by branchinduced sparsity functions, from the parent array of a kinematic tree (lambda) and an array of joint motion freedoms (nf), using the algorithm described in Table 6.4 of RBDA. Specifically, lambda is an array of integers, containing one entry per body, such that lambda(i) is the parent of body i; nf is an array of integers, containing one entry per tree joint (which is the same as one entry per body), such that nf(i) is the number of degrees of motion freedom (DoF) allowed by joint i; and newLambda is an array of integers, containing sum(nf) elements, such that newLambda can be regarded as the parent array of a new kinematic tree that is obtained from the original by replacing every joint having more than one DoF with an appropriate chain of singleDoF joints. Thus newLambda is an array containing one entry per joint variable, rather than one entry per joint. The idea of an expanded parent array is explained further in RBDA and in [Fea05].
By definition, if every joint in the kinematic tree has only one
DoF, then newLambda==lambda.
As the functions and data structures in this library only support
singleDoF joints, it follows that the use of expandLambda
is not strictly necessary with the current set of dynamics functions.
These functions perform the LTL and LTDL factorizations on a given matrix, H, which must be symmetric and positivedefinite, and must satisfy the following condition: the nonzero elements on row i of H below the main diagonal must appear only in columns lambda(i), lambda(lambda(i)), and so on. lambda can be any array of integers such that length(lambda) equals the dimension of H, and 0<=lambda(i)<i for all i. This sparsity pattern is called branchinduced sparsity, and it is explained in detail in RBDA and [Fea05]. Its relevance to dynamics is that the jointspace inertia matrix (see HandC) exhibits branchinduced sparsity, and the exploitation of this sparsity can result in significant cost savings.
LTL calculates a lowertriangular matrix, L, that satisfies L'*L==H (LTL factorization); and LTDL calculates a unitlowertriangular matrix, L, and a diagonal matrix, D, that satisfy L'*D*L==H (LTDL factorization). Both these factorizations preserve the branchinduced sparsity pattern of H. Thus, the nonzero elements on row i of L below the main diagonal appear only in columns lambda(i), lambda(lambda(i)), and so on.
This function calculates the product y=H*x, where x and y are
vectors, and H
is a symmetric matrix with branchinduced
sparsity. H
must satisfy the following condition: the nonzero
elements on row i
of H
below the main diagonal must appear only in
columns lambda(i),
lambda(lambda(i)),
and so on. lambda can be
any array of integers such that length(lambda)
equals the dimension of
H, and 0<=lambda(i)<i
for all i.
These functions calculate the following products: y=L*x, y=L'*x, y=inv(L)*x and y=inv(L')*x, respectively. In every case, x and y are vectors, and L is a lowertriangular matrix with branchinduced sparsity, such as those calculated by LTL and LTDL. L must satisfy the condition that the nonzero elements on row i of L below the main diagonal appear only in columns lambda(i), lambda(lambda(i)), and so on. mpyLi and mpyLit also require that the diagonal elements of L be nonzero. lambda can be any array of integers such that length(lambda) equals the dimension of H, and 0<=lambda(i)<i for all i.
Example: