Spatial Vector and Rigid-Body Dynamics Functions

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.)

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.

• XtoV( Xtrans( [a b c] ) )  ==  [ 0; 0; 0; a; b; c ]
• XtoV( Xroty( 0.01 ) )  is approximately equal to  [ 0; 0.01; 0; 0; 0; 0 ]

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 fixed-base to floating-base coordinates; v and a are the spatial velocity and acceleration of the floating base, expressed in fixed-base 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 6-DoF 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 floating-base 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.

• [q, qd, qdd] = fbKin( Xrotz(c)*Xroty(b)*Xrotx(a)*Xtrans[1 2 3]), zer, zer ),  where  zer = zeros(6,1),  results in  q == [ 1; 2; 3; a; b; c ]
• [X, v, a] = fbKin( zer, [1;2;3;4;5;6], zer )  results in  v == [ 4; 5; 6; 1; 2; 3 ]

Calculate the forward dynamics of a kinematic tree using the articulated-body 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 composite-rigid-body 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 floating-base kinematic tree using the articulated-body 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 fixed-base to floating-base coordinates; vfb and afb are the spatial velocity and acceleration of the floating base, expressed in fixed-base 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 6-DoF 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 fixed-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 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 joint-space inertia matrix, and is a symmetric, positive-definite matrix.  C is the joint-space 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 composite-rigid-body and recursive Newton-Euler 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 articulated-body algorithm.  model is a system model data structure describing the kinematic tree; fd is an array of boolean values identifying the forward-dynamics 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 forward-dynamics joint or an inverse-dynamics joint.  The task of the hybrid dynamics function is then to calculate the unknown acceleration from the given force at each forward-dynamics joint, and the unknown force from the given acceleration at each inverse-dynamics joint.  HD uses the argument fd for this purpose.  Specifically, fd(i)==1 for each joint i that is a forward-dynamics joint, and fd(i)==0 for each joint i that is an inverse-dynamics 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 Newton-Euler 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 floating-base 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 fixed-base to floating-base coordinates; vfb and afb are the spatial velocity and acceleration of the floating base, expressed in fixed-base 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 6-DoF 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 fixed-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 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 successor-frame 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 fixed-base kinematic trees containing revolute, prismatic and helical joints only.

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 non-integer 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 breadth-first 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 thin-walled 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 consecutively-numbered bodies (i.e., body i is taper times larger than body i-1).  The formulae are l(i)=taper^(i-1) and m(i)=taper^(3*(i-1)).  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 floating-base kinematic tree from a given fixed-base 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 6-DoF 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 6-DoF joint; and joints 7 to fbmodel.NB are the real joints in the floating-base 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 floating-base 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 x--y 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.

• Xpln( theta, [a b] )  ==  Xpln( theta, [0 0] ) * Xpln( 0, [a b] )

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.

• XtoVp( Xpln( 0, [a b] ) )  ==  [ 0; a; b ]
• XtoVp( Xpln( 0.01, [0 0] ) )  is approximately equal to  [ 0.01; 0; 0 ]

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.)

• motion vector transformation rule:   mB = X * mA   and   mA = inv(X) * mB
• force vector transformation rule:   fB = inv(X') * fA   and   fA = X' * fB
• inertia matrix transformation rule:   IB = inv(X') * IA * inv(X)   and   IA = X' * IB * X
• if X is any pure rotation then  inv(X') == X
• if  X=Xrotx(theta)  then  inv(X) == Xrotx(-theta),  and similarly for Xroty, Xrotz and Xpln(theta, [0 0])
  
• if  X=Xtrans(r)  then  inv(X) == Xtrans(-r),  and similarly for Xpln(0,r)

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 planar-vector version of the articulated-body algorithm.  model is a planar-vector 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 x--y 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 planar-vector version of the composite-rigid-body algorithm.  model is a planar-vector 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 x--y 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 joint-space inertia matrix, and is a symmetric, positive-definite matrix.  C is the joint-space 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 planar-vector versions of the composite-rigid-body and recursive Newton-Euler algorithms, respectively.  model is a planar-vector 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 x--y 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 planar-vector version of the recursive Newton-Euler algorithm.  model is a planar-vector 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 x--y 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 planar-vector 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 successor-frame 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 planar-vector version of the system model data structure is identical to the spatial-vector version, except for the following differences:

1. Joint types are identified by an array of type codes rather than an array of pitch values.  The array of joint type codes is called jcode.  The codes are: 1 for a revolute joint, 2 for a prismatic joint sliding in the x direction, and 3 for a prismatic joint sliding in the y direction.
2. The elements of the cell arrays Xtree and I are planar transforms and inertias (i.e., 3x3 matrices) rather than spatial transforms and inertias.
3. The field appearance is not supported.  (drawmodel only works for spatial-vector models.)

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 non-integer 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 breadth-first 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 thin-walled 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 consecutively-numbered bodies (i.e., body i is taper times larger than body i-1).  The formulae are l(i)=taper^(i-1) and m(i)=taper^(3*(i-1)).  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 object-oriented library built on top of OpenGL.)  model is a spatial-vector 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 auto-repeat 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 two-link 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 fixed-base equivalent of a floating-base 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 branch-induced 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 single-DoF 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 single-DoF 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 positive-definite, 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 branch-induced sparsity, and it is explained in detail in RBDA and [Fea05].  Its relevance to dynamics is that the joint-space inertia matrix (see HandC) exhibits branch-induced sparsity, and the exploitation of this sparsity can result in significant cost savings.

LTL calculates a lower-triangular matrix, L, that satisfies L'*L==H (LTL factorization); and LTDL calculates a unit-lower-triangular matrix, L, and a diagonal matrix, D, that satisfy L'*D*L==H (LTDL factorization).  Both these factorizations preserve the branch-induced 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 branch-induced 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 lower-triangular matrix with branch-induced 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.

• (7 Oct 2008)  Incorrect calls to HandC and HandCp appearing in test_p have been fixed.  The corrected source code can be viewed and downloaded directly via this documentation.
• (8 Mar 2008)  The undefined function 'skew' appearing in fbKin has been replaced with equivalent code.  The corrected source code can be viewed and downloaded directly via this documentation.