Version 2: June 2012 (latest bug fix: Feb 2015)
Released Under GNU GPL — Legal Notices — Download
Spatial_v2 is a suite of functions that implement spatial vector arithmetic and dynamics algorithms in Matlab code. Spatial_v2 accompanies the book Rigid Body Dynamics Algorithms (RBDA) and is also mentioned in the Springer Handbook of Robotics. (Handbook readers may wish to read this.) Ideally, you should have a copy of RBDA. However, if you don't have access to either of these books then you can still use spatial_v2, and you will find relevant tutorial materials on the spatial vectors home page. If you are new to this package then a good place to start is the Simulink Tutorial Examples. Alternatively, if you are not familiar with Simulink, then maybe try showmotion first.
| Additional Documents | |
|---|---|
| 3D Vectors and Rotation | |
| rx, ry, rz | rotational coordinate transforms about x, y and z axes |
| rv | convert between rotational coordinate transform matrix and rotation vector |
| rq | convert between rotational coordinate transform matrix and quaternion |
| rqd | calculate quaternion derivative from angular velocity |
| skew | convert between vector and skew-symmetric matrix |
| Spatial and Planar Vector Arithmetic | |
| crf | cross product operator (force) |
| crm | cross product operator (motion) |
| mcI | (de)compose rigid-body inertia to/from mass, centre of mass and rotational inertia |
| rotx, roty, rotz | spatial coordinate transforms (rotations about x, y and z axes) |
| xlt | spatial coordinate transform (translation) |
| plnr | compose/decompose a planar coordinate transform |
| plux | compose/decompose a spatial (Plücker) coordinate transform |
| pluho | convert between spatial and homogeneous (4×4) coordinate transforms |
| XtoV | compute a displacement vector from a coordinate transform |
| Xpt | apply a coordinate transform to one or more points |
| Vpt | calculate linear velocities from a spatial/planar velocity |
| Fpt | convert linear forces to spatial/planar forces |
| Dynamics Functions | |
| FDab | forward dynamics (articulated-body algorithm) |
| FDcrb | forward dynamics (composite-rigid-body algorithm) |
| FDgq | forward dynamics with constraints (composite-rigid-body algorithm) |
| HandC | coefficients of joint-space equation of motion |
| HD | hybrid dynamics (articulated-body algorithm) |
| ID | inverse dynamics (recursive Newton-Euler algorithm) |
| FDfb | floating-base forward dynamics (articulated-body algorithm) |
| IDfb | floating-base inverse dynamics |
| fbkin | forward and inverse kinematics of a floating base |
| fbanim | wrap-free floating-base kinematics for use with showmotion |
| EnerMo | calculates kinetic and potential energy, momentum and related quantities |
| jcalc | joint data calculation function used by dynamics functions |
| Model Constructors | |
| See also The System Model Data Structure | |
| autoTree | construct a variety of spatial kinematic trees |
| floatbase | construct a spatial floating-base model from a given spatial fixed-base model |
| planar | construct a simple unbranched planar robot with revolute joints and identical links |
| gfxExample | illustrates and explains a variety of drawing instructions |
| vee | model used in Simulink tutorial example 3 |
| singlebody | model used in Simulink tutorial example 4 |
| scissor | model used in Simulink tutorial example 5 |
| doubleElbow | model used in Simulink tutorial example 6 |
| rollingdisc | model used in Simulink tutorial example 7 |
| Animation | |
| showmotion | displays animations using Matlab's handle graphics |
| Simulink Tutorial Examples | |
| These Simulink models are all self-documenting, and can be found in the directory spatial_v2/simulink. | |
| example1.mdl | PD Control of a Planar 2R Robot — a basic example for beginners |
| example2.mdl | Discrete PD Control of a Planar 2R Robot — combines a sampled-time controller with continuous-time dynamics |
| example3.mdl | Free-Floating 'Vee' Robot — illustrates the use of HD |
| example4.mdl | Free-Floating Single Rigid Body — illustrates the use of FDfb and fbanim |
| example5.mdl | Free-Floating Two-Body Spinning Scissor — illustrates the use of gcFD, FDfb and fbanim |
| example6.mdl | Controlling a Robot with Kinematic Constraints — illustrates the use of FDgq |
| example7.mdl | Disc Bouncing and Rolling on a Horizontal Surface — demonstrates the Simulink ground-contact model |
| Simulink Ground-Contact Functions | |
| gcFD | Simulink wrapper for several dynamics functions |
| gcPosVel | calculates the positions and velocities of ground-contact points |
| gcontact | calculates the forces acting on ground-contact points |
| 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 |
These functions return 3×3 orthonormal matrices expressing coordinate transforms for 3-D Euclidean vectors from A to B coordinates, in which frame B (the frame that defines B coordinates) is rotated relative to frame A by an angle theta (radians) about their common x, y or z axis, as appropriate. See also general comments on coordinate transforms.
rv(v) calculates a 3×3 orthonormal matrix expressing the coordinate transform for 3-D Euclidean vectors from A to B coordinates, in which frame B (the frame that defines B coordinates) is rotated relative to frame A by an angle (in radians) given by the magnitude of v, about an axis given by the direction of v; and rv(E) performs the inverse operation. For example, rv([1;0;0]) returns the same matrix as rx(1), and rv(ry(2)) returns the vector [0;2;0]. rv(v) accepts any 3-D row or column vector as an argument, but rv(E) expects E to be accurately orthonormal, and returns a column vector with a magnitude in the range [0,pi]. (If the magnitide of the returned vector is (almost) exactly pi then its sign is unpredictable, as both v and −v represent the same rotation in this special case.) If the argument is a 3×3 matrix then it is assumed to be E, otherwise v. Note that v is common to both coordinate systems (i.e., it satisfies v=rv(v)*v), so it does not matter whether v is expressed in A coordinates or B coordinates. See also general comments on coordinate transforms.
This function converts between a unit quaternion, q, representing the orientation of a coordinate frame B relative to frame A, and the 3×3 coordinate rotation matrix E that transforms 3-D Euclidean vectors from A to B coordinates. For example, if B is rotated relative to A about their common x axis by an angle h, then q=[cos(h/2);sin(h/2);0;0] and rq(q) produces the same matrix as rx(h). If the argument is a 3×3 matrix then it is assumed to be E, otherwise q. rq(E) expects E to be accurately orthonormal, and returns a quaternion in a 4×1 matrix; but rq(q) accepts any nonzero quaternion, contained in either a row or a column vector, and normalizes it before use. As both q and −q represent the same rotation, rq(E) returns a value that satisfies q(1)≥0.
This function calculates the time derivative of a quaternion representing the relative orientation of two coordinate frames, given the relative angular velocity of those frames. Specifically, let the two frames be called A and B, and let q be a quaternion representing the orientation of frame B relative to frame A in the same sense as in function rq. Also, let wA and wB be the angular velocity of frame B relative to frame A, expressed in A and B coordinates, respectively; and let qd be the time derivative of q. Given these definitions, the precise operation of rqd can be stated as follows: both rqd(wA,q) and rqd(q,wB) calculate qd, the only difference between them being that one uses wA and the other wB. rqd examines the length of its first argument in order to work out how it is being called. If the first argument's length is 4 then it is assumed to be q, otherwise wA. The return value is a column vector, but each argument can be a row or a column vector.
The primary use of rqd is to support the calculation of q from qd by numerical integration. In this application, the magnitude of q has the capacity to drift without limit, due to integration truncation errors. (This is the main reason why rq accepts quaternions of any magnitude and normalizes them before use.) To prevent this from happening, rqd includes a magnitude-stabilizing term in the computed value of qd, so that if q is indeed obtained by integrating qd then its magnitude will converge towards 1.
skew(v) returns the 3×3 skew-symmetric matrix S corresponding to the given 3-D vector v; and skew(A) returns the 3-D vector corresponding to the skew-symmetric component of the given arbitrary 3×3 matrix A. For any two vectors a and b, skew(a)*b is the cross product of a and b. If the argument is a 3×3 matrix then it is assumed to be A, otherwise v. skew(A) returns a column vector, but skew(v) accepts a row or column vector argument.
crf(v) and crm(v) calculate the 6×6 and 3×3 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 velocity v, then these expressions give the time derivatives of f and m due to the motion of the body. If v is a 6-D vector then it is a spatial vector, and the return value is 6×6; otherwise v is assumed to be planar, and the return value is 3×3.
This function converts between a rigid-body inertia matrix, rbi, and the three quantities m, c and I, representing the mass, centre of mass, and rotational inertia about the centre of mass, respectively, of a rigid body. In the planar case, rbi is a 3×3 matrix, c is a 2-D vector, and I is a scalar. In the spatial case, rbi is a 6×6 matrix, c is a 3-D vector, and I is a 3×3 matrix. When c is an argument, it can be a row vector or a column vector, but it is returned as a column vector. mcI counts its arguments in order to decide which operation to perform, and it tests the dimensions of argument c or rbi, as appropriate, to determine whether to use planar or spatial arithmetic. Calculating m, c and I from rbi is possible only if m is not zero.
These functions calculate the spatial coordinate transform matrices that transform a motion vector from A coordinates to B coordinates, where frame B (the frame that defines B coordinates) is rotated relative to frame A by an angle theta (radians) about their common x, y or z axis, as appropriate. See also general comments on coordinate transforms.
This function calculates the spatial coordinate transform matrix that transforms a motion vector from A coordinates to B coordinates, where frame B (the frame that defines B coordinates) is translated relative to frame A by an amount r, which is a 3-D vector giving the coordinates of the origin of frame B expressed in A coordinates. r can be a row vector or a column vector. See also general comments on coordinate transforms.
This function calculates the 3×3 matrix X expressing the planar coordinate transform from A to B coordinates, given a 2-D vector r locating the origin of frame B in A coordinates, and the angle theta (radians) by which frame B is rotated relative to frame A. (Positive rotation is counter-clockwise.) It can also perform the reverse calculation. r can be supplied as either a row or a column vector, but it is returned as a column vector. plnr counts its arguments in order to decide which operation to perform. See also general comments on coordinate transforms.
This function calculates the 6×6 matrix X expressing the Plücker (i.e., spatial) coordinate transform from A to B coordinates, given a 3-D vector r locating the origin of frame B in A coordinates, and the 3×3 matrix E expressing the rotational coordinate transform from A to B coordinates. It can also perform the reverse calculation. r can be supplied as either a row or a column vector, but it is returned as a column vector. plux counts its arguments in order to decide which operation to perform. Example: plux(rx(1),[2 3 4]) makes the same transform as rotx(1)*xlt([2 3 4]). See also general comments on coordinate transforms.
This function converts a 6×6 Plücker (i.e., spatial) coordinate transform matrix to an equivalent 4×4 homogeneous coordinate transform matrix, and vice versa. X and T are equivalent if they both express a coordinate transform from a coordinate system defined by Cartesian frame A to a coordinate system defined by Cartesian frame B. pluho examines the dimensions of its argument in order to decide which operation to perform. Note: the 4×4 matrices used in Matlab handle graphics are displacement operators which are the inverses of coordinate transforms. (See general comments on coordinate transforms.) Therefore, if you want (e.g.) to translate the children of an hgtransform object by an amount r, and then rotate them by an angle h about the (translated) x axis, and you have already calculated X=plux(rx(h),r), then the correct matrix for the hgtransform object is inv(pluho(X)).
Calculate a motion vector v representing the relative location of coordinate frame B with respect to frame A, given the coordinate transform X from A to B coordinates. Specifically, let u be the smallest velocity vector that carries frame A to coincide with B in one time unit; then v approximates to u when the angle between the frames is small, and converges to the exact value as the angle between the frames converges to zero. The particular value of v calculated by XtoV also has the special property of being an invariant of X, meaning that v = X*v; so v can be regarded as being expressed in both A and B coordinates simultaneously. X can be either a spatial (6×6) or a planar (3×3) coordinate transform, and XtoV returns either a spatial or a planar vector accordingly.
These functions provide an interface between spatial and planar quantities, on the one hand, and 2-D and 3-D points and vectors, on the other hand. In each case, the function tests the dimensions of the first argument in order to determine whether it is working in 3-D or 2-D. The second argument, p, is then expected to be either a 3×n or 2×n array, as appropriate, containing the coordinates of a set of n 3-D or 2-D points. Xpt applies the Plücker or planar coordinate transform X to each point in p, returning the new coordinates in the array xp, which has the same dimensions as p. Vpt expects its first argument to be either a single spatial or planar vector, or else a 6×n or 3×n array of n spatial or planar vectors. In the first case, it calculates the linear velocity at each point in p due to the given single spatial or planar velocity v, and returns the results in vp. (This amounts to a vector field calculation: Vpt maps each point to the vector at that point, in accordance with the vector field defined by v.) In the second case, column i of vp is calculated from columns i of v and p. In both cases, vp has the same dimension as p. Finally, Fpt calculates spatial and planar forces from linear forces acting at points. Both fp and p must be either 3×n or 2×n matrices, and the return value, f, is either 6×n or 3×n, as appropriate. In both cases, column i of f is calculated from columns i of fp and p. If you want to calculate a single spatial or planar force that is the resultant of a system of n forces acting at n points, then use f=sum(Fpt(fp,p),2).
Calculate the forward dynamics of a kinematic tree using the articulated-body algorithm. model is a system model data structure describing the rigid-body system. q, qd, qdd and tau are column vectors of length model.NB containing the joint position, velocity, acceleration and force variables, respectively. f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial or planar force vector (as appropriate) specifying the force acting on body i, expressed in absolute (i.e., base) coordinates.
Calculate the forward dynamics of a kinematic tree using the composite-rigid-body algorithm. model is a system model data structure describing the rigid-body system. q, qd, qdd and tau are column vectors of length model.NB containing the joint position, velocity, acceleration and force variables, respectively. f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial or planar force vector (as appropriate) specifying the force acting on body i, expressed in absolute (i.e., base) coordinates. 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 constrained kinematic tree using the composite-rigid-body algorithm and a constraint function. model is a system model data structure describing the rigid-body system. It must include the field model.gamma_q, which must contain the handle of a function describing the constraints on the kinematic tree. See describing constraints with gamma_q for a description of how gamma_q works, and how to write your own. q, qd, qdd and tau are column vectors of length model.NB containing the joint position, velocity, acceleration and force variables, respectively. q and qd are not required to satisfy the constraints exactly, but they are expected to be reasonably close. The return value, qdd, will normally include a constraint-stabilization term such that if q and qd are obtained from qdd by numerical integration then the constraint violations will tend towards zero. Internally, FDgq uses corrected values of q and qd that satisfy the constraints either exactly or more accurately than the given values. f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial or planar force vector (as appropriate) specifying the force acting on body i, expressed in absolute (i.e., base) coordinates.
Calculate the coefficients H and C of the joint-space equation of motion H*qdd+C=tau for the kinematic tree described by model. H is a symmetric, positive-definite matrix called the joint-space inertia 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). H and C are calculated using the composite-rigid-body and recursive Newton-Euler algorithms, respectively. model is a system model data structure describing the rigid-body system. q and qd are column vectors of length model.NB containing the joint position and velocity variables, respectively. f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial or planar force vector (as appropriate) specifying the force acting on body i, expressed in absolute (i.e., base) coordinates.
Calculate a hybrid of forward and inverse dynamics using the articulated-body algorithm. model is a system model data structure describing the rigid-body system; and fd is an array of model.NB 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. They are all column vectors of length model.NB. f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial or planar force vector (as appropriate) specifying the force acting on body i, expressed in absolute (i.e., base) coordinates.
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.
Calculate the inverse dynamics of a kinematic tree using the recursive Newton-Euler algorithm. model is a system model data structure describing the rigid-body system. q, qd, qdd and tau are column vectors of length model.NB containing the joint position, velocity, acceleration and force variables, respectively. f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial or planar force vector (as appropriate) specifying the force acting on body i, expressed in absolute (i.e., base) coordinates.
Calculate the forward dynamics of a spatial floating-base kinematic tree using the articulated-body algorithm and a quaternion to represent the orientation of the floating base. Use this function if you are worried about the kinematic singularities in the three-prismatic, three-revolute chain used by floatbase to emulate a 6-DoF joint between the fixed and floating bases. If this singularity is not a problem, then you can use FDab or FDcrb instead. Note that FDfb works only with spatial models, not planar.
model is a system model data structure created by floatbase, or any other system model that adheres to the same conventions. xfb is a 13×1 state vector containing the following quantities: a unit quaternion expressing the orientation of the floating base relative to the fixed base; a 3-D vector specifying the position of the origin of the floating base's coordinate frame in fixed-base coordinates; and a spatial vector giving the velocity of the floating base in fixed-base coordinates. The return value xdfb is the time-derivative of xfb. The variables q, qd, qdd and tau are all column vectors of length model.NB−6 containing the position, velocity, acceleration and force variables, respectively, for the real joints in the system; that is, joints 7 to model.NB. (So element i in any of these vectors contains a variable belonging to joint i+6.) f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial force vector specifying the force acting on body i, expressed in absolute (i.e., base) coordinates. FDfb ignores the first five entries in f_ext.
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, and five massless rigid bodies. A singularity occurs in this chain when q(5)=±pi/2. FDfb avoids this singularity by ignoring this chain, and using instead the contents of xfb to directly define the position, orientation and velocity of body 6. In effect, FDfb is replacing an emulated 6-DoF joint with a real one. The use of a quaternion to represent orientation ensures that there are no singularities.
See also: fbkin for more information about xfb, fbanim if you want to view an animation of a simulation run employing FDfb, and Simulink tutorial examples 4 and 5 for examples of how to use FDfb in a Simulink model.
Calculate the inverse dynamics of a floating-base kinematic tree using the algorithm described in Table 9.6 of RBDA and the same quaternion-based singularity-free representation of floating-base orientation as used by FDfb. Note that IDfb is really a hybrid dynamics algorithm, despite its name, since the accelerations of the joints are given (i.e., the real joints—joints 7 onwards in a model created by floatbase), but not the acceleration of the floating base; and the objective of IDfb is to calculate the forces required at the joints and the resulting acceleration of the floating base. Use IDfb if you are worried about the kinematic singularities in the three-prismatic, three-revolute chain used by floatbase to emulate a 6-DoF joint between the fixed and floating bases. If the singularity is not a problem then you can use HD instead. If you want to calculate pure inverse dynamics then use ID instead. (ID itself will work at a singularity, but your ability to specify a velocity and acceleration of your choice is restricted.)
model is a system model data structure created by floatbase, or any other system model that adheres to the same conventions. xfb is a 13×1 state vector containing the following quantities: a unit quaternion expressing the orientation of the floating base relative to the fixed base; a 3-D vector specifying the position of the origin of the floating base's coordinate frame in fixed-base coordinates; and a spatial vector giving the velocity of the floating base in fixed-base coordinates. The return value xdfb is the time-derivative of xfb. The variables q, qd, qdd and tau are all column vectors of length model.NB−6 containing the position, velocity, acceleration and force variables, respectively, for the real joints in the system; that is, joints 7 to model.NB. (So element i in any of these vectors contains a variable belonging to joint i+6.) f_ext is an optional argument specifying the external forces acting on the bodies. If there are no external forces then this argument can be omitted, or it can be given the value {}. Otherwise, it must be a cell array of length model.NB, and each entry f_ext{i} must either be an empty array, to indicate that there is no external force acting on body i, or else a spatial force vector specifying the force acting on body i, expressed in absolute (i.e., base) coordinates. IDfb ignores the first five entries in f_ext.
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, and five massless rigid bodies. A singularity occurs in this chain when q(5)=±pi/2. IDfb avoids this singularity by ignoring this chain, and using instead the contents of xfb to directly define the position, orientation and velocity of body 6. In effect, IDfb is replacing an emulated 6-DoF joint with a real one. The use of a quaternion to represent orientation ensures that there are no singularities. See fbkin for more information about xfb.
|
[x, xd] = fbkin( q, qd, qdd ) x = fbkin( q, qd ) p = fbkin( q ) |
[q, qd, qdd] = fbkin( x, xd ) [q, qd] = fbkin( x ) q = fbkin( p ) |
[source code] |
This function performs kinematics calculations relating to a system model created by floatbase, or any system model that follows the same convention. In particular, fbkin converts between the singularity-free representation of the motion of a floating base used by FDfb and IDfb, and the joint variables of the six joints used to emulate the 6-DoF joint between the fixed and floating bases. The two calls on the top row perform position, velocity and acceleration calculations; the two on the middle row perform position and velocity calculations; and the two on the bottom row perform position calculations only. q, qd and qdd contain the position, velocity and acceleration variables, respectively, of the six joints emulating the 6-DoF joint. When q is an output, q(4) and q(6) are normalized to the range ±pi, and q(5) is normalized to the range ±pi/2. p is a 7-element vector having the form p=[qn;r], where qn is a quaternion describing the orientation of the floating base relative to the fixed base, and r is a 3-D vector specifying the position of the floating base's origin in fixed-base coordinates. x is a 13-element state vector having the form x=[qn;r;v], where v is the spatial velocity of the floating base expressed in fixed-base coordinates. xd is the time-derivative of x, and is therefore a 13-element vector of the form xd=[qnd;rd;a], where qnd, rd and a are the time-derivatives of qn, r and v. In particular, a is the spatial acceleration of the floating base in fixed-base coordinates. All inputs and outputs are column vectors. fbkin uses a combination of counting its inputs and outputs, and measuring the length of its first argument, in order to decide which calculations to perform.
In a system model created by floatbase, the first three joints are prismatic joints on the x, y and z axes; the next three are revolute joints on the x, y and z axes; body 6 is regarded as the floating base; and bodies 1 to 5 are massless. This arrangement suffers from a pair of singularities, which occur when the angle of joint 5 is ±pi/2. FDfb and IDfb avoid this problem by using a quaternion to represent the orientation of the floating base. The main purpose of fbkin is to translate between the quaternion-based representation used by these two functions and the joint-based representation used by all the other dynamics functions. However, the translation is not perfect for two reasons. First, the translation from x and xd to q, qd and qdd breaks down at the two singularities. (To be specific, it is the calculation of qd and qdd that breaks down. The calculation of q is still OK.) Second, the two singularities and the normalization of joint angles (mentioned above) mess up showmotion's strategy of linearly interpolating the joint variables between sample times. If the floating base gets close to a singularity then a showmotion animation may exhibit a disturbing amount of wobble that isn't really there; and the only way to fix it is to increase the sampling rate in the vicinity of the singularity, e.g. by decreasing the integration step size in the dynamics simulation that generated the data. The problems caused by normalization can be fixed by fbanim.
There are several formulae involving x, xd, etc., that are worth knowing. First, let X be the Plücker coordinate transform from fixed-base to floating-base coordinates; and let E be the rotational part of X (i.e., E is the 3×3 matrix that transforms 3-D vectors from fixed-base to floating-base coordinates). We now have E=rz(q(6))*ry(q(5))*rx(q(4)), and also E=rq(qn), r=q(1:3), and X=plux(E,r). Furthermore, as rq and plux are reversible, we also have qn=rq(E) and [E,r]=plux(X). If we divide v into its linear and angular components, so that v=[ω;vo], then rd=qd(1:3), vo=rd+r×ω and qnd=rqd(ω,qn). More details can be found in the source code.
This function converts floating-base position data from the quaternion-based format used by FDfb and IDfb to the joint-angle-based format required by showmotion. X is a matrix containing N sets of quaternion-based position data, and can have any of the following dimensions: 13×1×N, 13×N, 7×1×N or 7×N. Each column of X has the format described under fbkin for the vector x or p, and represents the position of a floating base at a particular time sample. If 13 rows are supplied then only the first 7 are actually used. Qr is an optional argument containing the joint position data for joints 7 onwards in the model to which X and Qr pertain. If Qr is supplied then its dimensions must be either M×1×N or M×N, where M=model.NB-6 for the relevant model. Q is either a 6×N or a (6+M)×N matrix, depending on whether or not Qr is supplied, containing joint position data in the format required by showmotion. The first 6 rows of Q contain the position data for the three prismatic and three revolute joints used in a floatbase-generated model to emulate a 6-DoF joint between the fixed and floating bases; and the remaining rows contain data copied across from Qr.
The purpose of fbanim is to solve the wrap-around problem. If you were to translate X into Q by applying fbkin to each column, then rows 4 and 6 in the resulting matrix would be normalized to the range ±pi; and row 5 would be normalized to ±pi/2. If you send this data to showmotion, then this is what you will see. Suppose the floating base is rotating about the x axis: the angle of rotation increases smoothly until it gets close to pi, then the floating base suddenly spins very rapidly backwards to an angle close to −pi, and then it continues rotating smoothly until the angle gets close to pi again, and so on. fbanim solves this problem by: (1) first translating X to Q using fbkin, and then (2) comparing adjacent columns to see if a wrap-around has occurred between them, and (3) fixing each wrap-around by adjusting the angles and carrying the adjustment forward. Item 2 relies on the assumption that the orientation of the floating base does not change very much from one sample time to the next, so that any large angle change can be assumed to be caused by a wrap-around. This assumption can break down close to a singularity.
If tout, xout and qout contain the sample times, floating-base data and remaining joint data (i.e., joints 7 onwards), and you want to see the animation, then call showmotion as follows:
Calculate the energy, momentum, and some related quantities, of a given rigid-body system. model is a system model data structure describing the rigid-body system; and q and qd are column vectors of length model.NB containing the joint position and velocity variables, respectively. The return value is a structure with the following fields:
| ret.KE | total kinetic energy | ret.mass | total mass | |||
| ret.PE | total potential energy | ret.cm | position of the centre of mass | |||
| ret.htot | total spatial momentum | ret.vcm | linear velocity of centre of mass | |||
| ret.Itot | total spatial inertia |
This function is used by the dynamics functions to calculate the joint transform, Xj, and motion subspace matrix, S, for each joint in the given system model. The arguments specify the type of joint and the value of the joint's position variable. By convention, Xj is the coordinate transform from the joint's predecessor frame to its successor frame, and S is expressed in successor frame coordinates. What this means is that, if Xj and S have been calculated for joint i, and Xup is the coordinate transform from body i's parent to body i, then Xup=Xj*model.Xtree{i}; and if vj is the velocity of body i relative to its parent, expressed in body i coordinates, then vj=S*qd(i), where qd(i) is the velocity variable for joint i.
Each type of joint is identified by a joint type code, which is a string. The argument jtyp can either be a string containing the type code, or else a structure containing a field called code that contains the type code. The latter format is required for any joint that takes parameters, in which case jtyp must also contain a field called pars that contains the parameters. At present, the only joint type that takes a parameter is the helical joint. For this joint, jtyp.code must be set to 'H', and jtyp.pars.pitch must be set to the pitch of the joint. jcalc handles both spatial and planar joint types. For all spatial joint types, Xj is a Plücker coordinate transform and S is a 6×1 matrix. For planar joint types, Xj is a planar coordinate transform and S is a 3×1 matrix. For all revolute joints, and also the helical joint, q is an angle in radians. For all prismatic joints, q is a length in metres. (If you know what you are doing, then you can choose a different length unit; but you must be sure you are using a consistent set of physical units overall.)
| Spatial Joint Types | Planar Joint Types | |||
|---|---|---|---|---|
| Code | Description | Code | Description | |
| 'Rx' | revolute joint on x axis | 'r' | revolute joint | |
| 'Ry' | revolute joint on y axis | 'px' | prismatic joint on x axis | |
| 'Rz', 'R' | revolute joint on z axis | 'py' | prismatic joint on y axis | |
| 'Px' | prismatic joint on x axis | |||
| 'Py' | prismatic joint on y axis | |||
| 'Pz', 'P' | prismatic joint on z axis | |||
| 'H' | helical joint on z axis; pars.pitch: pitch in length
units per radian, +ve for right-handed screw |
|||
Generate system models of a variety of spatial kinematic trees having revolute joints, mainly for the purpose of generating test examples for dynamics functions. 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 breadth-first, so that the depth of the finished tree is a minimum for the given values of nb and bf. This means that the bodies and joints are numbered in breadth-first order.
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 linear 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 local 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/4 then the outboard axis points in the direction (0,−1,1) in the local coordinate system. The inboard axis serves as the axis for joint i; and the outboard axis serves as the axis for each joint that connects body i to one of its children. If skew=0 then every joint axis is parallel to the z axis, and the kinematic tree is a planar mechanism moving in the x–y plane.
autoTree also creates a set of drawing instructions, so that the resulting model can be viewed using showmotion. 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 spatial kinematic tree from a given fixed-base spatial 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 emulate a 6-DoF joint. As a result, body 1 in model becomes body 6 in fbmodel, and is regarded as the floating base. Likewise, joints 2 to model.NB in model become joints 7 to fbmodel.NB in fbmodel, and fbmodel.NB = model.NB+5. 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 ±pi/2. The standard forward and hybrid dynamics functions will break down at this singularity, but will work properly elsewhere. Furthermore, if you use an adaptive variable-step integrator, like Matlab's Ode45, then you can get very close to the singularity before suffering a significant loss of accuracy. (For example, it should be possible to approach to within about 0.01 radians of the singularity.) Nevertheless, several special floating-base functions are included in spatial_v2 that avoid this singularity by using a quaternion instead of three joint angles to represent the orientation of the floating base.
floatbase requires that model uses spatial arithmetic, and that only one body is connected to the base. It issues a warning if model.Xtree{1} is not the identity matrix, because any offset contained in this coordinate transform will be lost; and it issues a warning if model contains the field gamma_q, because floating the base will change the joint numbers, which may invalidate the function stored in this field. Drawing instructions, camera settings and Simulink ground-contact data are all preserved, and are edited to account for the changes in body numbers.
Construct a planar robot consisting of n identical links connected by revolute joints. The model uses planar-vector arithmetic. Each link lies on its local x axis, and has unit length, unit mass, and a rotational inertia of 1/12 about its centre of mass. This is consistent with the link being a uniform thin rod. When the joint angles are zero, the links are lined up on the x axis of the base coordinate system. Drawing instructions are included in the model. This function is suitable for use with Simulink.
This function constructs a robot having the same kinematics as planar(2), but adorned with a variety of graphical objects which, between them, illustrate all of the available drawing instructions. The source code is full of comments explaining how to use the various drawing instructions, and can be treated as a tutorial on how to prepare the drawing instructions for your own models. If gfxExample is called with an argument (any argument) then it gives the robot a tracking camera that tracks the tip of the white triangle. Otherwise, the robot is viewed with a fixed camera. To view this model, type showmotion(gfxExample).
This function creates the robot used in Simulink tutorial example 3. It is a free-floating spatial two-body system, in a zero-gravity environment, consisting of two thin rods connected by a revolute joint. The first rod has a mass and length of 1, and the second rod has a mass and length of L2. If the argument L2 is omitted then it defaults to 1, making the two rods identical. The unusual feature of this robot is that the two rods are at 45° to the joint axis. Specifically, when the robot is in its initial position, rod 1 lies on the x axis of the base coordinate frame, rod 2 lies on the y axis, and the revolute joint's axis points in the direction (1,1,0). Despite its simplicity, it is not immediately obvious what will happen when the robot starts to turn its joint. To help the viewer understand the motion, some construction lines are drawn in the base coordinate system to show the x, y and z axes, the line at 45° to the x and y axes, and the line joining the centres of mass of the two rods. The centre of mass on each rod is marked with a little red sphere; and if L2≠1 then three orange spheres are added: one to mark the system's centre of mass, and two to mark the points on each rod that coincide with the system's centre of mass when the angle between the two rods is zero. The source code is well commented.
This function creates a model of a box-shaped, free-floating single rigid body in a zero-gravity environment. The body has unit mass and dimensions of 3, 2 and 1 in the x, y and z directions, respectively. In its initial position, it has one vertex at the origin and another at the point (3,2,1). Its centre of mass is therefore offset from the origin. For visual guidance, the positive x, y and z axes are drawn in red, green and blue. This model is used in Simulink tutorial example 4 (chaotic tumbling).
This function creates a model of a free-floating rigid-body system, in a zero-gravity environment, consisting of two identical strip-shaped bodies which are connected at their middles by a revolute joint so that the system as a whole resembles a pair of scissors. In its initial position, the system's centre of mass lies at the origin, the revolute joint lies on the y axis, and the long axes of the strips are parallel to the z axis. This model is used in Simulink tutorial example 5.
This function creates a planar robot resembling planar(2), in which the second joint (the elbow) has been replaced by a pair of closely-spaced revolute joints which are constrained to both have the same joint angle. A compound joint of this kind can be implemented by a pair of gears, or a pair of pulleys and a figure-eight cable, both implementing a 1:1 gearing between the two joints at the elbow. This constraint is implemented via a gamma_q function, which is included in the model. (See Describing Constraints with gamma_q.) So the dynamics of this particular robot must be calculated using FDgq. This model is used in Simulink tutorial example 6.
This function creates a system model consisting of a free-floating single rigid body in the shape of a polyhedral approximation to a disc. The argument npt specifies the number of vertices in the polygon used to approximate a circle. The special feature of this model is that it includes a set of 2*npt ground-contact points—one for each vertex in the polyhedron—so that it can be used in conjunction with the Simulink ground-contact model. In this context, the disc can bounce, roll and tumble over a simulated compliant ground plane at z=0, as demonstrated in Simulink tutorial example 7.
|
showmotion( model [, tdata, qdata ] ) showmotion( filename ) showmotion( 'save', filename ) showmotion( 'about' [, meta ] ) |
start a new animation load animation from file save current animation display/edit metadata |
This function displays animations of system models. Typically, the motion data comes from a dynamics simulation run, in which case showmotion shows you how the system moved during that simulation. However, the motion data can come from any source, and does not have to be the result of a dynamics calculation. Note that showmotion is a post-processing tool: it displays a set of recorded motion data after that data has been calculated. You can use showmotion after a simulation run has finished, or after it has been stopped or paused, but you cannot use it to show you what is happening while the simulation run is in progress.
showmotion can be called with several argument formats. In the first call listed above, model is a system model data structure containing an appearance field and, optionally, also a camera field. The former supplies the drawing instructions that showmotion needs in order to display the model; and the latter controls how the model is viewed (details here). tdata is a row or column vector of monotonically-increasing time values. It is not necessary for the time values to be strictly monotonic. However, the last element in tdata must be strictly greater than the first. qdata is a matrix containing joint position data. It can be either an m×n array or an m×1×n array, where m is the number of joint variables (i.e., m=model.NB) and n is the number of elements in tdata. The second option is the array output format of Simulink; so Simulink outputs can be fed directly into showmotion. The contents of qdata(:,i) or qdata(:,1,i) (as appropriate) are interpreted as the vector of joint position variables at time tdata(i). To achieve a smooth animation, showmotion uses linear interpolation to calculate the values of the position variables at intermediate times.
If the arguments tdata and qdata are omitted, then showmotion supplies a default animation in which each joint variable in turn is ramped from 0 to 1 and back to 0 in the space of one second (so the complete animation lasts model.NB seconds). This is useful when you just want to view an existing model, or when you are building your own models and want to see if you have got the drawing instructions right and put the joints in the right places. (Note: the default animation ignores any kinematic constraints that may have been specified in model.gamma_q, and therefore cannot be used to test the correctness of this function.)
In the second and third calls listed above, filename is the name of a Matlab '.mat' file. In the second call, filename is the name of an existing file containing a previously-saved animation that showmotion is to load. In the third call, filename is the name of the file to which the current animation is to be saved. (Note: showmotion does not check to see if the save-file already exists, and does not warn you if you are about to overwrite an existing file.) In both cases, the '.mat' extension is optional. Thus, to load an animation from the file 'results.mat', filename can be either 'results' or 'results.mat'.
The fourth call listed above allows you to view and edit the metadata associated with an animation. The metadata consists of the following items: title, author(s), date stamp and description. Calling showmotion('about') causes showmotion to print the metadata associated with the current animation in the Matlab command window. Calling showmotion('about',meta) lets you replace one or more items of metadata with new values. meta is a structure containing one or more of the following fields: author, title and description. For each field present, showmotion replaces the relevant item with the specified new value. All three fields are strings. However, whereas author and title specify the new value directly, description is the full file name of a plain text file containing the new description. The date stamp is set automatically when an animation is saved, and cannot be edited. Probably, the easiest way to set the metadata is as follows:
showmotion lets you view an animation at any speed, from any angle, and at any magnification. It is intended to be easy and intuitive to use. However, there are, inevitably, some aspects of showmotion that are not so obvious. To find out more about how to use this program, see How to Use Showmotion. A few tips are listed below.
This is a Simulink wrapper for the forward-dynamics functions FDab, FDcrb, FDfb and FDgq. It serves two purposes: (1) to accept ground contact forces in the format used in the Simulink ground-contact model, and convert them into the format required by the forward-dynamics functions; and (2) to concatenate into a single vector the two return values of FDfb. The first argument is a system model data structure; the second is the Matlab function handle of the dynamics function to be used; and the third is the concatenation of the vectors q, qd, tau and (optionally) the ground contact force data from which f_ext is obtained. However, if the dynamics function is FDfb then u is the concatenation of x, q, qd, tau and optional ground contact force data. The return value is either the vector qdd calculated by the dynamics function, or the concatenation of xdfb and qdd in the case of FDfb. To allow u to have the dimensions of a column vector, the 3×n or 6×n (as appropriate) matrix of planar or spatial ground contact forces must be reshaped into a single column vector in which the first 3 or 6 elements contain the coordinates of the force acting on the first point, and so on. Both the Matlab reshape function and the Simulink reshape block can perform this operation. Examples of how to use gcFD can be found in Simulink tutorial examples 5 and 7.
This function calculates the positions and velocities of points that can make contact with the ground plane. It is used in conjunction with gcontact to implement the Simulink ground-contact model. model is a system model data structure containing the field gc, which is a structure containing the fields body and point. The latter is either a 2×np matrix, or a 3×np matrix, depending on whether model uses planar or spatial arithmetic, and the former is an np-element array of body numbers, where np is the number of points in the ground-contact model, which can be any number greater than zero. Thus, gc defines a total of np points, such that point i is fixed in body gc.body(i) and has coordinates gc.point(:,i) in the local coordinate system of body gc.body(i). The return value, posvel is either a 4×np or 6×np matrix, depending on whether model uses planar or spatial arithmetic, such that the top half of posvel(:,i) contains the position of point i, and the bottom half contains the linear velocity of point i, both expressed in base coordinates.
gcPosVel counts its arguments in order to determine how it has been called. If three arguments have been supplied then the second and third arguments are assumed to be q and qd. If either two or four arguments have been supplied then the second argument is assumed to be xfb. In the case of three arguments, q and qd are column vectors containing the position and velocity variables of every joint in model. In the case of two or four arguments, xfb is a 13-element state vector in the same format as used by the floating-base dynamics functions (e.g. see FDfb), and q and qd contain the position and velocity variables for joint 7 onwards. In this case, model must be a system model created by floatbase, or any other system model that adheres to the same conventions; and q and qd may be omitted if and only if model.NB=6.
This function calculates the forces acting on a set of points due to contact with a compliant ground plane. It is the main component in the Simulink ground-contact model. In 3D space, the ground plane is defined to be the x-y plane, and the z axis points up. In 2D space, the x axis is defined to be the ground plane, and the y axis points up.
In the first call listed above, gcontact uses a contact model that includes friction. In this case, K, D and mu are strictly-positive scalars giving the stiffness, damping and friction coefficients of the ground plane; p and pd are nd×np matrices giving the positions and linear velocities of a set of np points, where nd is the dimension of the space containing the points (so nd=2 or 3); u is an (nd-1)×np matrix of ground tangential-deformation state variables, which are used to calculate the friction forces; f is an nd×np matrix of ground-reaction forces acting on the points specified in p; ud is the time derivative of u; and fcone is a 1×np matrix used to distinguish between those points which are in contact with the ground and sliding over it, and those which are in contact and not sliding. (The latter are said to be sticking.) Specifically, point i is in contact with the ground if f(nd,i)>0, and it is sticking if fcone(i)≤1. The test for sticking will always return false for any point that is not in contact.
In the second call listed above, gcontact uses a frictionless contact model, and returns only the normal component of the contact force. In this case, K is a strictly-positive scalar giving the stiffness of the ground plane in the normal direction; D is a non-negative scalar giving the damping coefficient in the normal direction; the third argument is zero (gcontact distinguishes between the two calls by testing the value of its third argument); p and pd are 1×np matrices giving the normal components of the positions and velocities of a set of np ground-contact points; and f is a 1×np matrix giving the normal components of the ground-reaction forces acting on those points. Point i can be classified as being in contact with the ground if f(i)>0.
In both cases, gcontact determines the values of np and (if appropriate) nd from the size of p. Also, to facilitate its use with Simulink, if gcontact is called with only one return variable then it returns the concatenation of f, ud and fcone (i.e., the return value is [f;ud;fcone]).
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 thing 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 of 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; i.e., 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. Thus, L must satisfy the condition that the nonzero elements on row i 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: