Embed Size (px)
Citation preview
Some Applications of Screw Theory to Lumped-Parameter
Modeling of Visco-Elastically Coupled Rigid Bodies
Ernest D. Fasse
Dept. of Aerospace and Mechanical Engineering
The University of Arizona, Tucson, Arizona, USA
Abstract
This work considers the problem of modeling visco-elastically coupled rigid bodies, with
application to modeling and computer simulation of spatial, flexural mechanisms. Two modeling
methods are presented, both of which use elements of screw theory and dual number calculus.
The potential utility of the methods is demonstrated by simulating the behavior of a complex
spatial, flexural mechanism. Computational details are given in appendices.
1 Introduction
Modeling elastically coupled rigid bodies is an important problem in multibody dynamics (Schiehlen1997; Shabana 1997). This work concerns the problem of modeling what can be called flexuraljoints, where two essentially rigid bodies are coupled by a substantially more elastic body. Suchan idealized system is shown in Fig. 1. The geometry of the elastic body is not important, eventhough the geometry depicted is that of an axisymmetric beam. Flexural joints (flexures) are usedin a variety of engineering systems including adjustment mechanisms, locking mechanisms, shaftmisalignment couplers, and compliant robotic end-effectors (Paros and Weisbord 1965; Drake 1977;Whitney 1982; Smith and Chetwynd 1992; Koster 1998; Goldfarb and Speich 1999; Blanding 1999),see also (Kobrinskii 1969; Reshetov 1982). The use of flexural mechanisms is quite old. Centuries-oldmaterials like paper, leather, rope, and wire are well suited for flexural joints. Modern flexural jointdesign is arguably rooted in scientific instrument design (Negretti and Zambra 1925; Marsh 1929;Taylor and Waldram 1933; Dunbar 1934). Some practical examples of flexural joints are illustratedin Fig. 2. A natural example of a system with flexural joints is the human spine, which has beenmodeled as a system of elastically coupled rigid bodies (Panjabi, Brand, and White 1976; Merrill,Goldsmith, and Deng 1984; Brelin-Fornari 1998).
Two modeling methods are presented. In the first method, relative displacements of the coupledbodies are represented using twists, which are continuous rigid body motions that can be identifiedwith screw motions. Specifically these twists are represented using vectors of dual numbers, whichsimplifies kinematic analysis and computation. The strain energy potential function is approximatedby a quadratic function of the twist displacement. In the second method, relative displacementsare represented using dual quaternions. The strain energy potential function is approximated by a
1
(a)
PSfrag replacements AB
ab
a,b
(b)
PSfrag replacements AB
a b
a,b
Figure 1: Elastically coupled rigid bodies shown in (a) Undeformed, relaxed configuration and (b)Deformed, strained configuration. In this case two bodies are coupled by a compliant strut.
(a) (b)
Figure 2: Practical examples of flexural joints: (a) flexible shaft couplers, and (b) a mock recon-struction of a quasi-planar grinding spindle adjustment mechanism incorporating notch hinges.
quadratic function of the dual quaternion displacement. Both functions are shown to be independentthe choice of reference coordinate frame. Constitutive equations are derived via a virtual workargument using a simple dual number calculus. Compatible viscous behavior is defined for each ofthe methods. The potential utility of the methods is demonstrated by simulating the behavior ofa device similar to the ARCADE (Aristoteles Calibration Device). This complex spatial, flexuralmechanism incorporates 35 effectively rigid bodies, 36 notch hinges, and 12 cantilever flexures.
The rest of this paper is organized as follows. Section 2 reviews prior work relevant to geometricmodeling of elastically coupled rigid bodies. Section 3 introduces dual numbers, finite twists, dualquaternions, and other background information. Section 4 presents the “twist-based method”. Thepotential energy function defined in this section is identical to that presented in (Fasse and Zhang1999; Fasse, Zhang, and Arabyan 2000), but the derivation of the constitutive equations and otheranalysis using dual number methods is new. Also the viscous constitutive equations presented arenew. Section 5 presents a “dual quaternion-based method”. Section 6 presents preliminary resultsapplying the methods to modeling and computer simulation of a complex flexural mechanism. Finallythe results are summarized and discussed in Sec. 7.
2
2 Prior Work
The geometry of elastically coupled rigid bodies has been extensively reported in the literature.Most of the literature is on (1) the analysis of stiffness1 and (2) the synthesis of arbitrary stiffnessusing combinations of simple compliant elements.
Screw theory has been applied successfully to the analysis of elastically coupled rigid bodies.Ball (1900) defined “principal screws of the potential” and showed that they form a set of conjugate,co-reciprocal screws. Ball did not look at specific potential functions. Dimentberg (1965) went onto analyze a rigid body suspended by a system of ideal line springs, for which the potential functionof each spring is a quadratic function of the spring elongation. Griffis and Duffy (1991) showed thatspatial stiffness and compliance matrices can be characterized intuitively by their screw eigenvectors(eigenscrews). Their work is particularly relevant to the twist-based method presented in Sec. 4.Patterson and Lipkin (1993b, 1993a) looked in more detail at eigenvalue and generalized eigenvalueproblems, defining twist eigenscrews, wrench eigenscrews, and compliant axes. They classify stiffnessand compliance matrices in terms of the compliant axes. Ciblak (1998) continued this work, forexample, looking at the geometry of the centers of stiffness, compliance, and elasticity. Loncaric(1985, 1987), see also (Brockett and Loncaric 1986), examined the geometry of spatial complianceusing tools of Lie geometry. He defined normal forms of stiffness and compliance matrices at thecenters of stiffness and compliance.
The synthesis problem is addressed by, for example, Huang and Schimmels (1998a, 1998b, 1998c),see also (Huang 1998); Ciblak and Lipkin (1998), see also (Ciblak 1998); and Roberts (1999). Theseauthors examine in depth the synthesis of systems of ‘simple’ springs.
Network modeling of spatial, mechanical systems is investigated in (Bidard 1994; Maschke,Bidard, and van der Schaft 1994; Maschke 1996). Specific potential functions of compliant elementsare not given. Other works regarding the geometry of compliant elements are (Howard, Zefran, andKumar 1995; Zefran and Kumar 1997; Zefran and Kumar 1999), which look at how symmetry ofthe stiffness matrix is affected by the symmetry of the affine connection used to define it.
Fasse and colleagues have focused on the formulation of constitutive equations for computersimulation (Fasse and Breedveld 1998a; Fasse and Breedveld 1998b; Fasse and Zhang 1999; Fasse,Zhang, and Arabyan 2000). This work builds on previous work regarding artificial means of con-trolling mechanical impedance of robot manipulators (Fasse 1995; Fasse 1996; Fasse and Broenink1997; Fasse 1997; Fasse and Gosselin 1999; Stramigioli 1998).
Particularly relevant to the twist-based method presented in this work are references (Griffisand Duffy 1991; Fasse and Zhang 1999; Fasse, Zhang, and Arabyan 2000). The potential functionintroduced in Sec. 4 is identical to that presented in (Fasse and Zhang 1999; Fasse, Zhang, andArabyan 2000). The derivation of the constitutive equations and other analysis using dual numbermethods is new. Griffis and Duffy (1991) also define a potential function that is a quadratic functionof a twist displacement. Their methods shall be compared to those of Sec. 4 in the concludingdiscussion.
Zhang and Fasse (2000b) presented a geometric potential function method based on quaternioncalculus. The potential functions are similar to those presented by Caccavale et al. (1998, 1999), wholooked at active spatial impedance control of robots using potential function methods. The potentialfunction introduced in Sec. 5 is similar to but different than the potential functions previouslyintroduced. The primary benefit of the new potential functions is that their definitions do notdepend on which body or which coordinate frame is chosen as a reference.
There has been a lot of activity in compliant mechanism design and analysis in recent years.
1Henceforth ‘stiffness’ or ‘compliance’ typically refer to ‘stiffness and/or compliance’, except in technical definitionsfor which the distinction is important.
3
A detailed review of this literature is outside the scope of this work, but the reader is directedto such works as (Howell and Midha 1994; Howell and Midha 1995; Howell, Midha, and Norton1996; Frecker, Ananthasuresh, Nishiwaki, Kikuchi, and Kota 1997; Hetrick and Kota 1999; Jensen,Howell, and Salmon 1999). It is worth noting that not all flexural mechanisms can be thought ofas effectively rigid bodies connected by flexural joints. In some mechanisms elastic deformation isdistributed throughout the material body, not localized in discrete joints.
3 Background
3.1 Dual numbers
Dual numbers were introduced by Clifford (1873). They are useful in rigid body kinematic analysisand are used extensively in this work. For an in-depth introduction to dual numbers and theirapplication see (Dimentberg 1965; Selig 1996; Fischer 1999). Dual numbers are analogous to complexnumbers, having both primary and dual parts. The purely dual element ε is a nonzero element suchthat ε2 = 0. Dual numbers shall be denoted using the Fraktur font, for example:
a = α+ ε a (1)
In this case the primary part is pr(a) = α; the dual part is du(a) = a. Notation of the primary anddual parts shall be defined case by case.
Let b = β + ε b be another dual number. Addition and multiplication of dual numbers a and b
are defined in the obvious ways.
a + b = (α+ β) + ε (a+ b) (2)
ab = (αβ) + ε (αb+ βa) (3)
Because addition and multiplication are defined and because multiplication is distributive the dualnumbers define an algebraic ring. Division of a by b is defined if the primary part of b is nonzero.
a
b=α+ ε a
β + ε b
β − ε b
β − ε b=α
β+ ε
aβ − αb
β2(4)
Division by nonzero, purely dual numbers is not defined. For this reason the algebraic system ofdual numbers does not define an algebraic field, as does the system of complex numbers.
Analytic functions of dual numbers are defined entirely by the analytic functions of the primarypart. Suppose that φ(α) is a real, analytic function of real number α. The corresponding functionof dual number a is
f(a) = φ(α) + ε aφ′(α) (5)
where φ′ is the derivative of φ with respect to α. This is easily verified by looking at the Taylorexpansion of f(a). By remembering this rule the analytic functions of dual numbers can easily bederived as needed. For example,
sin(a) = sin(α) + ε a cos(α) (6)
cos(a) = cos(α)− ε a sin(α) (7)
exp(a) = exp(α) + ε a exp(α) (8)
ln(a) = ln(α) + ε a1
α(9)
4
3.2 Vectors
We distinguish between free, bound, and line vectors. A free vector is a vector with a well-definedmagnitude and direction. General free vectors are denoted v. We identify this vector with itscoordinates with respect to some coordinate frame, v = [v1; v2; v3]. Unit free vectors are denoted e.Associated with any free vector v is a cross-product matrix v.
v =
0 −v3 v2
v3 0 −v1
−v2 v1 0
(10)
This matrix is the unique matrix such that vw = v × w for any free vector w, where × denotes theEuclidean cross product.
A bound vector is a vector tangent to a point with a well-defined magnitude and direction.General bound vectors are denoted vp. Bound vector vp can be identified with point p and freevector v. Point p = [p1; p2; p3] is identified with its displacement coordinates with respect to somecoordinate frame. Bound vectors are not used in this work but are useful for understanding linevectors.
A line vector (sliding vector) is a vector with a well-defined axis, magnitude and direction. Aline vector can be considered mathematically to be an equivalence class of bound vectors. Boundvectors vp and wq are equivalent if w = v and q = p + dv for some real d. A line vector can beidentified with a dual vector. Let v define the magnitude and direction of a line vector. Let p beany point on the axis of the line vector. The dual vector associated with this line vector is
v = v + ε pv (11)
Together v and pv constitute the Plucker coordinates of the line vector. Because we identify linevectors with dual vectors, line vectors are denoted with the Fraktur font. The Plucker coordinatesof a line vector do not depend on the choice of p; they do depend on the choice of the referenceframe. Unit line vectors are denoted e = e+ ε pe
It is possible to define the cross-product matrix of a dual vector as well. Let m = µ + ε m bea dual vector (three-tuple of dual numbers). The associated cross-product matrix is m = µ + ε m,where the real cross-products µ and m are defined by (10).
3.3 Frames and rigid body displacements
The configuration of a rigid body can be identified with a coordinate frame attached to the body.Typically this frame is thought of as being associated with a point p and three orthonormal freevectors e1, e2 and e3. The frame can also be identified with three intersecting, orthonormal linevectors. Let e1 = e1 + ε pe1, e2 = e2 + ε pe2, and e3 = e3 + ε pe3 be such a set of unit line vectors.These three line vectors can then be identified with a transformation matrix
T =[
e1 e2 e3]
(12)
The primary and dual parts of this transformation matrix are given by
T = R+ ε pR (13)
where R =[
e1 e2 e3]
is an orthonormal rotation matrix. It is easily verified that transformationmatrices are “orthonormal” in the sense that T−1 = TT.
5
Similar notation is used for rigid body displacements. Let ‘a’ and ‘b’ be frame indices. Letmatrix Ta = Ra + ε paRa represent frame ‘a’. Let matrix Tb = Rb + ε pbRb represent frame ‘b’. Thedisplacement of frame ‘b’ from frame ‘a’ in the coordinates of frame ‘a’ is
Tab = TT
a Tb (14)
Given transformation matrix T = R+ ε pR let
AdT =
[
R 0pR R
]
(15)
be the associated adjoint matrix. The adjoint matrix relates twists, wrenches, and other entitiessuch as stiffness in different frames.
3.4 Finite twists
A rigid body displacement relating two frames can be associated with a twist, which is a rigid bodymotion that is constant in the coordinates of either frame. Let aa
b = αab + ε aa
b be a complex angle.The real part has units of angle; the dual part has units of distance. Let ua
b = υab + ε ua
b be a unitline vector. Let tab = aa
buab = τa
b + ε tab define the twist tab with complex angle aab and axis ua
b.Given twist tab, the associated transformation matrix is
Tab = exp
(
tab)
= I + sin(aab)u
ab + vers(aa
b) (uab)
2(16)
where I is the 3× 3 identity matrix and where vers(aab) = 1− cos(aa
b) is the versine of complex angleaab. Equation (16) is analogous to Rodrigues’ formula for rotations.
Computation of the primary and dual parts yields
Rab = exp (τa
b ) = I + sin(αab)υ
ab + vers(αa
b) (υab)
2(17)
pab = Sa
btab (18)
where
Sab = I +
vers(α)
αυa
b +
(
1−sin(α)
α
)
(υab)
2(19)
This explicit computation of the primary and dual parts is not necessary, but a dual matrix Sab
similar to Sab turns out to be useful in the sequel.
More of interest is the computation of twist tab given transformation Tab. This can be computed
using the following algorithm. Let
cab =1
2(tr (Ta
b)− 1) (20)
which is the cosine of aab. If pr (c
ab) = 1, then
tab = ε du (Tab) (21)
If −1 < pr (cab) < 1 then
aab = cos−1 (cab) (22)
uab =
1
sin (aab)
as (Tab) (23)
tab = aabua
b (24)
6
where as (Tab) = 0.5
(
Tab − (Ta
b)T)
denotes the antisymmetric (skew-symmetric) part of Tab.
2
3.5 Dual quaternions
Having defined the twist it is straightforward to define dual quaternions. A dual quaternion (Clif-
ford’s biquaternion) is a pair q = (s, x). Scalar dual number s = σ + ε s is the scalar part of q. Thethree-tuple of dual numbers x = ξ + ε x is the vector part of q.
Given transformation Tab one can define an associated complex angle aa
b and axis uab, as described
in the previous section. An associated unit modulus dual quaternion qab can be defined as follows:
sab = cos
(
aab
2
)
(25)
xab = sin
(
aab
2
)
uab (26)
This dual quaternion is not unique. If qab represents Ta
b then −qab also represents Ta
b.For developing intuition it is useful to look at the scalar and vector parts in more detail. The
scalar part is
sab = cos
(
αab
2
)
− ε sin
(
αab
2
)
aab
2(27)
For small aab this is approximately
sab ≈ 1 (28)
The more interesting vector part is
xab =
(
sin
(
αab
2
)
+ ε cos
(
αab
2
)
aab
2
)
uab (29)
For small aab this is approximately
xab ≈
(
αab
2+ ε
aab
2
)
uab (30)
This is a screw motion about axis uab of angle αa
b/2 about the axis and displacement aab/2 along the
axis. Again, axis uab is a unit line vector.
Given dual quaternion qab, computation of the associated transformation Ta
b is straightforward.
Tab = I + 2sa
bxab + 2 (xab)2
(31)
This is completely analogous to the computation of a purely prime orthonormal matrix given a setof Euler parameters (Nikravesh 1988).
Computation of qab given Ta
b is more involved. Here is an algorithm suitable for computation.First consider the primary part (σa
b, ξab). Let
Rab = pr(Ta
b) (32)
pab = du(Ta
b)(Rab)
T (33)
2It should be emphasized that these equations can actually be used for computation given a suitable computingenvironment. In many programming languages it is possible to define dual number classes (e.g., C++) or modules(e.g., Fortran 90) and overload standard operators (addition, multiplication, division, etc.) and functions (sine, cosine,arc-cosine, etc.). It is often unnecessary to explicitly compute the primary and dual parts of assignments. Fischer(1999) gives a dual number class duplex for C++. The equations described in this work were implemented in C++,Fortran 90, Matlab, and Scilab.
7
From this the scalar part σab can be computed
σab =
1
2
√
tr(Rab) + 1 (34)
We are free, for example, to choose the non-negative root, in which case 0 ≤ σab ≤ 1.
If σab > 0, then
ξab =as(Ra
b)
2σab
(35)
This is not well behaved numerically for small σab (large rotations) and is used only for σa
b > 0.1 inthe software implementation. Otherwise let
ξab =
√
1
2sy(Ra
b) +1
4(1− tr(Ra
b))I (36)
where the square root is the matrix square root. Term sy(Rab) = 0.5
(
Rab + (Ra
b)T)
denotes the
symmetric part of Rab. Details of computation of this square root are given in Appendices C and
D. Expression (36) is always well defined, but is not well behaved numerically for large σab (small
rotations). The expression is used only for σab ≤ 0.1 in the software implementation.
Computation of the dual part (sab, xab) is then straightforward.
sab = −1
2(ξab)
Tpab (37)
xab =
1
2(σa
bI− ξab)pab (38)
4 Twist-based method
4.1 Definition of potential function
Let ‘A’ and ‘B’ index two elastically coupled rigid bodies, as depicted in Fig. 1 Let ‘a’ index aframe attached to body ‘A’. Let ‘b’ index a frame attached to body ‘B’. Frames ‘a’ and ‘b’ areassumed to coincide in unstressed equilibrium. Let Ta represent the configuration of frame ‘a’. LetTb represent the configuration of frame ‘b’. Transformation Ta
b = TTa Tb then represents the relative
displacements of the two frames, and thus the state of elastic deformation. Using the algorithm ofSec. 3.4 it is possible to compute the (finite) twist displacement tab = τa
b + ε tab. Primary part τab has
units of angle; dual part tab has units of length.The potential energy is defined to be
U1K(Ta
b) =1
2
[
(τab )
T(tab)
T]
K
[
τab
tab
]
(39)
where K is the 6× 6 symmetric stiffness matrix
K =
[
Ko Kc
KTc Kt
]
(40)
Matrix Ko is the 3×3 symmetric rotational stiffness matrix. Matrix Kc is the 3×3 coupling stiffnessmatrix. Matrix Kt is the 3× 3 symmetric translational stiffness matrix.
8
Potential function (39) is identical to that introduced in (Fasse and Zhang 1999; Fasse, Zhang, andArabyan 2000). The following derivation of the constitutive equations using dual number methodsis new.
If tr(Kt) is not an eigenvalue of Kt, then there exist unique points on the bodies pa and pb
(coincident in equilibrium) at which the coupling stiffness matrix Kc is symmetric (Loncaric 1987;Brockett and Loncaric 1986). It is not assumed that frames ‘a’ and ‘b’ are at the center of stiffness(that their unit line vectors go through the center of stiffness). Nonetheless it is strongly recom-mended that the center of stiffness be chosen as a reference. First, it is a unique, unambiguouslydefined point for most systems. Second, as shown by Ciblak (1998), any compliant axis decouplingtranslation and rotation must intersect the center of stiffness. It must also intersect the centersof compliance and elasticity. If two compliant axes exist then the three centers coincide. Manymanufactured compliant joints have compliant axes by design. Thus for these systems the center ofstiffness has an intuitive, physical significance.
4.2 Kinematics of a virtual displacement
Consider an infinitesimal, virtual displacement of frame ‘b’ on body ‘B’ from an actual configurationTb to a virtual configuration Tb′ . This displacement can be described by an infinitesimal twistδtbb′ = δτb
b′ + ε δtbb′ . The corresponding change of finite displacement Tab is
δTab = Ta
bδtbb′ (41)
The corresponding change of finite twist tab is
δtab = (Sab)−T
δtbb′ (42)
where if pr(aab) = 0 then
(Sab)−1
= I−aab
2uab = I−
1
2tab (43)
Otherwise if pr(aab) 6= 0 then
(Sab)−1
= I−aab
2uab +
(
1−aab (1 + cos(aa
b))
2 sin(aab)
)
(uab)
2(44)
In terms of primary and dual parts (42) is equivalent to[
δτab
δtab
]
= (Γab)
T
[
δτbb′
δtbb′
]
(45)
where
Γab =
pr[
(Sab)−1]
du[
(Sab)−1]
0 pr[
(Sab)−1]
(46)
4.3 Constitutive equations
The infinitesimal, virtual work done on the elastic body given virtual displacement δtbb′ is
δW =U1K (tab + δtab)− U1
K (tab)
=[
(τab )
T (tab)T]
K
[
δτab
δtab
]
=[
(τab )
T (tab)T]
K (Γab)
T
[
δτbb′
δtbb′
]
(47)
9
Let mbe = µb
e + εmbe be the wrench exerted by body ‘B’ on the elastic body expressed as a motor
with respect to ‘b’ in coordinates of ‘b’. (In other words, the origin of frame ‘b’ is the reference forthe moment part, and both the moment and vector parts are computed in the coordinates of frame‘b’.) Primary part µb
e is the force exerted by body ‘B’ on the elastic body in coordinates of ‘b’. Dualpart mb
e is the moment exerted by body ‘B’ on the elastic body. This moment is with respect to theorigin of frame ‘b’ in coordinates of frame ‘b’. The infinitesimal, virtual work done on the elasticbody given virtual displacement δtbb′ is
δW =[
(mbe )
T (µbe )
T]
[
δτbb′
δtbb′
]
(48)
The virtual works defined by (47) and (48) must be equal for arbitrary displacements δtbb′ . Weconclude that
[
mbe
µbe
]
= ΓabK
[
τab
tab
]
(49)
A similar expression holds for body ‘A’, exchanging indices ‘a’ and ‘b’.Let me = µe + ε me be the wrench exerted by body ‘B’ on the elastic body expressed as a motor
in the distinguished inertial reference frame. Primary part µe is the force exerted by body ‘B’ onthe elastic body in inertial coordinates. Dual part me is the moment exerted by body ‘B’ on theelastic body. This moment is with respect to the origin of the inertial frame in coordinates of theinertial frame.
Wrench me is then determined by[
me
µe
]
= AdTTT
b
[
mbe
µbe
]
=
[
Rb pbRb
0 Rb
] [
mbe
µbe
] (50)
The wrench exerted by body ‘A’ on the elastic body is equal and opposite. For an elastic elementwith stiffnessK, wrench me is computable given configurations Ta and Tb using equations (14), (20)–(24), (43), (44), (46), (49), and (50). This assumes a suitable dual-number computing environment.Essential Fortran 90 and Matlab code corresponding to these equations is given in Appendices Cand D. Similar code has been developed in C++ and Scilab, but this code is not well tested.
4.4 Small displacement analysis
For small, real relative displacements of the bodies from equilibrium the configuration-wrench mapcan be approximated by a linear relation. Assume that Ta
b ≈ I + δtab, where δtab is a small, finite,
real twist displacement. For small displacements the following approximations are true: tab ≈ δtaband (Sa
b)−1≈ I− 1
2δtab. From (46) and (49) it follows that
[
mbe
µbe
]
≈
[
pr(
I− 12δtab)
du(
I− 12δtab)
0 pr(
I− 12δtab)
]
K
[
12δτa
b12δtab
]
(51)
Neglecting the second-order terms yields[
mbe
µbe
]
≈ K
[
δτab
δtab
]
(52)
This shows that matrix K does indeed determine the stiffness of the system at Tab = I. Frames ‘a’
and ‘b’ can be identified for small displacements, so similar expressions hold in the coordinates of‘a’.
10
4.5 Body and frame independence
In this section it is shown that the elastic potential function depends neither on which body is chosenas a reference, nor on which coordinate frames are chosen as references. First consider the choiceof body. Suppose that we exchange the roles of bodies ‘A’ and ‘B’ in the definition of the energyfunction:
U1K(Tb
a) =1
2
[
(
τba
)T (
tba)T]
K
[
τba
tba
]
=1
2
[
(τab )
T(tab)
T]
K
[
τab
tab
] (53)
The second equality holds because tba = −tab. This shows that U1K(Tb
a) = U1K(Ta
b), which proves thatthe definition of the energy function does not depend on which body is chosen as a reference.
Next consider the choice of frames. Let frames ‘a′’ and ‘b′’ be another pair of frames attachedto bodies ‘A’ and ‘B’ that coincide in unstressed equilibrium. It is well known that stiffnesses ofdifferent frames are related by adjoint matrices. In particular, the relationship between stiffnessesK ′ and K is
K ′ = AdTTa
a′K AdTa
a′= AdT
Tb
b′
K AdTb
b′(54)
This is a congruence transformation. The latter equality follows from the fact that Taa′ = Tb
b′ . Let
ta′
b′ be the twist associated with transformation matrix Ta′
b′ . The relationship between ta′
b′ and tab is
ta′
b′ = Ta′
a tab (55)
This is easily proven by looking at the Taylor expansion of (16). In terms of primary and dual partsthis is equivalent to
[
τa′
b′
ta′
b′
]
= AdTa′
a
[
τab
tab
]
(56)
From (54) and (56) it follows that
U1K′(Ta′
b′) =1
2
[
(
τa′
b′
)T (
ta′
b′
)T]
K ′[
τa′
b′
ta′
b′
]
=1
2
[
(τab )
T(tab)
T]
AdTTa′
a
AdTTa
a′K AdTa
a′AdTa′
a
[
τab
tab
]
=1
2
[
(τab )
T(tab)
T]
K
[
τab
tab
]
(57)
This shows that U1K′(Ta′
b′) = U1K(Ta
b), which proves that the definition of the energy function doesnot depend on which frames ‘a’ and ‘b’ are chosen as references.
4.6 Definition of viscous behavior
A viscous behavior can be defined that is compatible with the previously defined elastic behavior.Let the twist rate be tab = τa
b + ε tab. Let ωbB and vb
B be the angular and linear velocity of body ‘B’expressed as a motor with respect to frame ‘b’. Let ωa
A and vaA be the angular and linear velocity of
body ‘A’ expressed as a motor with respect to frame ‘a’. The twist rate is[
τab
tab
]
= (Γab)
T
[
ωbB
vbB
]
−(
Γba
)T[
ωaA
vaA
]
(58)
11
where Γab is defined in (46) and
Γba =
pr[
(
Sba
)−1]
du[
(
Sba
)−1]
0 pr[
(
Sba
)−1]
(59)
Matrix (Sba)−1 is easily computed from (Sa
b)−1.
(Sba)−1 = (Sa
b)−T (60)
The viscous damping behavior can be defined succinctly and unambiguously in terms of virtualwork
δW =[
(τab )
T (
tab)T]
BT
[
δτab
δtab
]
(61)
This is the virtual work done by bodies ‘A’ and ‘B’ on the viscous element given a virtual displacementδtab.
4.7 Constitutive equations
Let δtbb′ be a virtual displacement of body ‘B’. The corresponding virtual twist displacement is
[
δτab
δtab
]
= (Γab)
T
[
δτbb′
δtbb′
]
(62)
The virtual work done on the viscous body is thus
δW =[
(τab )
T (
tab)T]
BT (Γab)
T
[
δτbb′
δtbb′
]
(63)
Let mbv = µb
v + εmbv be the wrench exerted by body ‘B’ on the viscous body expressed as a motor
with respect to ‘b’ in coordinates of ‘b’. The infinitesimal, virtual work done on the viscous bodygiven virtual displacement δtbb′ is
δW =[
(mbv)
T (µbv)
T]
[
δτbb′
δtbb′
]
(64)
The virtual works defined by (63) and (64) must be equal for arbitrary displacements δtbb′ . Weconclude that
[
mbv
µbv
]
= ΓabB
[
τab
tab
]
(65)
A similar expression holds for body ‘A’, exchanging indices ‘a’ and ‘b’.Let mv = µv + εmv be the wrench exerted by body ‘B’ on the viscous body expressed as a motor
in the distinguished inertial reference frame. Wrench mv is then[
mv
µv
]
=
[
Rb pbRb
0 Rb
] [
mbv
µbv
]
(66)
The wrench exerted by body ‘A’ on the viscous body is equal and opposite. For a viscous elementwith damping matrix B, wrench mv is computable given Ta, Tb, ω
aA, v
aA, ω
bB and vb
B using equations(14), (20)–(24), (44), (46), (60), (59), (58), (65), and (66). Essential code corresponding to theseequations is given in Appendices C and D.
12
It can be shown that the behavior defined by (61) does not depend on which body is chosenas a reference or on which frames are chosen as references. It can be shown that for the nominalconfiguration Ta
b = I,[
mbv
µbv
]
= B
{[
ωbB
vbB
]
−
[
ωbA
vbA
]}
(67)
Finally it can be shown that the real work done on the viscous element is non-negative if thesymmetric part of B has non-negative eigenvalues.
4.8 Large displacements
The quadratic potential energy function (39) is defined for large displacements, but only accuratefor small displacements. For greater accuracy given large displacements, higher-order polynomialscould be used. Let tab = τa
b + ε tab be the twist coordinates of the displacement. To simplify notationlet τa
b = [τ1; τ2; τ3] and let tab = [t1; t2; t3]. Let N be the order of the potential energy polynomial.The potential energy can be written
U =
0≤i+...+n≤N∑
i,j,k,l,m,n
ci,j,k,l,m,n τi1τj2 τ
k3 tl1tm2 t
n3 (68)
This elastic potential function depends neither on which body is chosen as a reference, nor onwhich coordinate frames are chosen as references. This is because twists expressed in differentcoordinate frames are related by linear transformations, see (55). Thus given any second pair offrames ‘a′’ and ‘b′’ there exists another set of coefficients c′i,j,k,l,m,n such that
U =
0≤i+...+n≤N∑
i,j,k,l,m,n
c′i,j,k,l,m,n (τ ′1)i(τ ′2)
j(τ ′3)k(t′1)
l(t′2)m(t′3)
n(69)
Thus an N -th order polynomial of one set of twist coordinates is an N -th order polynomial of anyset of twist coordinates. Because of this, the set of polynomials of twists can be considered anintrinsically defined basis of analytic functions of rigid body displacements. So although (68) mayappear to be mindless curve-fitting, there are sound geometric reasons for approximating potentialenergy functions as polynomials of twist coordinates.
Next consider the computation of the associated generalized forces. Let ζi be the generalizedforce corresponding to τi. Let zi be the generalized force corresponding to ti. The generalized forcescorresponding to (68) are
ζ1 =
0≤i+...+n≤N∑
i,j,k,l,m,n
ci,j,k,l,m,n i τi−11 τ j2 τ
k3 tl1tm2 t
n3 (70)
ζ2 =
0≤i+...+n≤N∑
i,j,k,l,m,n
ci,j,k,l,m,n j τi1τj−12 τk3 t
l1tm2 t
n3 (71)
ζ3 =
0≤i+...+n≤N∑
i,j,k,l,m,n
ci,j,k,l,m,n k τi1τj2 τ
k−13 tl1t
m2 t
n3 (72)
13
z1 =
0≤i+...+n≤N∑
i,j,k,l,m,n
ci,j,k,l,m,n l τi1τj2 τ
k3 tl−11 tm2 t
n3 (73)
z2 =
0≤i+...+n≤N∑
i,j,k,l,m,n
ci,j,k,l,m,n m τ i1τj2 τ
k3 tl1tm−12 tn3 (74)
z3 =
0≤i+...+n≤N∑
i,j,k,l,m,n
ci,j,k,l,m,n n τi1τj2 τ
k3 tl1tm2 t
n−13 (75)
Coefficient c0,0,0,0,0,0 is arbitrary and can be set to zero. Coefficients c1,0,0,0,0,0, c0,1,0,0,0,0,c0,0,1,0,0,0, c0,0,0,1,0,0, c0,0,0,0,1,0 and c0,0,0,0,0,1 must be zero so that the equilibrium is unstressed.
Finally consider computation of the associated elastic wrench in body coordinates. This is donesimilarly to (49). Let ζ = [ζ1; ζ2; ζ3] and z = [z1; z2; z3]. The associated elastic wrench is
[
mbe
µbe
]
= Γab
[
ζz
]
(76)
This shows that the methods could be extended to be more accurate given large displacements.Practical estimation of model coefficients and efficient computation of wrenches is nontrivial. Briefly,the following experimental procedure is envisioned: One body ‘A’ would be kept stationary whilethe other body ‘B’ were displaced by some mechanism. For each trial the configuration Tb wouldbe measured, as would be the wrench acting by the mechanism on the body, mb
e . Knowing thestationary configuration Ta it would be possible to compute the displacement Ta
b. It would then bepossible to compute the generalized forces
[
ζz
]
= (Γab)−1
[
mbe
µbe
]
(77)
where
(Γab)−1
=
[
pr(Sab) du(Sa
b)0 pr(Sa
b)
]
(78)
and
Sab = I +
vers(a)
auab +
(
1−sin(a)
a
)
(uab)
2(79)
The resultant τab , t
ab, ζ and z could be substituted into (70)–(75), yielding six equations that were
linear in the coefficients ci,j,k,l,m,n. If n trials were performed, 6n such equations would be generated.This system of linear equations could then be solved exactly or in a least-squares sense for thecoefficients ci,j,k,l,m,n. A similar procedure could be used to estimate model coefficients using finiteelement methods.
5 Dual quaternion method
The development of the dual quaternion-based method is very similar to the development of thetwist-based method. The organization of this section is thus identical to that of Sec. 4.
14
5.1 Definition of potential function
Again let Ta and Tb represent the configurations of frames ‘a’ and ‘b’ attached to bodies ‘A’ and‘B’. Transformation Ta
b = TTa Tb then represents the relative displacements of the two frames. Using
the algorithm of Sec. 3.5 it is possible to compute the dual quaternion displacement qab = (sa
b, xab).
The vector part of the displacement is xab = ξab + ε xab. Primary part ξa
b is dimensionless; dual partxa
b has units of length.The potential energy is defined to be
U2K(Ta
b) = 2[
(ξab)T
(xab)
T]
K
[
ξabxa
b
]
(80)
where again K is the 6× 6 symmetric stiffness matrix
K =
[
Ko Kc
KTc Kt
]
(81)
Note that this is an even function of qab. This is important, because if qa
b represents Tab, then −qa
b
also represents Tab.
This potential function is new, although similar to those of (Zhang and Fasse 2000b; Zhang1999), which are derived from those of (Caccavale, Siciliano, and Villani 1998; Caccavale, Natale,Siciliano, and Villani 1999), which are related to those of (Fasse and Broenink 1997; Fasse 1997;Fasse and Breedveld 1998a). Hollis, Salcudean, and Allan (1991) were perhaps the first to applyregular unit quaternions (Euler parameters) to modeling compliant behavior.3
5.2 Kinematics of a virtual displacement
Again consider an infinitesimal, virtual displacement of frame ‘b’ on body ‘B’ from an actual con-figuration Tb to a virtual configuration Tb′ . This displacement can be described by an infinitesimaltwist δtbb′ = δτb
b′+εδtbb′ . The change of finite displacement Ta
b is given by (41), and the correspondingchange of dual quaternion qa
b is
δqab =
1
2(La
b)Tδtbb′ (82)
whereLa
b =[
−xab sabI− xab
]
(83)
An array form for qab is assumed in (82), namely that qa
b = [sab; x
ab].
Expanding the primary and dual parts of the vector part δxab = 12
[
sabI + xab
]
δtbb′ yields
[
δξabδxa
b
]
=1
2(Λa
b)T
[
δτbb′
δtbb′
]
(84)
where
Λab =
[
pr (sabI− xab) du (sa
bI− xab)0 pr (sa
bI− xab)
]
(85)
3In this method displacement is represented by the algebraic difference of quaternions instead of a relative quater-nion computed using the natural quaternion product. As a result the desired first-order behavior (stiffness) is achievedonly approximately.
15
5.3 Constitutive equations
The infinitesimal, virtual work done on the elastic body given virtual displacement δtbb′ is
δW =4[
(ξab)T (xa
b)T]
K
[
δξabδxa
b
]
=2[
(ξab)T (xa
b)T]
K (Λab)
T
[
δτbb′
δtbb′
] (86)
Again let mbe = µb
e + ε mbe be the wrench exerted by body ‘B’ on the elastic body expressed as
a motor at ‘b’ in coordinates of ‘b’. The infinitesimal, virtual work done on the elastic body givenvirtual displacement δtbb′ is given by (48). The virtual works defined by (86) and (48) must be equalfor arbitrary displacements δtbb′ . We conclude that
[
mbe
µbe
]
= 2ΛabK
[
ξabxa
b
]
(87)
A similar expression holds for body ‘A’, exchanging indices ‘a’ and ‘b’. Note that this is an evenfunction of qa
b.The wrench exerted by body ‘B’ on the elastic body expressed as a motor in the inertial reference
frame is[
me
µe
]
= AdTTT
b
[
mbe
µbe
]
=
[
Rb pbRb
0 Rb
] [
mbe
µbe
] (88)
The wrench exerted by body ‘A’ on the elastic body is equal and opposite. For an elastic elementwith stiffnessK, wrench me is computable given configurations Ta and Tb using equations (14), (32)–(38), (85), (87), and (88). Essential code corresponding to these equations is given in Appendices Cand D.
5.4 Small displacement analysis
Again assume that Tab ≈ I + δtab, where δt
ab is a small, finite, real twist displacement. For small
displacements the following approximations are true: tab ≈ δtab, sab ≈ 1, and xab ≈
12δtab. From (85)
and (87) it follows that[
mbe
µbe
]
≈ 2
[
I− 12δtab − 1
2δtab
0 I− 12δtab
]
K
[
12δτa
b12δtab
]
(89)
Neglecting the second-order terms yields
[
mbe
µbe
]
≈ K
[
δτab
δtab
]
(90)
This shows that matrix K does indeed determine the stiffness of the system at Tab = I. Frames ‘a’
and ‘b’ can be identified for small displacements, so similar expressions hold in the coordinates of‘a’.
16
5.5 Body and frame independence
Again the elastic potential function depends neither on which body is chosen as a reference, nor onwhich coordinate frames are chosen as references. First consider the choice of body. Suppose thatwe exchange the roles of bodies ‘A’ and ‘B’ in the definition of the energy function:
U2K(Tb
a) = 2[
(
ξba)T (
xba
)T]
K
[
ξbaxb
a
]
= 2[
(ξab)T
(xab)
T]
K
[
ξabxa
b
] (91)
where the second equality holds because if qba = (sb
a , xba) represents Tb
a , then qab = (sa
b, xab) = (sb
a ,−xba)represents Ta
b. This shows that U2K(Tb
a) = U2K(Ta
b), which proves that the definition of the energyfunction does not depend on which body is chosen as a reference.
Next consider the choice of frames. Again let frames ‘a′’ and ‘b′’ be another pair of framesattached to bodies ‘A’ and ‘B’ that coincide in unstressed equilibrium. The relationship betweenstiffnesses K ′ and K is again given by (54).
Let qa′
b′ = (sa′
b′ , xa′
b′) be the dual quaternion associated with transformation matrix Ta′
b′ . The
relationship between qa′
b′ and qab is
sa′
b′ = sab (92)
xa′
b′ = Ta′
a xab (93)
In terms of primary and dual parts, (93) is equivalent to
[
ξa′
b′
xa′
b′
]
= AdTa′
a
[
ξabxa
b
]
(94)
From (54) and (94) it follows that
U2K′(Ta′
b′) =2
[
(
ξa′
b′
)T (
xa′
b′
)T]
K ′[
ξa′
b′
xa′
b′
]
=2[
(ξab)T
(xab)
T]
AdTTa′
a
AdTTa
a′K AdTa
a′AdTa′
a
[
ξabxa
b
]
=2[
(ξab)T
(xab)
T]
K
[
ξabxa
b
]
(95)
This shows that U2K′(Ta′
b′) = U2K(Ta
b), which proves that the definition of the energy function doesnot depend on which frames ‘a’ and ‘b’ are chosen as references.
5.6 Definition of viscous behavior
A viscous behavior can be defined that is compatible with the previously defined elastic behavior.Let the dual quaternion rate be qa
b = (sab, x
ab). Let ωb
B and vbB be the angular and linear velocity of
body ‘B’ expressed as a motor with respect to frame ‘b’. Let ωaA and va
A be the angular and linearvelocity of body ‘A’ expressed as a motor with respect to frame ‘a’. The rate of change of the vectorpart is
[
ξabxa
b
]
=1
2(Λa
b)T
[
ωbB
vbB
]
−1
2
(
Λba
)T[
ωaA
vaA
]
(96)
17
where Λab is defined in (85) and
Λba =
[
pr(
sbaI− xba
)
du(
sbaI− xba
)
0 pr(
sbaI− xba
)
]
=
[
pr (sabI + xab) du (sa
bI + xab)0 pr (sa
bI + xab)
] (97)
Equations (96) and (97) assume the choice that sba = sa
b and xba = −xab.The viscous damping behavior can be defined succinctly and unambiguously in terms of virtual
work
δW = 4
[
(
ξab
)T
(xab)
T
]
BT
[
δξabδxa
b
]
(98)
This is the virtual work done by bodies ‘A’ and ‘B’ on the viscous element given virtual displacementδxab.
5.7 Constitutive equations
Let δtbb′ be a virtual displacement of body ‘B’. The corresponding virtual change of the vector partof the dual quaternion is
[
δξabδxa
b
]
=1
2(Λa
b)T
[
δτbb′
δtbb′
]
(99)
The virtual work done on the viscous body is thus
δW = 2
[
(
ξab
)T
(xab)
T
]
BT (Λab)
T
[
δτbb′
δtbb′
]
(100)
Let mbv = µb
v + εmbv be the wrench exerted by body ‘B’ on the viscous body expressed as a motor
with respect to ‘b’ in coordinates of ‘b’. The infinitesimal, virtual work done on the viscous bodygiven virtual displacement δtbb′ is
δW =[
(mbv)
T (µbv)
T]
[
δτbb′
δtbb′
]
(101)
The virtual works defined by (100) and (101) must be equal for arbitrary displacements δtbb′ . Weconclude that
[
mbv
µbv
]
= 2ΛabB
[
ξabxa
b
]
(102)
A similar expression holds for body ‘A’, exchanging indices ‘a’ and ‘b’. Note that together (96) and(102) define an even function of qa
b.Let mv = µv + εmv be the wrench exerted by body ‘B’ on the viscous body expressed as a motor
in the distinguished inertial reference frame. Wrench mv is then
[
mv
µv
]
=
[
Rb pbRb
0 Rb
] [
mbv
µbv
]
(103)
The wrench exerted by body ‘A’ on the viscous body is equal and opposite. For a viscous element withdamping matrix B, wrench mv is computable given Ta, Tb, ω
aA, v
aA, ω
bB and vb
B using equations (14),(32)–(38), (85), (97), (96), (102), and (103). Essential Fortran 90 and Matlab code corresponding
18
to these equations is given in Appendices C and D. Again, similar code has been developed in C++and Scilab, but this code is not well tested.
It can be shown that the behavior defined by (98) does not depend on which body is chosenas a reference or on which frames are chosen as references. It can be shown that for the nominalconfiguration Ta
b = I,[
mbv
µbv
]
= B
{[
ωbB
vbB
]
−
[
ωbA
vbA
]}
(104)
Finally it can be shown that the real work done on the viscous element is non-negative if thesymmetric part of B has non-negative eigenvalues.
5.8 Large displacements
The quadratic potential energy function (80) is defined for large displacements, but only accuratefor small displacements. For greater accuracy given large displacements, higher-order polynomialscould be used. Let qa
b = (sab, x
ab) be the dual quaternion of the displacement. To simplify notation
let sab = σ + ε s and let xab = ξ + ε x = [ξ1; ξ2; ξ3] + ε [x1;x2;x3]. Let N be the order of the potential
energy polynomial. The potential energy can be written
U =
0≤i+...+p≤N∑
i,...,p
ci,...,p σiξj1ξ
k2 ξl3smxn1x
o2xp3 (105)
This elastic potential function depends neither on which body is chosen as a reference, nor onwhich coordinate frames are chosen as references. This is because dual quaternions expressed indifferent coordinate frames are related by linear transformations, see (92) and (93). Thus given anysecond pair of frames ‘a′’ and ‘b′’ there exists another set of coefficients c′i,...,p such that
U =
0≤i+...+p≤N∑
i,...,p
c′i,...,p (σ′)i(ξ′1)
j(ξ′2)k(ξ′3)
l(s′)m(x′1)n(x′2)
o(x′3)p (106)
Thus an N -th order polynomial of one set of dual quaternions is an N -th order polynomial of any setof dual quaternions. Because of this, the set of polynomials of dual quaternions can be consideredan intrinsically defined basis of analytic functions of rigid body displacements. So there are soundgeometric reasons for approximating potential energy functions as polynomials of dual quaternions.The potential energy should be an even function of the dual quaternions, thus we expect all of theodd-power coefficients to be zero, ci,...,p = 0 if i+ . . .+ p is odd.
Next consider the computation of the associated generalized forces. Let Σ be the generalizedforce corresponding to σ. Let S be the generalized force corresponding to s. Ξi be the generalizedforce corresponding to ξi. Let Xi be the generalized force corresponding to xi. The generalizedforces corresponding to (105) are
Σ =
0≤i+...+p≤N∑
i,...,p
ci,...,p i σi−1ξj1ξ
k2 ξl3smxn1x
o2xp3 (107)
Ξ1 =
0≤i+...+p≤N∑
i,...,p
ci,...,p j σiξj−1
1 ξk2 ξl3smxn1x
o2xp3 (108)
Ξ2 =
0≤i+...+p≤N∑
i,...,p
ci,...,p k σiξj1ξ
k−12 ξl3s
mxn1xo2xp3 (109)
19
Ξ3 =
0≤i+...+p≤N∑
i,...,p
ci,...,p l σiξj1ξ
k2 ξl−13 smxn1x
o2xp3 (110)
S =
0≤i+...+p≤N∑
i,...,p
ci,...,p m σiξj1ξk2 ξl3sm−1xn1x
o2xp3 (111)
X1 =
0≤i+...+p≤N∑
i,...,p
ci,...,p n σiξj1ξ
k2 ξl3smxn−1
1 xo2xp3 (112)
X2 =
0≤i+...+p≤N∑
i,...,p
ci,...,p o σiξj1ξ
k2 ξl3smxn1x
o−12 xp3 (113)
X3 =
0≤i+...+p≤N∑
i,...,p
ci,...,p p σiξj1ξ
k2 ξl3smxn1x
o2xp−13 (114)
Coefficient c0,...,0 is arbitrary and can be set to zero. Coefficients c1,0,0,...,0, c0,1,0,...,0, etc., mustbe zero so that the equilibrium is unstressed.
Finally consider computation of the associated elastic wrench in body coordinates. This is donesimilarly to (87). Let Ξ = [Ξ1; Ξ2; Ξ3] and X = [X1;X2;X3]. The associated elastic wrench is
[
mbe
µbe
]
=1
2Λa
b
ΣΞSX
(115)
where Λab is redefined to be
Λab =
[[
−ξab σabI− ξab
] [
−xab sabI− xa
b
]
0[
−ξab σabI− ξab
]
]
(116)
This shows that the methods could be extended to be more accurate given large displacements.Practical estimation of model coefficients and efficient computation of wrenches is nontrivial. Briefly,the following experimental procedure is envisioned: One body ‘A’ would be kept stationary whilethe other body ‘B’ were displaced by some mechanism. For each trial the configuration Tb wouldbe measured, as would be the wrench acting by the mechanism on the body, mb
e . Knowing thestationary configuration Ta it would be possible to compute the displacement Ta
b. It would then bepossible to compute the generalized forces
ΣΞSX
= 2
[
−(ξab)T
σabI + ξab
] [
−(xab)
T
sabI + xab
]
0
[
−(ξab)T
σabI + ξab
]
[
mbe
µbe
]
(117)
The resultant σ, ξ, s, x, Σ, Ξ, S andX could be substituted into (107)–(114), yielding eight equationsthat were linear in the coefficients ci,...,p. If n trials were performed, 8n such equations would begenerated. This system of linear equations could then be solved exactly or in a least-squares sensefor the coefficients ci,...,p. A similar procedure could be used to estimate model coefficients usingfinite element methods.4
4The map from wrenches to generalized forces defined by (117) is not unique. One is free to add workless generalized
20
(a) (b)
Figure 3: Mock reconstruction of ARCADE (Aristoteles Calibration Device).
6 Flexural mechanism example
This section demonstrates the potential utility of the methods by simulating the behavior of acomplex flexural mechanism. The mechanism is similar to the ARCADE (Aristoteles CalibrationDevice) (Koster 1998). This complex spatial, flexural mechanism incorporates 35 effectively rigidbodies including the base, 36 notch hinges, and 12 cantilever flexures. The simulation results aretoo preliminary to validate the methods. In any case they are interesting and illustrate the intendedapplication of the methods.
The ARCADE device was designed as a linear actuator for calibrating accelerometers on theEuropean Space Agency (ESA) satellite ‘Aristoteles’. Details of actuation are not given by Koster,nor are they important for this discussion. The function of the device is for the central cylinder tooscillate back and forth with respect to the base.
The mock device shown in Fig. 3 has five main components. Two quasi-triangular end platesconstitute much of the frame. The central cylinder is mostly hollow and has massive end platesattached to it. The bulk of the mechanism is machined from three identical linkage plates. Eachlinkage plate can be thought of as a collection of effectively rigid bodies connected by flexural joints.Each end of these plates is connected to one of the quase-triangular end plates. The innermost,central body of each linkage plate is connected to the central cylinder.
Based on Koster’s description of the design principles of ARCADE the author believes that twogroups of bodies on the linkage plates are meant to be rigidly attached. Specifically, on each linkageplate there is a body that is near both the central cylinder and one of the end plates. Each groupof three bodies near an end plate should be rigidly attached. The means by which this is achievedis not clear from the assembly drawing given by Koster, so this was not done in the reconstruction.For this reason, the three bodies near each end plate are treated as independent in the simulation.
Most (33) of the links are modeled as being prismatic blocks with the density of aluminum.Dimensions are those of the mock device. Typical link lengths are 2–3 cm. Typical link widths are0.5 cm. All links have the same thickness of 0.95 cm, which is the linkage plate thickness. Most (30)of these 33 links are connected to other links via two compliant joints. The remaining three of the33 links are connected to other links via 6 compliant joints.
The inner cylinder is modeled as a uniform cylinder with a density 10% that of aluminum. (This
constraint forces. Constraint forces lie in the two-dimensional null space of the map defined by (115) and (116). It isnot difficult to explicitly characterize this null space for the cases that σa
b6= 0 and that σa
b= 0. The characterization
is neither interesting nor obviously useful and has been omitted.
21
is consistent with its being hollow.) The cylinder radius is 1.9 cm. The cylinder length is 19.7cm. The cylinder is connected to other links via six compliant joints. It looks like more bodies areconnected to the cylinder, but in fact only three bodies on the linkage plates are rigidly connectedto the cylinder. Each of these three bodies is connected to two other bodies via a compliant joint.
The base is modeled as having inertial properties of a uniform cylinder with a density of 10%that of aluminum. The radius of the fictitious cylinder is 19 cm. The length of the fictitious cylinderis 197 cm. This is of course completely arbitrary. Essentially the base is modelled as being 100times as massive as the inner cylinder. (The base is meant to be attached to a much more massivesatellite core structure.)
The 36 notch hinges are assumed to be identical (in fact they differ slightly). The Young’smodulus of the hinges is assumed to be 5% of that of aluminum, so that elastic deformations aremore visible. The assumed Poisson ratio is that of aluminum. The hole diameters are 0.8 cm. Theinter-hole spacings of the model are 0.05 cm. The inter-hole spacings of the mock device are 0.1cm, which are thought to be thicker than those of the actual device. The hinge thickness is 0.95cm, the linkage plate thickness. Stiffness parameters corresponding to these material parametersand dimensions were computed using functions reported in (Zhang 1999; Zhang and Fasse 2000a).Those functions are approximations to data from thousands of static finite element simulations.
The twelve cantilever flexures are identical. Again the Young’s modulus of the flexures is assumedto be 5% of that of aluminum. The assumed Poisson ratio is that of aluminum. The length of thecantilever beams is 0.9 cm. The thickness of the beams is 0.05 cm. The actual thickness of the mockdevice flexures are 0.2 cm, which is far too thick. (As a result the mock device moves very little.)The width of the beams is the linkage plate thickness of 0.95 cm. Stiffness parameters correspondingto these material parameters and dimensions were computed using linear beam theory(Fasse andBreedveld 1998a). These parameters were computed at the center of stiffness, which is at thecentroid of the beam. It would be more accurate to compute the stiffness parameters using staticfinite element analysis.
The ARCADE device has six helical tension springs that are used to change the lowest eigen-frequency of the mechanism. These springs were not modeled in the simulation. They could bemodeled by the methods presented in this paper or by more conventional methods.
The author does not have a method for estimating the damping coefficients realistically. Thosecoefficients were chosen arbitrarily in the simulations.
Renderings of the simulated device are shown in Fig. 4. The initial conditions of the simulationcorrespond to a pure rotation at a rate of 105 rad/s (1000 rpm) about the long axis of the device.The central cylinder has an additional velocity of 5 m/s along the axis of the cylinder. These initialconditions were chosen arbitrarily. The actual ARCADE would be nearly irrotational. Its innercylinder would oscillate at a frequency of 0.33 Hz with an amplitude of 3 mm.
While the simulation is in many ways realistic, it is unrealistic in the following ways. First,the topology of the mock mechanism is probably different than that of the actual mechanism.Second, the assumed Young’s modulus of the flexural joints is only 5% of that of the actual joints.Third, the damping characterstics of the flexural joints were chosen arbitrarily. The author doesnot have a method for choosing damping coefficients, although methods may be described in theliterature. Fourth, the assumed initial conditions and lack of actuation are inconsistent with theintended function of the device. Using more realistic initial conditions would result in less interestingmotions.
These results are too preliminary to validate the methods. Still, the simulation results arepromising. They demonstrate that the methods of Secs. 4 and 5 are potentially viable alternatives tofinite element methods for simulation of complex flexible multibody systems. Finite element methodsrequire detailed geometric and material constitutive models. The methods described in this paper
22
(a)
PSfrag replacements
(b)
PSfrag replacements
(c)
PSfrag replacements
(d)
PSfrag replacements
(e)
PSfrag replacements
(f)
PSfrag replacements
Figure 4: Simulation of a flexible multibody system model of ARCADE-like device. Time intervalbetween successive panels is 0.5 ms.
23
require only Cartesian stiffness parameters, which can be determined analytically, experimentally,or numerically using for example off-line finite element analysis.
The simulations were performed using a combination of Matlab and Fortran 90 on a sharedIBM RS/6000-SP system running AIX 4. Computation of the viscoelastic constituve equationsis a computational bottleneck, so implementing those equations in Fortran speeds computationconsiderably. Simulating 1 ms of real time takes about 12 hours using only Matlab code, which isunacceptably slow. It takes about 40 minutes using a combination.
Computation of the twist-based method constitutive equations takes about 0.5 s using Matlab,and 0.0008 s using Fortran. The first time (0.5 s) is an average time to compute the functioncomply twist.m given in Appendix D. The second time is an average time to compute the corre-sponding Matlab executable function comply twist.mexrs6, which includes but is not limited to acall to the primary function comply twist.f listed in Appendix C and the unlisted gateway functioncomply twistg.f. Computation of the dual quaternion method constitutive equations takes about0.3 s using Matlab, and 0.0007 s using Fortran.
One reason that the simulations are so slow is that the differential equations of the model are stiffand 420th-order5. Much more attention needs to be given to integrating the equations of motionmore effectively.
7 Summary and discussion
Two methods of modeling elastically coupled rigid body pairs were presented. In the first method,relative displacements of the coupled bodies were represented using finite twists. The strain energypotential function was approximated by a quadratic function of the twist displacement. This is thesame potential function reported in (Fasse and Zhang 1999; Fasse, Zhang, and Arabyan 2000), butthe application of dual number methods is new. Griffis and Duffy (1991) defined a potential energyfunction that was a quadratic function of a twist displacement, similar to (39). The configuration-wrench relation that they give is that the wrench is the stiffness matrix times the twist, similar to(52). This relation is only valid for infinitesimal displacements. For finite displacements one mustuse the more general relation (49). The virtual displacement analysis of Sec. 4.2 and associatedderivation of constitutive equations of Sec. 4.3 assuming large displacements is believed to be new.
In the second method, relative displacements were represented using dual quaternions. Thestrain energy potential function was approximated by a quadratic function of the dual quaterniondisplacement. This is a continuation of the work reported in (Zhang and Fasse 2000b), which againwas an extension of results of Caccavale et al. (1998, 1999). The new method treats translation androtation in a more consistent way.
The potential functions derived were shown to be defined independently of the body and frameschosen as references. This is important for computer simulations. It means that the simulationresults do not depend on these arbitrary choices6. The author knows of no other generic, lumped-parameter compliance modeling methods for which this is true, not even for planar systems. Thisis the primary benefit compared with other methods reported by the author (Fasse and Breedveld1998a; Zhang and Fasse 2000b). Preliminary details of the planar case for both methods are givenin Appendices A and B. Dual number methods cannot be used for the planar case.
The potential utility of the methods was demonstrated by simulating the behavior of a com-plex spatial, flexural mechanism. Preliminary simulation results are promising. They demonstrate
5There are actually 455 states: 105 Cartesian coordinates, 140 conventional Euler parameters, 105 linear velocities,and 105 angular velocities.
6Nonetheless, the author advocates choosing the coincident center of stiffness frames as references.
24
that the methods of Secs. 4 and 5 are potentially viable alternatives to finite element methods forsimulation of complex flexible multibody systems.
Acknowledgements
The mock reconstruction of the grinding spindle adjustment mechanism was built by AME-352student Chris Weber. The mock reconstruction of ARCADE was built by AME-352 student Chadvan Liere. The author is grateful to Ara Arabyan for discussions regarding computation and largedisplacements, and to Parviz Nikravesh for discussions regarding computation. Special thanks go toBernard Dov Adelstein of NASA Ames, who suggested looking at dual number methods in earnest.
References
Ball, R. (1900). A Treatise on the Theory of Screws. Cambridge University Press. 1998 paperback edition.
Bidard, C. (1994). Screw vector bond graphs for the modelling and kinestatic analysis of mechanisms. Ph. D. thesis,Universite Claude Bernard, Lyon, France. In French.
Blanding, D. (1999). Exact Constraint: Machine Design Using Kinematic Principles. New York: ASME Press.
Brelin-Fornari, J. M. (1998). A Lumped Parameter Model of the Human Head and Neck with Active Models. Ph.D. thesis, The University of Arizona.
Brockett, R. and J. Loncaric (1986). The geometry of compliance programming. In C. Byrnes and A. Lindquist(Eds.), Theory and Applications of Nonlinear Control Systems, pp. 35–42. Amsterdam, North Holland.
Caccavale, F., C. Natale, B. Siciliano, and L. Villani (1999). Six-DOF impedance control based on angle/axisrepresentations. IEEE Trans. on Robotics and Automation 15, 289–300.
Caccavale, F., B. Siciliano, and L. Villani (1998). Quaternion-based robot impedance control with nondiagonalstiffness for robot manipulators. In Proc. American Control Conference, pp. 468–472.
Ciblak, N. (1998). Analysis of Cartesian Stiffness and Compliance with Applications. Ph. D. thesis, Georgia Insti-tute of Technology.
Ciblak, N. and H. Lipkin (1998). Synthesis of stiffnesses by springs. In Proc. ASME Design Engineering Technical
Conf., Number DETC98-MECH-5879, CD-ROM.
Clifford, W. (1873). Preliminary sketch of biquaternions. Proc. of the London Mathematical Society 4, 381–395.
Dimentberg, F. (1965). The Screw Calculus and Its Applications in Mechanics. U.S. Dept. of Commerce. TranslationNo. AD680993.
Drake, S. (1977). Using Compliance in Lieu of Sensory Feedback for Automatic Assembly. Ph. D. thesis, MIT,Cambridge, MA.
Dunbar, C. (1934). An interferometer microscope. J. of Scientific Instruments 11, 85–89.
Fasse, E. (1997). On the spatial compliance of robotic manipulators. ASME J. of Dynamic Systems, Measurement
and Control 119, 839–844.
Fasse, E. and P. Breedveld (1998a). Modelling of elastically coupled bodies: Part I: General theory and geometricpotential function method. ASME J. of Dynamic Systems, Measurement and Control 120, 496–500.
Fasse, E. and P. Breedveld (1998b). Modelling of elastically coupled bodies: Part II: Exponential and generalizedcoordinate methods. ASME J. of Dynamic Systems, Measurement and Control 120, 501–506.
Fasse, E. and J. Broenink (1997). A spatial impedance controller for robotic manipulation. IEEE Trans. on Robotics
and Automation 13, 546–556.
Fasse, E. and C. Gosselin (1999). Spatio-geometric impedance control of Gough-Stewart platforms. IEEE Trans.
on Robotics and Automation 15, 281–288.
Fasse, E. and S. Zhang (1999). Lumped-parameter modelling of spatial compliance using a twist-based potentialfunction. In Proc. of the ASME Dynamic Systems and Control Division, Volume 15, pp. 779–786.
Fasse, E., S. Zhang, and A. Arabyan (2000). Modelling of elastically coupled bodies using a twist-based potentialfunction. ASME J. of Dynamic Systems, Measurement and Control . Submitted for publication.
Fasse, E. D. (1995). Simplification of compliance selection using spatial compliance control. In Proc. of the ASME
Dynamic Systems and Control Division, Volume 57-1, pp. 193–198.
Fasse, E. D. (1996). A spatial compliance controller for serial manipulators. In Proc. of the ASME Dynamic Systems
and Control Division, Volume 58, pp. 607–613.
25
Fischer, I. S. (1999). Dual-Number Methods in Kinematics, Statics and Dynamics. Boca Raton: CRC Press.
Frecker, M., G. Ananthasuresh, S. Nishiwaki, N. Kikuchi, and S. Kota (1997). Topological synthesis of compliantmechanisms using multi-criteria optimization. ASME J. of Mechanical Design 119, 238–245.
Goldfarb, M. and J. Speich (1999). A well-behaved revolute flexure joint for compliant mechanism design. ASME
J. of Mechanical Design 121, 424–429.
Griffis, M. and J. Duffy (1991). Kinestatic control: A novel theory for simultaneously regulating force and displace-ment. ASME J. of Mechanical Design 113, 508–515.
Hetrick, J. and S. Kota (1999). An energy formulation for parametric size and shape optimization of compliantmechanisms. ASME J. of Mechanical Design 121, 229–234.
Hollis, R. L., S. Salcudean, and A. P. Allan (1991). A six-degree-of-freedom magnetically levitated variable compli-ance fine-motion wrist: Design, modeling, and control. IEEE Trans. on Robotics and Automation 7, 320–332.
Howard, W., M. Zefran, and V. Kumar (1995). On the 6 × 6 stiffness matrix for three dimensional motions. InProc. 9th IFToMM, pp. 1575–1579.
Howell, L. and A. Midha (1994). Evaluation of equivalent spring stiffness for use in a pseudo-rigid-body model oflarge-deflection compliant mechanisms. In ASME Design Technical Conferences, 23rd Biennial Mechanisms
Conference, pp. 405–412.
Howell, L. and A. Midha (1995). Parametric deflection approximations for end-loaded, large-deflection beams incompliant mechanisms. ASME J. of Mechanical Design 117, 156–165.
Howell, L., A. Midha, and T. Norton (1996). Evaluation of equivalent spring stiffness for use in a pseudo-rigid-bodymodel of large-deflection compliant mechanisms. ASME J. of Mechanical Design 118, 126–131.
Huang, S. (1998). The Analysis and Synthesis of Spatial Compliance. Ph. D. thesis, Marquette University, Milwau-kee, WI.
Huang, S. and J. Schimmels (1998a). Achieving an arbitrary spatial stiffness with springs connected in parallel.ASME J. of Mechanical Design 120, 520–526.
Huang, S. and J. Schimmels (1998b). The bounds and realization of spatial stiffness achieved with simple springsconnected in parallel. IEEE Trans. on Robotics and Automation 14, 466–475.
Huang, S. and J. Schimmels (1998c). A procedure for the synthesis of simple spring realizable spatial stiffnesses. InProc. of the ASME Dynamic Systems and Control Division, Volume 62, pp. 637–642.
Jensen, B., L. Howell, and L. Salmon (1999). Design of two-link, in-plane, bistable compliant micromechanisms.ASME J. of Mechanical Design 121, 416–423.
Kobrinskii, A. (1969). Dynamics of mechanisms with elastic connections and impact systems. London: Iliffe BooksLtd.
Koster, M. (1998). Constructieprincipes voor het nauwkeurig bewegen en positioneren (Second ed.). Twente Uni-versity Press. In Dutch.
Loncaric, J. (1985). Geometrical Analysis of Compliant Mechanisms in Robotics. Ph. D. thesis, Harvard University,Cambridge, MA.
Loncaric, J. (1987). Normal forms of stiffness and compliance matrices. IEEE Trans. on Robotics and Automation 3,567–572.
Marsh, M. (1929). An instrument for the measurement of thickness of compressible solids. J. of Scientific Instru-ments 6, 382–385.
Maschke, B. (1996). Elements on the modelling of mechanical systems. In C. Melchiorri and A. Tornambe (Eds.),Modelling and Control of Mechanisms and Robots, pp. 1–31. World Scientific Publishing.
Maschke, B., C. Bidard, and A. van der Schaft (1994). Screw-vector bond graphs for the kinestatic and dynamicmodelling of multibody systems. In Proc. of the ASME Dynamic Systems and Control Division, pp. 637–644.
Merrill, T., W. Goldsmith, and Y.-C. Deng (1984). Three-dimensional response of a lumped parameter head-neckmodel due to impact and impulsive loading. J. Biomechanics 17, 81–85.
Negretti and Zambra (1925). A surveying barograph. J. of Scientific Instruments 2, 280–288.
Nikravesh, P. (1988). Computer-Aided Analysis of Mechanical Systems. Prentice Hall.
Panjabi, M., R. Brand, and A. White (1976). Three dimensional flexibility and stiffness properties of the humanthoracic spine. J. Biomechanics 9, 185–192.
Paros, J. and L. Weisbord (1965, nov). How to design flexure hinges. Machine Design, 151–156.
Patterson, T. and H. Lipkin (1993a). A classification of robot compliance. ASME J. of Mechanical Design 115,581–584.
Patterson, T. and H. Lipkin (1993b). Structure of robot compliance. ASME J. of Mechanical Design 115, 576–580.
Reshetov, L. (1982). Self-Aligning Mechanisms. Moscow: Mir Publishers.
26
Roberts, R. (1999). Minimal realization of a spatial stiffness matrix with simple springs connected in parallel. IEEETrans. on Robotics and Automation 15, 953–958.
Schiehlen, W. (1997). Multibody system dynamics: Roots and perspectives. Multibody System Dynamics 1, 149–188.
Selig, J. (1996). Geometrical Methods in Robotics. Springer Verlag.
Shabana, A. (1997). Flexible multibody dynamics: Review of past and recent developments. Multibody System
Dynamics 1, 189–222.
Smith, S. and D. Chetwynd (1992). Foundations of Ultraprecision Mechanism Design. Gordon and Breach SciencePublishers.
Stramigioli, S. (1998). From Differentiable Manifolds to Interactive Robot Control. Ph. D. thesis, Delft Universityof Technology.
Taylor, H. and J. Waldram (1933). Improvements on the Schlieren method. J. of Scientific Instruments 10, 378–389.
Whitney, D. (1982). Quasi-static assembly of compliantly supported rigid parts. ASME J. of Dynamic Systems,
Measurement and Control 104, 65–77.
Zefran, M. and V. Kumar (1997). Affine connections for the Cartesian stiffness matrix. In Proc. IEEE Int. Conf.
on Robotics and Automation, pp. 1376–1381.
Zefran, M. and V. Kumar (1999). A geometric approach to the study of the Cartesian stiffness matrix. ASME J.
of Mechanical Design. Accepted for publication.
Zhang, S. (1999). Lumped-Parameter Modelling of Elastically Coupled Bodies: Derivation of Constitutive Equations
and Determination of Stiffness Matrices. Ph. D. thesis, The University of Arizona.
Zhang, S. and E. Fasse (2000a). A finite-element-based method to determine canonical stiffness parameters of elasticelements applied to notch hinges. ASME J. of Mechanical Design. Submitted for publication.
Zhang, S. and E. Fasse (2000b). Spatial compliance modeling using a quaternion-based potential function method.Multibody System Dynamics 4, 75–101.
A Planar case: twist method
Consider the restriction of the twist-based method presented in Sec. 4 to the x-y plane. For thespatial case twists are of the form
t = τ1i+ τ2j + τ3k + t1iε+ t2jε+ t3kε (118)
where i, j and k are the unit quaternions such that i2 = j2 = k2 = −1 and ijk = −1. For the planarcase twists are of the form
t = τk + t1iε+ t2jε (119)
This can also be written in the formt = αu (120)
where angle α is a scalar and the axis u is
u = k + υ1iε+ υ2jε (121)
In general given v =[
v1; v2
]
let v =[
−v2; v1
]
. Given scalar b let
sk(b) =
[
0 −bb 0
]
(122)
Given tab, computation of 2×2 orthonormal matrix Rab and 2×1 displacement pa
b is straightforward.
Rab = cosαa
bI + sinαab sk(1) (123)
pab = sinαa
bυab + versαa
bυab (124)
27
Here is an algorithm for computation of tab given Rab and pa
b. Let
cab =1
2tr(Ra
b) (125)
which is the cosine of αab. If c
ab = 1, then
τab = 0 (126)
tab = pab (127)
If −1 < cab < 1, then
αab = cos−1(cab) (128)
υab =
1
2 sinαab
(
pab + (Ra
b)Tpa
b
)
(129)
τab = αa
b (130)
tab = αabυ
ab (131)
As potential energy let
U2K(Ra
b, pab) =
1
2
[
τab (tab)
T]
K
[
τab
tab
]
(132)
where K is the 3× 3 symmetric stiffness matrix
K =
[
Ko Kc
KTc Kt
]
(133)
Block Ko is a scalar, block Kc is 1× 2, and block Kt is 2× 2.Given nonzero αa
b let
ρab = 1−
αab(1 + cosαa
b)
2 sinαab
(134)
If αab = 0 then ρa
b = 0. We can then define
Γab =
[
1 − 12(tab)
T + ρab(υ
ab)
T
0 (1− ρab)I−
αa
b
2sk(1)
]
(135)
Let mbe =
[
mbe ; µb
e
]
be the wrench exerted by body ‘B’ on the elastic body expressed as a motorat ‘b’ in coordinates of ‘b’. It is given by
[
mbe
µbe
]
= ΓabK
[
τab
tab
]
(136)
Given R and p let
AdR,p =
[
1 0T
−p R
]
(137)
be the associated adjoint matrix.The wrench exerted by body ‘B’ on the elastic body expressed as a motor in the inertial reference
frame is[
me
µe
]
= Ad−TRb,pb
[
mbe
µbe
]
=
[
1 (pb)TRb
0 Rb
] [
mbe
µbe
] (138)
28
Next consider viscous behavior. The rate of change of the twist is determined by[
τab
tab
]
= (Γab)
T
[
ωbB
vbB
]
−(
Γba
)T[
ωaA
vaA
]
(139)
where
Γba =
[
1 − 12(tba)
T + ρba(υ
ba )
T
0 (1− ρba)I−
αb
a
2sk(1)
]
=
[
1 12(tab)
T + ρab(υ
ab)
T
0 (1− ρab)I +
αa
b
2sk(1)
]
(140)
The viscous damping behavior is defined by
δW =[
τab
(
tab)T]
BT
[
δτab
δtab
]
(141)
Let mbv =
[
mbv; µb
v
]
be the wrench exerted by body ‘B’ on the elastic body expressed as a motorat ‘b’ in coordinates of ‘b’. It is given by
[
mbv
µbv
]
= ΓabB
[
τab
tab
]
(142)
The wrench exerted by body ‘B’ on the viscous body expressed as a motor in the inertial referenceframe is
[
mv
µv
]
=
[
1 (pb)TRb
0 Rb
] [
mbv
µbv
]
(143)
B Planar case: Clifford algebra method
Consider the restriction of the dual quaternion method presented in Sec. 5 to the x-y plane. Inthis method rigid body displacements were represented using unit dual quaternions, which are anexample of what is more generally known as a Clifford algebra (Selig 1996). Planar rigid bodydisplacements can also be represented by a Clifford algebra, but the notion has to be modifiedsomewhat. For the spatial case the elements of the Clifford algebra are of the form
q = σ + sε+ ξ1i+ ξ2j + ξ3k + x1iε+ x2jε+ x3kε (144)
where i, j and k are the unit quaternions such that i2 = j2 = k2 = −1 and ijk = −1. Thesub-algebra representing planar displacements has elements of the form
q = σ + ξk + x1iε+ x2jε (145)
The scalar and vector parts are
σ = cos(α
2
)
(146)
x = ξk + x1iε+ x2jε = sin(α
2
)
u (147)
whereu = k + υ1iε+ υ2jε (148)
29
Thus
ξ = sin(α
2
)
(149)
x = sin(α
2
)
(υ1iε+ υ2jε) (150)
In general given v =[
v1; v2
]
let v =[
−v2; v1
]
. Given scalar b let
sk(b) =
[
0 −bb 0
]
(151)
Given σab and xab, computation of 2×2 orthonormal matrix Ra
b and 2×1 displacement pab is straight-
forward.
Rab =
(
1− 2 (ξab)
2)
I + 2σab sk(ξ
ab) (152)
pab = 2σa
bxab + 2ξabx
ab (153)
Here is an algorithm for computation of σab and xab given Ra
b and pab. First consider the scalar
part σab. Let
σab =
1
2
√
tr(Rab) + 2 (154)
We are free, for example, to choose the non-negative root, in which case 0 ≤ σab ≤ 1.
Next consider the vector part xab. If σab > 0, then
sk(ξab) =
as (Rab)
2σab
(155)
This is not well behaved numerically for small σab and used only for σa
b > 0.1 in the softwareimplementation. Otherwise let
ξab =
√
1
2−
1
4tr(Ra
b) (156)
Sign ambiguity is resolved by looking at the antisymmetric part or Rab. let M = as(Ra
b). IfM2,1 > 0 then ξa
b > 0. If M2,1 < 0 then ξab < 0. If M2,1 = 0 then σa
b = 0 and the sign of ξab can be
chosen arbitrarily. Expression (156) is always well defined, but it is not well behaved for large σab.
This expression is only used for σab ≤ 0.1 in the software implementation.
Computation of xab is then straightforward.
xab =
1
2(σa
bI− sk(ξab))p
ab (157)
As potential energy let
U2K(Ra
b, pab) = 2
[
ξab (xab)
T]
K
[
ξabxa
b
]
(158)
where K is the 3× 3 symmetric stiffness matrix
K =
[
Ko Kc
KTc Kt
]
(159)
Block Ko is a scalar, block Kc is 1× 2, and block Kt is 2× 2.
30
Let
Λab =
[
σab − (xa
b)T
0 σabI− sk (ξa
b)
]
(160)
Let mbe =
[
mbe ; µb
e
]
be the wrench exerted by body ‘B’ on the elastic body expressed as a motorat ‘b’ in coordinates of ‘b’. It is given by
[
mbe
µbe
]
= 2ΛabK
[
ξabxa
b
]
(161)
Given R and p let
AdR,p =
[
1 0T
−p R
]
(162)
be the associated adjoint matrix.The wrench exerted by body ‘B’ on the elastic body expressed as a motor in the inertial reference
frame is[
me
µe
]
= Ad−TRb,pb
[
mbe
µbe
]
=
[
1 (pb)TRb
0 Rb
] [
mbe
µbe
] (163)
Next consider viscous behavior. The rate of change of the vector part is determined by
[
ξabxa
b
]
=1
2(Λa
b)T
[
ωbB
vbB
]
−1
2
(
Λba
)T[
ωaA
vaA
]
(164)
where
Λba =
[
σba −
(
xba
)T
0 σba I− sk
(
ξba)
]
=
[
σab (xa
b)T
0 σabI + sk (ξa
b)
]
(165)
The viscous damping behavior is defined by
δW = 4[
ξab (xab)
T]
BT
[
δξabδxa
b
]
(166)
Let mbv =
[
mbv; µb
v
]
be the wrench exerted by body ‘B’ on the elastic body expressed as a motorat ‘b’ in coordinates of ‘b’. It is given by
[
mbv
µbv
]
= 2ΛabB
[
ξabxa
b
]
(167)
The wrench exerted by body ‘B’ on the viscous body expressed as a motor in the inertial referenceframe is
[
mv
µv
]
=
[
1 (pb)TRb
0 Rb
] [
mbv
µbv
]
(168)
31
C Essential Fortran 90 code
This appendix contains the essential Fortran 90 code implementing the twist-based and dual quaternion-based compliance modeling methods. The main functions are trans2twist in module dual numbers,comply twist, trans2quat in module dual numbers, and comply quat. Function trans2twist,which is a member function of the bare-bones dual number module, computes the twist associatedwith a dual transformation matrix, implementing the algorithm described in Sec. 3.4. Functioncomply twist implements the twist-based method described in Sec. 4. Function trans2quat inthe dual number module computes the dual quaternion associated with a dual transformation ma-trix, implementing the algorithm described in Sec. 3.5. Function comply quat implements the dualquaternion-based method described in Sec. 5. Other software has been and is being developed andshall ultimately be made available.
Note that while functions comply twist and comply quat use dual number methods, they donot take dual numbers as arguments, nor do they return dual numbers. It is not assumed that theusers of these routines will be familiar with dual number methods! Not listed are the Matlab andScilab gateway functions.
comply twist.f
!COMPLYTWIST(pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B) computes a v i s c o e l a s t i c wrench! in g l oba l coo rd ina t e s g iven compliance frame va r i a b l e s pa ,Ra , pb and Rb ;! g iven 6 x6 s t i f f n e s s matrix K = [Ko Kc ; Kc ’ Kt ] ; g iven l i n e a r and angular! v e l o c i t i e s va , oma , vb and omb ; and given 6 x6 damping c o e f f i c i e n t matrix!B = [Bo Bc ; Bc ’ Bt ] . The wrench i s computed us ing the ” tw i s t−based”! method . The wrench returned , w0 = [ f0 ; m0 ] , i s the wrench exer ted! by body B on the compliant , v i s c o e l a s t i c element . This i s a l s o the! wrench exer ted by the compliant element on body A.!! arguments! pa : p o s i t i o n o f compliant j o i n t frame o f body A (3 x1 )! pb : p o s i t i o n o f compliant j o i n t frame o f body B (3 x1 )! Ra : j o i n t frame o r i e n t a t i o n matrix o f body A (3 x3 )! Rb : j o i n t frame o r i e n t a t i o n matrix o f body B (3 x3 )! K : S t i f f n e s s matrix at a/b ( 6 x6 )! va : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in! c oo rd ina t e s o f that frame .! oma : angular v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in! c oo rd ina t e s o f that frame .! vb : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in! c oo rd ina t e s o f that frame .! omb : angular v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in! c oo rd ina t e s o f that frame .! B : Damping matrix at a/b ( 6 x6 )!module comply twist
use dual numbersimplicit none
contains
function comply ( pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B) result (w)real ∗ 8 , dimension ( 3 ) , intent ( in ) : : pa , pb , va , vb , oma , ombreal ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : Ra , Rbreal ∗ 8 , dimension ( 6 , 6 ) , intent ( in ) : : K, Breal ∗ 8 , dimension ( 6 ) : : w
type ( dual mat ) : : usk , SbaInv , SabInv , Ta , Tb , Tba , DMtmptype ( dua l vec ) : : u , tw
32
type ( dual ) : : a lpha
real ∗ 8 , dimension ( 6 , 6 ) : : Gam ba , Gam ab , Adreal ∗ 8 , dimension ( 6 ) : : dtw , w b , we b , wv b , tmp61 , tmp62
! Compute dual t rans fo rmat ion matr i ce s , i n c l ud ing the disp lacementTa = dual mat (Ra , matmul ( sk ( pa ) , Ra) )Tb = dual mat (Rb , matmul ( sk (pb ) ,Rb) )Tba = transpose (Ta)∗Tb ! eq (14)
tw = t ran s2 tw i s t (Tba ) ! eqs (20)–(24)
i f (max( tw%pr ( 1 ) , tw%pr ( 2 ) , tw%pr (3)) <= 1 e−5) then
SbaInv = eye − 0 .5 8 ∗ sk ( tw ) ! eq (43)else
alpha = modulus ( tw)
i f ( abs ( p i − alpha%pr ) < 1 e−5) then
print ∗ , ” comply twist : Not de f ined f o r r o t a t i o n s o f p i . ”else ! use eq (44)
u = tw/alpha ; usk = sk (u)SbaInv = eye − 0 .5 8 ∗( alpha ∗usk ) &
+ (1.0 8 − ( alpha ∗ ( 1 . 0 8 + cos ( alpha ) ) ) / ( 2 . 0 8 ∗ s i n ( alpha )))∗ &( usk∗usk )
end i f
end i f
! Compute e l a s t i c wrench in body coo rd ina t e sGam ba ( 1 : 3 , 1 : 3 ) = SbaInv%pr ; Gam ba ( 1 : 3 , 4 : 6 ) = SbaInv%du ! eq (46)Gam ba ( 4 : 6 , 1 : 3 ) = 0 . 0 ; Gam ba ( 4 : 6 , 4 : 6 ) = SbaInv%pr
tmp61 ( 1 : 3 ) = tw%pr ; tmp61 ( 4 : 6 ) = tw%duwe b = matmul (Gam ba , matmul (K, tmp61 ) ) ! eq (49)
! Compute v i s c ou s wrench in body coo rd ina t e sSabInv = transpose ( SbaInv ) ! eq (60)Gam ab ( 1 : 3 , 1 : 3 ) = SabInv%pr ; Gam ab ( 1 : 3 , 4 : 6 ) = SabInv%du ! eq (59)Gam ab ( 4 : 6 , 1 : 3 ) = 0 . 0 ; Gam ab ( 4 : 6 , 4 : 6 ) = SabInv%pr
tmp61 ( 1 : 3 ) = omb ; tmp61 ( 4 : 6 ) = vbtmp62 ( 1 : 3 ) = oma ; tmp62 ( 4 : 6 ) = vadtw = matmul ( t ranspose (Gam ba ) , tmp61 ) & ! eq (58)
− matmul ( t ranspose (Gam ab ) , tmp62 )
wv b = matmul (Gam ba , matmul (B, dtw ) ) ! eq (65)
! V i s c o e l a s t i c wrench in body coo rd ina t e sw b = we b + wv b
! Convert to g l oba l , i n e r t i a l c oo rd ina t e sAd( 1 : 3 , 1 : 3 ) = Tb%pr ; Ad( 1 : 3 , 4 : 6 ) = Tb%du ! eqs (50) and (66) , moment f i r s t ,Ad ( 4 : 6 , 1 : 3 ) = 0 . 0 ; Ad( 4 : 6 , 4 : 6 ) = Tb%pr ! f o r c e secondtmp61 = matmul (Ad, w b )w(1 : 3 ) = tmp61 ( 4 : 6 ) ; w( 4 : 6 ) = tmp61 ( 1 : 3 ) ! Swap , re turn f o r c e f i r s t ,
!moment second .end function comply
end module comply twist
comply quat.f
33
!COMPLYQUAT(pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B) computes a v i s c o e l a s t i c wrench! in g l oba l coo rd ina t e s g iven compliance frame va r i a b l e s pa ,Ra , pb and Rb ;! g iven 6 x6 s t i f f n e s s matrix K = [Ko Kc ; Kc ’ Kt ] ; g iven l i n e a r and angular! v e l o c i t i e s va , oma , vb and omb ; and given 6 x6 damping c o e f f i c i e n t matrix!B = [Bo Bc ; Bc ’ Bt ] . The wrench i s computed us ing the ” tw i s t−based”! method . The wrench returned , w0 = [ f0 ; m0 ] , i s the wrench exer ted! by body B on the compliant , v i s c o e l a s t i c element . This i s a l s o the! wrench exer ted by the compliant element on body A.!! arguments! pa : p o s i t i o n o f compliant j o i n t frame o f body A (3 x1 )! pb : p o s i t i o n o f compliant j o i n t frame o f body B (3 x1 )! Ra : j o i n t frame o r i e n t a t i o n matrix o f body A (3 x3 )! Rb : j o i n t frame o r i e n t a t i o n matrix o f body B (3 x3 )! K : S t i f f n e s s matrix at a/b ( 6 x6 )! va : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in! c oo rd ina t e s o f that frame .! oma : angular v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in! c oo rd ina t e s o f that frame .! vb : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in! c oo rd ina t e s o f that frame .! omb : angular v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in! c oo rd ina t e s o f that frame .! B : Damping matrix at a/b ( 6 x6 )!module comply quat
use dual numbersimplicit none
contains
function comply ( pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B) result (w)real ∗ 8 , dimension ( 3 ) , intent ( in ) : : pa , pb , va , vb , oma , ombreal ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : Ra , Rbreal ∗ 8 , dimension ( 6 , 6 ) , intent ( in ) : : K, Breal ∗ 8 , dimension ( 6 ) : : w
type ( dual mat ) : : Ta , Tb , Tba , DMtmptype ( dua l quat ) : : qbatype ( dual ) : : sbatype ( dua l vec ) : : xba
real ∗ 8 , dimension ( 6 , 6 ) : : Lam ba , Lam ab , Adreal ∗ 8 , dimension ( 6 ) : : dx , w b , we b , wv b , tmp61 , tmp62
! Compute dual t rans fo rmat ion matr i ce s , i n c l ud ing the disp lacementTa = dual Mat (Ra , matmul ( sk ( pa ) , Ra) )Tb = dual Mat (Rb , matmul ( sk (pb ) ,Rb) )Tba = transpose (Ta)∗Tb ! eq (14)
qba = trans2quat (Tba ) ! eqs (20)–(24)sba = qba%s c a l ; xba = qba%vec
! Compute e l a s t i c wrench in body coo rd ina t e sDMtmp = sba∗ eye − sk ( xba ) ! eq (85)Lam ba ( 1 : 3 , 1 : 3 ) = DMtmp%pr ; Lam ba ( 1 : 3 , 4 : 6 ) = DMtmp%duLam ba ( 4 : 6 , 1 : 3 ) = 0 . 0 ; Lam ba ( 4 : 6 , 4 : 6 ) = DMtmp%pr
tmp61 ( 1 : 3 ) = xba%pr ; tmp61 ( 4 : 6 ) = xba%duwe b = 2.0∗matmul (Lam ba , matmul (K, tmp61 ) ) ! eq (87)
34
DMtmp = sba∗ eye + sk ( xba ) ! eq (97)Lam ab ( 1 : 3 , 1 : 3 ) = DMtmp%pr ; Lam ab ( 1 : 3 , 4 : 6 ) = DMtmp%duLam ab ( 4 : 6 , 1 : 3 ) = 0 . 0 ; Lam ab ( 4 : 6 , 4 : 6 ) = DMtmp%pr
tmp61 ( 1 : 3 ) = omb ; tmp61 ( 4 : 6 ) = vbtmp62 ( 1 : 3 ) = oma ; tmp62 ( 4 : 6 ) = vadx = 0.5∗matmul ( t ranspose (Lam ba ) , tmp61 ) & ! eq (96)
− 0.5∗matmul ( t ranspose (Lam ab ) , tmp62 )
wv b = 2.0∗matmul (Lam ba , matmul (B, dx ) ) ! eq (102)
! V i s c o e l a s t i c wrench in body coo rd ina t e sw b = we b + wv b
! Convert to g l oba l , i n e r t i a l c oo rd ina t e sAd( 1 : 3 , 1 : 3 ) = Tb%pr ; Ad( 1 : 3 , 4 : 6 ) = Tb%du ! eqs (88) and (103) , moment f i r s t ,Ad ( 4 : 6 , 1 : 3 ) = 0 . 0 ; Ad( 4 : 6 , 4 : 6 ) = Tb%pr ! f o r c e secondtmp61 = matmul (Ad, w b )w(1 : 3 ) = tmp61 ( 4 : 6 ) ; w( 4 : 6 ) = tmp61 ( 1 : 3 ) ! Swap , re turn f o r c e f i r s t ,
!moment second .end function comply
end module comply quat
dual numbers.f
! Bare−bones dual number module f o r implementing v i s c o e l a s t i c compliance! mode l l ing r ou t i n e s . The most important f unc t i on s are t r an s2 tw i s t and! t rans2quat .!module dual numbers
type dualreal ∗ 8 : : pr , du
end type dual
type dua l vecreal ∗ 8 , dimension ( 3 ) : : pr , du
end type dua l vec
type dual matreal ∗ 8 , dimension ( 3 , 3 ) : : pr , du
end type dual mat
type dual quattype ( dual ) : : s c a ltype ( dua l vec ) : : vec
end type dual quat
real ∗ 8 , dimension ( 3 , 3 ) , parameter : : eye = &reshape ( source = ( / 1 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 1 / ) , shape =(/3 ,3/ ) )
! S i r , I send a rhyme e x c e l l i n g in sacred truth and r i g i d s p e l l i n g .! Numerical s p r i t e s e l u c i d a t e f o r me the l e x i c on ’ s f u l l weight .! I f nature gain , who can complain , tho ’ Dr . Johnson fu lminate .real ∗ 8 , parameter : : p i = 3.141592653589793238462643383279
interface operator (−)module procedure dua l m inus r ea lmodule procedure dual mat minus dual matmodule procedure r ea l m inus dua l
35
module procedure rea l mat minus dual matend interface
interface operator (+)module procedure dual mat p lus dua l matmodule procedure r e a l p l u s d u a l
end interface
interface operator (∗ )module procedure dua l t imes dua lmodule procedure dua l t imes dua l matmodule procedure dua l t ime s dua l v e cmodule procedure dua l t ime s r ea l matmodule procedure dual mat t imes dual matmodule procedure r e a l t ime s dua lmodule procedure r e a l t ime s dua l mat
end interface
interface operator (/ )module procedure dua l by dua lmodule procedure dual mat by dualmodule procedure dua l vec by dua l
end interface
interface acosmodule procedure dua l aco s
end interface
interface asmodule procedure dual mat asmodule procedure r ea l mat a s
end interface
interface cosmodule procedure dua l co s
end interface
interface modulusmodule procedure dual modulus
end interface
interface s i nmodule procedure dua l s i n
end interface
interface skmodule procedure dua l v e c skmodule procedure r e a l v e c s k
end interface
interface sk invmodule procedure dua l mat sk invmodule procedure r e a l ma t sk i nv
end interface
interface s q r tmodule procedure dua l s q r t
end interface
interface sy
36
module procedure dual mat symodule procedure r ea l mat sy
end interface
interface t r a c emodule procedure dua l mat t racemodule procedure r e a l ma t t r a c e
end interface
interface t ransposemodule procedure dua l mat t ranspose
end interface
contains
function t r an s2 tw i s t (T) result ( tw)type ( dual mat ) , intent ( in ) : : Ttype ( dua l vec ) : : twtype ( dual ) : : a lpha , catype ( dua l vec ) : : u
ca = 0.5 8 ∗( t r a c e (T) − 1 . 0 8 ) ! eq (20)
i f ( ca%pr > 1 . 0 ) then
print ∗ , ” t r an s2 tw i s t : ca%pr g r e a t e r than 1 . 0 , t r e a t i n g as 1 . 0 . ”end i f
i f ( ca%pr >= 1.0) then
tw%pr = 0 . 0 ; tw%du = sk inv (T%du ) ! eq (21)e l s e i f ( ca%pr <= −1.0) then
print ∗ , ” t r an s2 tw i s t : Not implemented f o r r o t a t i o n s o f p i . ”else
alpha = acos ( ca ) ! eq (22)u = sk inv ( as (T)/ s i n ( alpha ) ) ! eq (23)tw = alpha ∗u ! eq (24)
end i f
end function t r an s2 tw i s t
function t rans2quat (T) result ( q )type ( dual mat ) , intent ( in ) : : Ttype ( dua l quat ) : : qreal ∗ 8 : : sigma , s , x i1 , x i2 , x i3real ∗ 8 , dimension ( 3 , 3 ) : : M, R, psk , x i skreal ∗ 8 , dimension ( 3 ) : : p , x , x i
R = T%pr ! eq (32)psk = matmul (T%du , t ranspose (R) ) ; p = sk inv ( psk ) ! eq (23)
sigma = 0.5∗ s q r t ( t r a c e (R)+1 .0 ) ! eq (34)i f ( sigma < 0 . 0 ) then
print ∗ , ” t rans2quat : s q r t assumed to re turn a p o s i t i v e number”else i f ( sigma > 0 . 1 ) then
x i sk = 0.5∗ as (R)/ sigma ; x i = sk inv ( x i sk ) ! eq (35)else ! ( sigma <= 0.1) ! eq (36)
M = 0.5∗ sy (R) + 0.25∗ (1 .0− t r a c e (R) )∗ eyex i1 = sq r t (M( 1 , 1 ) ) ; x i2 = sq r t (M( 2 , 2 ) ) ; x i3 = sq r t (M( 3 , 3 ) )i f ( x i3 > 0 . 0 ) then
i f (M( 1 , 3 ) < 0 . 0 ) then
x i1 = − x i1end i f
i f (M( 2 , 3 ) < 0 . 0 ) then
x i2 = − x i2
37
end i f
else i f ( x i2 > 0 . 0 ) then
i f (M( 1 , 2 ) < 0 . 0 ) then
x i1 = − x i1end i f
i f (M( 3 , 2 ) < 0 . 0 ) then
x i3 = − x i3end i f
else i f ( x i1 > 0 . 0 ) then
i f (M( 2 , 1 ) < 0 . 0 ) then
x i2 = − x i2 ! Shouldn ’ t get hereend i f
i f (M( 3 , 1 ) < 0 . 0 ) then
x i3 = − x i3 ! Shouldn ’ t get hereend i f
else
print ∗ , ” t rans2quat : Shouldn ’ ’ t get here . ”end i f
x i (1 ) = xi1 ; x i (2 ) = xi2 ; x i (3 ) = xi3 ; x i sk = sk ( x i )end i f
s = −0.5∗ dot product ( x i , p ) ! eq (37)x = 0.5∗matmul ( ( sigma∗ eye−x i sk ) , p ) ! eq (38)
q = dual quat ( dual ( sigma , s ) , dua l vec ( x i , x ) )end function t rans2quat
function dua l m inus r ea l (A, B) result (C)type ( dual ) , intent ( in ) : : Areal ∗ 8 , intent ( in ) : : Btype ( dual ) : : CC%pr = A%pr − BC%du = A%du
end function dua l m inus r ea l
function dual mat minus dual mat (A, B) result (C)type ( dual mat ) , intent ( in ) : : A , Btype ( dual mat ) : : CC%pr = A%pr − B%prC%du = A%du − B%du
end function dual mat minus dual mat
function r ea l m inus dua l (A, B) result (C)real ∗ 8 , intent ( in ) : : Atype ( dual ) , intent ( in ) : : Btype ( dual ) : : CC%pr = A − B%prC%du = −B%du
end function r ea l m inus dua l
function rea l mat minus dual mat (A, B) result (C)real ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : Atype ( dual mat ) , intent ( in ) : : Btype ( dual mat ) : : CC%pr = A − B%prC%du = −B%du
end function rea l mat minus dual mat
function dual mat p lus dua l mat (A, B) result (C)type ( dual mat ) , intent ( in ) : : A , B
38
type ( dual mat ) : : CC%pr = A%pr + B%prC%du = A%du + B%du
end function dual mat p lus dua l mat
function r e a l p l u s d u a l (A,B) result (C)real ∗ 8 , intent ( in ) : : Atype ( dual ) , intent ( in ) : : Btype ( dual ) : : CC%pr = A + B%prC%du = B%du
end function r e a l p l u s d u a l
function dua l t imes dua l (A, B) result (C)type ( dual ) , intent ( in ) : : A , Btype ( dual ) : : CC%pr = A%pr ∗ B%prC%du = (A%pr ∗ B%du ) + (A%du ∗ B%pr )
end function dua l t imes dua l
function dua l t imes dua l mat (A, B) result (C)type ( dual ) , intent ( in ) : : Atype ( dual mat ) , intent ( in ) : : Btype ( dual mat ) : : CC%pr = A%pr ∗ B%prC%du = (A%pr ∗ B%du ) + (A%du ∗ B%pr )
end function dua l t imes dua l mat
function dua l t ime s dua l v e c (A, B) result (C)type ( dual ) , intent ( in ) : : Atype ( dua l vec ) , intent ( in ) : : Btype ( dua l vec ) : : CC%pr = A%pr ∗ B%prC%du = (A%pr ∗ B%du ) + (A%du ∗ B%pr )
end function dua l t ime s dua l v e c
function dua l t ime s r ea l mat (A, B) result (C)type ( dual ) , intent ( in ) : : Areal ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : Btype ( dual mat ) : : CC%pr = A%pr∗BC%du = A%du∗B
end function dua l t ime s r ea l mat
function dual mat t imes dual mat (A, B) result (C)type ( dual mat ) , intent ( in ) : : A , Btype ( dual mat ) : : CC%pr = matmul (A%pr , B%pr )C%du = matmul (A%pr , B%du) + matmul (A%du , B%pr )
end function dual mat t imes dual mat
function r e a l t ime s dua l (A, B) result (C)real ∗ 8 , intent ( in ) : : Atype ( dual ) , intent ( in ) : : Btype ( dual ) : : CC%pr = A ∗ B%prC%du = A ∗ B%du
end function r e a l t ime s dua l
function r e a l t ime s dua l mat (A, B) result (C)
39
real ∗ 8 , intent ( in ) : : Atype ( dual mat ) , intent ( in ) : : Btype ( dual mat ) : : CC%pr = A ∗ B%prC%du = A ∗ B%du
end function r e a l t ime s dua l mat
function dua l by dua l (A,B) result (C)type ( dual ) , intent ( in ) : : A , Btype ( dual ) : : CC%pr = A%pr /(B%pr )C%du = (A%du∗B%pr − A%pr∗B%du )/(B%pr ∗∗2)
end function dua l by dua l
function dual mat by dual (A,B) result (C)type ( dual mat ) , intent ( in ) : : Atype ( dual ) , intent ( in ) : : Btype ( dual mat ) : : CC%pr = A%pr /(B%pr )C%du = (A%du∗B%pr − A%pr∗B%du )/(B%pr ∗∗2)
end function dual mat by dual
function dua l vec by dua l (A,B) result (C)type ( dua l vec ) , intent ( in ) : : Atype ( dual ) , intent ( in ) : : Btype ( dua l vec ) : : CC%pr = A%pr /(B%pr )C%du = (A%du∗B%pr − A%pr∗B%du )/(B%pr ∗∗2)
end function dua l vec by dua l
function dua l aco s (CA) result (A)type ( dual ) , intent ( in ) : : CAtype ( dual ) : : Areal ∗ 8 : : ALPHA
i f (CA%pr > 1 . 0 ) then
print ∗ , ” acos : CA%pr g r e a t e r than 1 . 0 , t r e a t i n g as 1 .0”ALPHA = 0.0
else
ALPHA = acos (CA%pr ) ;end i f
A%pr = ALPHA; A%du = (−CA%du )/( s i n (ALPHA))end function dua l aco s
function dual mat as (A) result (AAS)type ( dual mat ) , intent ( in ) : : Atype ( dual mat ) : : AASAAS%pr = as (A%pr ) ; AAS%du = as (A%du)
end function dual mat as
function r ea l mat a s (A) result (AAS)real ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : Areal ∗ 8 , dimension ( 3 , 3 ) : : AASAAS( 1 , 1 ) = 0 . 0 ; AAS( 2 , 2 ) = 0 . 0 ; AAS( 3 , 3 ) = 0 . 0AAS( 1 , 2 ) = 0 . 5 ∗ (A(1,2)−A( 2 , 1 ) ) ; AAS(2,1) = −AAS( 1 , 2 )AAS( 1 , 3 ) = 0 . 5 ∗ (A(1,3)−A( 3 , 1 ) ) ; AAS(3,1) = −AAS( 1 , 3 )AAS( 2 , 3 ) = 0 . 5 ∗ (A(2,3)−A( 3 , 2 ) ) ; AAS(3,2) = −AAS( 2 , 3 )
end function r ea l mat a s
function dua l co s (A) result (B)
40
type ( dual ) , intent ( in ) : : Atype ( dual ) : : BB%pr = cos (A%pr ) ; B%du = − s i n (A%pr )∗ (A%du)
end function dua l co s
function dual modulus (A) result (B)type ( dua l vec ) , intent ( in ) : : Atype ( dual ) : : Btype ( dual ) : : BSQBSQ%pr = dot product (A%pr ,A%pr )BSQ%du = 2.0∗ dot product (A%pr ,A%du)B = sq r t (BSQ)
end function dual modulus
function dua l s i n (A) result (B)type ( dual ) , intent ( in ) : : Atype ( dual ) : : BB%pr = s i n (A%pr ) ; B%du = cos (A%pr )∗ (A%du)
end function dua l s i n
function dua l v e c sk (V) result (VSK)type ( dua l vec ) , intent ( in ) : : Vtype ( dual mat ) : : VSKVSK = dual mat ( sk (V%pr ) , sk (V%du ) )
end function dua l v e c sk
function r e a l v e c s k (V) result (VSK)real ∗ 8 , dimension ( 3 ) , intent ( in ) : : Vreal ∗ 8 , dimension ( 3 , 3 ) : : VSKVSK(1 , 1 ) = 0 . 0 ; VSK(1,2) = −V( 3 ) ; VSK(1 , 3 ) = V(2)VSK(2 , 1 ) = V( 3 ) ; VSK(2 , 2 ) = 0 . 0 ; VSK(2,3) = −V(1)VSK(3,1) = −V( 2 ) ; VSK(3 , 2 ) = V( 1 ) ; VSK(3 , 3 ) = 0 .0
end function r e a l v e c s k
function r e a l ma t sk i nv (VSK) result (V)real ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : VSKreal ∗ 8 , dimension ( 3 ) : : VV(1) = VSK( 3 , 2 ) ; V(2) = VSK( 1 , 3 ) ; V(3) = VSK( 2 , 1 )
end function r e a l ma t sk i nv
function dua l mat sk inv (VSK) result (V)type ( dual mat ) , intent ( in ) : : VSKtype ( dua l vec ) : : VV%pr = sk inv (VSK%pr ) ; V%du = sk inv (VSK%du)
end function dua l mat sk inv
function dua l s q r t (A) result (B)type ( dual ) , intent ( in ) : : Atype ( dual ) : : Bi f (A%pr == 0.0) then
i f (A%du == 0.0) then
B = dual ( 0 . 0 , 0 . 0 )else
print ∗ , ” dua l s q r t : Sqrt o f pure ly dual number i s not de f ined . ”end i f
else !No e r r o r check ing , t h i s c ra she s f o r negat ive A%prB%pr = sq r t (A%pr )B%du = A%du/(2 . 0∗B%pr )
end i f
end function dua l s q r t
41
function dual mat sy (A) result (ASY)type ( dual mat ) , intent ( in ) : : Atype ( dual mat ) : : ASYASY%pr = sy (A%pr ) ; ASY%du = sy (A%du)
end function dual mat sy
function r ea l mat sy (A) result (ASY)real ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : Areal ∗ 8 , dimension ( 3 , 3 ) : : ASYASY(1 , 1 ) = A( 1 , 1 ) ; ASY(2 , 2 ) = A( 2 , 2 ) ; ASY(3 , 3 ) = A( 3 , 3 )ASY(1 , 2 ) = 0 . 5 ∗ (A(1,2)+A( 2 , 1 ) ) ; ASY(2 , 1 ) = ASY( 1 , 2 )ASY(1 , 3 ) = 0 . 5 ∗ (A(1,3)+A( 3 , 1 ) ) ; ASY(3 , 1 ) = ASY( 1 , 3 )ASY(2 , 3 ) = 0 . 5 ∗ (A(2,3)+A( 3 , 2 ) ) ; ASY(3 , 2 ) = ASY( 2 , 3 )
end function r ea l mat sy
function dua l mat t ranspose (A) result (B)type ( dual mat ) , intent ( in ) : : Atype ( dual mat ) : : BB%pr = transpose (A%pr )B%du = transpose (A%du)
end function dua l mat t ranspose
function dua l mat t race (A) result (B)type ( dual mat ) , intent ( in ) : : Atype ( dual ) : : BB%pr = A%pr ( 1 , 1 ) + A%pr ( 2 , 2 ) + A%pr ( 3 , 3 )B%du = A%du (1 , 1 ) + A%du (2 , 2 ) + A%du ( 3 , 3 )
end function dua l mat t race
function r e a l ma t t r a c e (A) result (B)real ∗ 8 , dimension ( 3 , 3 ) , intent ( in ) : : Areal ∗ 8 : : BB = A(1 , 1 ) + A(2 , 2 ) + A( 3 , 3 )
end function r e a l ma t t r a c eend module dual Numbers
42
D Essential Matlab code
This appendix contains the essential Matlab code implementing the twist-based and dual quaternion-based compliance modeling methods. The main functions are @dual/trans2twist, comply twist,@dual/trans2quat, and comply quat. Function @dual/trans2twist, which is a member func-tion of the dual number class, computes the twist associated with a dual transformation matrix,implementing the algorithm described in Sec. 3.4. Function comply twist implements the twist-based method described in Sec. 4. Function @dual/trans2quat computes the dual quaternionassociated with a dual transformation matrix, implementing the algorithm described in Sec. 3.5.Function comply quat implements the dual quaternion-based method described in Sec. 5. Mostof the other functions (@dual/sin, @dual/acos, @dual/transpose, etc.) are general-purposemember functions of a rudimentary dual-number class. This class was inspired by the C++ duplex
class given in (Fischer 1999), but was not derived directly from it.Again, while functions comply twist and comply quat use dual number methods, they do not
take dual numbers as arguments, nor do they return dual numbers. It is not assumed that the usersof these routines will be familiar with dual number methods.
@dual/trans2twist.m
function tw = trans2twist (T)%TRANS2TWIST(T) r e tu rn s the tw i s t cor re spond ing to dual t rans fo rmat ion matrix T%This i s a l o c a l d i f feomorphism .
co s a lpha = dual ( 0 . 5 , 0 ) ∗ ( trace (T)−dual ( 1 , 0 ) ) ; %eq (20)
i f ( co s a lpha . p == 1.0)tw = sk inv (dual ( zeros ( 3 , 3 ) ,du(T ) ) ) ; %eq (21)
e l s e i f ( co s a lpha . p == −1.0)error ( ’ Not implemented f o r r o t a t i o n s o f p i , s o r ry . ’ )
else
alpha = acos ( co s a lpha ) ; %eq (22)u = sk inv (as (T)/ sin ( alpha ) ) ; %eq (23)tw = alpha ∗u ; %eq (24)
end
comply twist.m
function w0=comply twist ( pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B)%COMPLYTWIST(pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B) computes a v i s c o e l a s t i c wrench%in g l oba l coo rd ina t e s g iven compliance frame va r i a b l e s pa ,Ra , pb and Rb ;%given 6 x6 s t i f f n e s s matrix K = [Ko Kc ; Kc ’ Kt ] ; g iven l i n e a r and angular%v e l o c i t i e s va , oma , vb and omb ; and given 6 x6 damping c o e f f i c i e n t matrix%B = [Bo Bc ; Bc ’ Bt ] . The wrench i s computed us ing the ” tw i s t−based”%method . The wrench returned , w0 = [ f0 ; m0 ] , i s the wrench exer ted% by body B on the compliant , v i s c o e l a s t i c element . This i s a l s o the%wrench exer ted by the compliant element on body A.%%arguments% pa : p o s i t i o n o f compliant j o i n t frame o f body A (3 x1 )% pb : p o s i t i o n o f compliant j o i n t frame o f body B (3 x1 )% Ra : j o i n t frame o r i e n t a t i o n matrix o f body A (3 x3 )% Rb : j o i n t frame o r i e n t a t i o n matrix o f body B (3 x3 )% K : S t i f f n e s s matrix at a/b ( 6 x6 )% va : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in% coo rd ina t e s o f that frame .
43
% oma : angular v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in% coo rd ina t e s o f that frame .% vb : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in% coo rd ina t e s o f that frame .% omb : angular v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in% coo rd ina t e s o f that frame .% B : Damping matrix at a/b ( 6 x6 )
%Computation o f c o n s t i t u t i v e equat ions us ing tw i s t−based method .eye = dual (eye ( 3 ) , zeros ( 3 , 3 ) ) ; %de f i n e dual i d e n t i t y matrix
%Compute dual t rans fo rmat ion matr i ce s , i n c l ud ing the disp lacementTa = dual (Ra , sk ( pa )∗Ra ) ;Tb = dual (Rb , sk (pb)∗Rb) ;Tba = Ta ’∗Tb; %eq (14)
tw i s t = trans2twist (Tba ) ; %eqs (20)–(24)
i f (abs (pr ( tw i s t )) <= 1 e−5)SbaInv = eye − dual ( 0 . 5 , 0 ) ∗ sk ( tw i s t ) ; %eq (43)
else
alpha = modulus( tw i s t ) ;i f (pr ( alpha ) == pi )
error ( ’ Not i n v e s t i g a t e d f o r r o t a t i o n s o f p i , s o r ry . ’ ) ;else %use eq (44)
u = tw i s t / alpha ; usk = sk (u ) ;SbaInv = eye − ( alpha /dual ( 2 , 0 ) ) ∗ usk . . .
+ (dual ( 1 , 0 ) − ( alpha ∗(dual (1 ,0)+ cos ( alpha ) ) ) / . . .(dual ( 2 , 0 )∗ sin ( alpha ) ) )∗ usk∗usk ;
end
end
%Compute e l a s t i c wrench in body coo rd ina t e sGam ba = [pr ( SbaInv ) , du( SbaInv ) ; zeros ( 3 , 3 ) , pr ( SbaInv ) ] ; %eq (46)
we b = Gam ba∗K∗ [pr ( tw i s t ) ; du( tw i s t ) ] ; %eq (49)
%Compute v i s c ou s wrench in body coo rd ina t e sSabInv = SbaInv ’ ; %eq (60)Gam ab = [pr ( SabInv ) , du( SabInv ) ; zeros ( 3 , 3 ) , pr ( SabInv ) ] ; %eq (59)
dtw = Gam ba ’ ∗ [ omb ; vb ] − Gam ab ’ ∗ [ oma ; va ] ; %eq (58)
wv b = Gam ba∗B∗dtw ; %eq (65)
%V i s c o e l a s t i c wrench in body coo rd ina t e sw b = we b + wv b ;
%Convert to g l oba l , i n e r t i a l c oo rd ina t e stemp = [Rb , sk (pb)∗Rb ; zeros ( 3 , 3 ) , Rb]∗w b ; %eqs (50) and (66) , moment f i r s t , f o r c e secondw0 = [ temp ( 4 : 6 ) ; temp ( 1 : 3 ) ] ; %Swap , re turn f o r c e f i r s t , moment second .
@dual/trans2quat.m
function q = trans2quat (T)%TRANS2QUAT(T) r e tu rn s a dual quatern ion cor re spond ing to dual%trans fo rmat ion matrix T in the form o f a 4 x1 dual matrix .
R = pr (T) ; %eq (32)
44
psk = du(T)∗R ’ ; p = sk inv ( psk ) ; %eq (33)
%Primary s c a l a r and vec to r par t ssigma = 0.5∗ sqrt ( trace (R)+1) ; %eq (34)
i f ( sigma < 0)error ( ’ This a lgor i thm assumes that sq r t r e tu rn s a non−negat ive square root . ’ )
e l s e i f ( sigma > 0 . 1 )x i sk = 0.5∗ as (R)/ sigma ; x i = sk inv ( x i sk ) ; %eq (35)
else %(sigma <= 0.1) %eq (36)M = 0.5∗ sy (R) + 0.25∗ (1 .0− trace (R) )∗ eye ( 3 ) ;x i1 = sqrt (M( 1 , 1 ) ) ; x i2 = sqrt (M( 2 , 2 ) ) ; x i3 = sqrt (M( 3 , 3 ) ) ;i f ( x i3 > 0)
i f (M(1 , 3 )<0 ) x i1 = − x i1 ; end
i f (M(2 , 3 )<0 ) x i2 = − x i2 ; end
e l s e i f ( x i2 > 0)i f (M(1 , 2 )<0 ) x i1 = − x i1 ; end
i f (M(3 , 2 )<0 ) x i3 = − x i3 ; end
e l s e i f ( x i1 > 0)i f (M(2 , 1 )<0 ) x i2 = − x i2 ; end %Shouldn ’ t get herei f (M(3 , 1 )<0 ) x i3 = − x i3 ; end %Shouldn ’ t get here
else
error ( ’ Shouldn ’ ’ t get here . Algorithm only works f o r very s p e c i a l case . ’ ) ;end
x i = [ x i1 ; x i2 ; x i3 ] ; x i s k = sk ( x i ) ;end
%Dual s c a l a r and vec to r par t ss = −0.5∗ x i ’∗p ; %eq (37)x = 0 .5∗ ( sigma∗eye(3)− x i sk )∗p ; %eq (38)
q = dual ( [ sigma ; x i ] , [ s ; x ] ) ;
comply quat.m
function w0=comply quat ( pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B)%COMPLYQUAT(pa ,Ra , pb ,Rb ,K, va , oma , vb , omb ,B) computes a v i s c o e l a s t i c wrench%in g l oba l coo rd ina t e s g iven compliance frame va r i a b l e s pa ,Ra , pb and Rb ;%given 6 x6 s t i f f n e s s matrix K = [Ko Kc ; Kc ’ Kt ] ; g iven l i n e a r and angular%v e l o c i t i e s va , oma , vb and omb ; and given 6 x6 damping c o e f f i c i e n t matrix%B = [Bo Bc ; Bc ’ Bt ] . The wrench i s computed us ing the ” quatern ion−based”%method . The wrench returned , w0 = [ f0 ; m0 ] , i s the wrench exer ted% by body B on the compliant , v i s c o e l a s t i c element . This i s a l s o the%wrench exer ted by the compliant element on body A.%%arguments% pa : p o s i t i o n o f compliant j o i n t frame o f body A (3 x1 )% pb : p o s i t i o n o f compliant j o i n t frame o f body B (3 x1 )% Ra : j o i n t frame o r i e n t a t i o n matrix o f body A (3 x3 )% Rb : j o i n t frame o r i e n t a t i o n matrix o f body B (3 x3 )% K : S t i f f n e s s matrix at a/b ( 6 x6 )% va : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in% coo rd ina t e s o f that frame .% oma : angular v e l o c i t y o f compliant j o i n t frame o f body A (3 x1 ) in% coo rd ina t e s o f that frame .% vb : l i n e a r v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in% coo rd ina t e s o f that frame .% omb : angular v e l o c i t y o f compliant j o i n t frame o f body B (3 x1 ) in% coo rd ina t e s o f that frame .
45
% B : Damping matrix at a/b ( 6 x6 )
%Computation o f c o n s t i t u t i v e equat ions us ing dual quatern ion−based method .eye = dual (eye ( 3 ) , zeros ( 3 , 3 ) ) ; %de f i n e dual i d e n t i t y matrix
%Compute dual t rans fo rmat ion matr i ce s , i n c l ud ing the disp lacementTa = dual (Ra , sk ( pa )∗Ra ) ;Tb = dual (Rb , sk (pb)∗Rb) ;Tba = Ta ’∗Tb; %eq (14)
qba = trans2quat (Tba ) ; %eqs (32)–(38)
sba = qba ( 1 ) ; xba = qba ( 2 : 4 ) ;
%Compute e l a s t i c wrench in body coo rd ina t e stmp = sba∗ eye − sk ( xba ) ;Lam ba = [pr (tmp ) , du(tmp ) ; zeros ( 3 , 3 ) , pr (tmp ) ] ; %eq (85)
we b = 2∗Lam ba∗K∗ [pr ( xba ) ; du( xba ) ] ; %eq (87)
%Compute v i s c ou s wrench in body coo rd ina t e stmp = sba∗ eye + sk ( xba ) ;Lam ab = [pr (tmp ) , du(tmp ) ; zeros ( 3 , 3 ) , pr (tmp ) ] ; %eq (97)
dx = 0.5∗Lam ba ’ ∗ [ omb ; vb ] − 0 . 5∗Lam ab ’ ∗ [ oma ; va ] ; %eq (96)
wv b = 2∗Lam ba∗B∗dx ; %eq (102)
%V i s c o e l a s t i c wrench in body coo rd ina t e sw b = we b + wv b ;
%Convert to g l oba l , i n e r t i a l c oo rd ina t e stemp = [Rb , sk (pb)∗Rb ; zeros ( 3 , 3 ) , Rb]∗w b ; %eqs (88) and (103) , moment f i r s t , f o r c e secondw0 = [ temp ( 4 : 6 ) ; temp ( 1 : 3 ) ] ; %Swap , re turn f o r c e f i r s t , moment second .
@dual/acos.m
function a = acos ( c a lpha )%ACOS(C) Arc−co s i n e o f dual number Calpha = acos ( c a lpha . p ) ;d = − c a lpha . d/( sin ( alpha ) ) ;a = dual ( alpha , d ) ;
@dual/as.m
function usk = as (T)%AS(T) r e tu rn s the ant i−symmetric ( skew−symmetric ) part o f dual matrix T.usk = dual ( 0 . 5 , 0 ) ∗ (T−T ’ ) ;
as.m
function usk = as (A)%AS(A) r e tu rn s the ant i−symmetric ( skew−symmetric ) part o f matrix A.usk = 0 .5∗ (A−A’ ) ;
46
@dual/cos.m
function cos dn = cos (dn)%COS(DN) i s the s i n e o f dual−number DN.cos dn = dual ( cos (dn . p),−dn . d∗ sin (dn . p ) ) ;
@dual/display.m
function d i sp l ay (dn)%DISPLAY Command window d i sp l ay o f dual number .[m, n ] = s ize (dn ) ;var = inputname ( 1 ) ;i f isempty ( var )
fpr intf ( ’\ nans = \n ’ ) ;else
fprintf ( ’\n%s = \n ’ , var ) ;end
i f isempty (dn ) % i s dn an empty dual−number?fpr intf ( ’\n [ empty dual−number ]\n\n ’ )
e l s e i f ( (m> 4 ) | ( n>4))error ( ’ Too big to d i sp l ay , can only handle a r rays up to 4x4 . ’ )
else
for i = 1 :mfpr intf ( ’\n ’ ) ;for j = 1 : n
i f (dn( i , j ) . p >= 0)i f (dn( i , j ) . d >= 0)
fpr intf ( ’ %.4 f+d%.4 f ’ , dn ( i , j ) . p , dn ( i , j ) . d ) ;else
fprintf ( ’ %.4 f−d%.4 f ’ , dn ( i , j ) . p,−dn( i , j ) . d ) ;end
else
i f (dn( i , j ) . d >= 0)fpr intf ( ’%.4 f+d%.4 f ’ , dn ( i , j ) . p , dn( i , j ) . d ) ;
else
fprintf ( ’%.4 f−d%.4 f ’ , dn ( i , j ) . p,−dn( i , j ) . d ) ;end
end
end
end
fprintf ( ’\n ’ ) ;end
@dual/du.m
function d lp r t = du(dn)%DU(DN) re tu rn s the dual part o f a dual matrix DN[m, n ] = s ize (dn ) ;for i =1:m
for j =1:nd lp r t ( i , j ) = dn( i , j ) . d ;
end
end
@dual/dual.m
47
function dual number = dual (p , d)%DUAL cons t ruc to r f o r dual−number ob j e c t s% DN=DUAL(P,D) c r e a t e s a dual number ( or vec to r ) with primary part P% and dual part D.i f ( nargin == 0)
dn . p = 0 ;dn . d = 0 ;dn = class (dn , ’ dual ’ ) ;
e l s e i f ( nargin == 2)[mp, np ] = s ize (p ) ;[md, nd ] = s ize (d ) ;i f ( (mp ˜= md) | ( np ˜= nd ) )
error ( ’ Dimensions o f arguments must be the same . ’ )else
for i =1:mpfor j =1:np
dn( i , j ) . p = p( i , j ) ;dn ( i , j ) . d = d( i , j ) ;
end
end
dn = class (dn , ’ dual ’ ) ;end
else
error ( ’ Must be i n i t i a l i z e d with r e a l and dual par t s , two arguments ’ ) ;end
dual number = dn ;
@dual/eq.m
function eqAB = eq (A,B)%EQ(A,B) determines whether or not two dual matr i ce s are equal[ma, na ] = s ize (A) ;[mb, nb ] = s ize (B) ;i f ( (ma ˜= mb) | ( na ˜= nb ) )
error ( ’ Matr ices must have same dimensions to be compared . ’ )else
for i = 1 :mafor j = 1 : na
i f ( (A( i , j ) . p==B( i , j ) . p ) & (A( i , j ) . d==B( i , j ) . d ) )eqAB( i , j ) = 1 ;
else
eqAB( i , j ) = 0 ;end
end
end
end
@dual/minus.m
function dn = minus( dn1 , dn2 )%MINUS Subtract two dual numbers[m, n ] = s ize ( dn1 ) ;for i =1:m
for j =1:ndn( i , j ) = dual ( dn1 ( i , j ) . p−dn2 ( i , j ) . p , dn1 ( i , j ) . d−dn2 ( i , j ) . d ) ;
end
end
48
@dual/modulus.m
function moddn = modulus(dn)%MODULUS(DN) re tu rn s the modulus o f dual vec to r DN (mx1 or 1 xn )%This i s a complex ( dual number ) modulus , not a s c a l a r . Perhaps t h i s%func t i on i s misnamed .[m, n ] = s ize (dn ) ;i f (m==1)
moddn = sqrt (dn∗dn ’ ) ;e l s e i f ( n==1)
moddn = sqrt (dn ’∗ dn ) ;else
error ( ’ Matrix must be mx1 or 1xn . ’ )end
@dual/plus.m
function dn = plus ( dn1 , dn2 )%PLUS Add two dual numbers[m, n ] = s ize ( dn1 ) ;for i =1:m
for j =1:ndn( i , j ) = dual ( dn1 ( i , j ) . p+dn2 ( i , j ) . p , dn1 ( i , j ) . d+dn2 ( i , j ) . d ) ;
end
end
@dual/pr.m
function prprt = pr (dn)%PR(DN) re tu rn s the primary part o f a dual matrix DN.[m, n ] = s ize (dn ) ;for i =1:m
for j =1:nprprt ( i , j ) = dn( i , j ) . p ;
end
end
@dual/sin.m
function s in dn = sin (dn)%SIN(DN) i s the s i n e o f dual−number DN.s in dn = dual ( sin (dn . p ) , dn . d∗cos (dn . p ) ) ;
@dual/sk.m
function dn sk = sk (dn)%SK(DN) Cross−product matrix o f an array o f dual numbers[m, n ] = s ize (dn ) ;i f ( (m==3)&(n==1))
dn sk ( 1 , 1 ) = dual ( 0 , 0 ) ; dn sk (1 ,2) = −dn ( 3 ) ; dn sk ( 1 , 3 ) = dn ( 2 ) ;dn sk ( 2 , 1 ) = dn ( 3 ) ; dn sk ( 2 , 2 ) = dual ( 0 , 0 ) ; dn sk (2 ,3) = −dn ( 1 ) ;dn sk (3 ,1) = −dn ( 2 ) ; dn sk ( 3 , 2 ) = dn ( 1 ) ; dn sk ( 3 , 3 ) = dual ( 0 , 0 ) ;
else
error ( ’ Only de f ined f o r 3x1 array ’ ) ;end
49
@dual/sk inv.m
function dn = sk inv ( dn sk )%SK INV(DN SK) re tu rn s the vec to r a s s o c i a t ed with c r o s s−product matrix DN SK.% There i s no e r r o r−check ing . This assumes that DN SK i s 3 x3 and% antisymmetr ic .dn ( 1 , 1 ) = dn sk ( 3 , 2 ) ; dn ( 2 , 1 ) = dn sk ( 1 , 3 ) ; dn ( 3 , 1 ) = dn sk ( 2 , 1 ) ;
@dual/sqrt.m
function sqrtdn = sqrt (dn)%SQRT(DN) Square root o f dual number DN.i f ( dn . p == 0)
i f ( dn . d == 0)sqrtdn = dual ( 0 , 0 ) ;
else
error ( ’ Square root o f pure ly dual number i s undef ined . ’ )end
else
sqrtdn = dual ( sqrt (dn . p ) , dn . d/(2∗ sqrt (dn . p ) ) ) ;end
@dual/sy.m
function syT = sy (T)%SY(T) r e tu rn s the symmetric part o f dual matrix T.syT = dual ( 0 . 5 , 0 ) ∗ (T+T ’ ) ;
sy.m
function sypart = sy (A)%SY(A) r e tu rn s the symmetric part o f matrix A.sypart = 0 .5∗ (A+A’ ) ;
@dual/trace.m
function t r = trace (A)%TRACE(A) The t r a c e o f dual matrix A.[m, n ] = s ize (A) ;i f (m ˜= n)
error ( ’ Argument must be a square matrix . ’ ) ;else
t r = dual ( 0 , 0 ) ;for i =1:m
t r = t r + A( i , i ) ;end
end
@dual/transpose.m
function dn T = transpose (dn)%TRANSPOSE of a matrix o f dual numbers[m, n ] = s ize (dn ) ;for i =1:m
50
for j =1:ndn T( j , i ) = dn( i , j ) ;
end
end
@dual/uminus.m
function min dn = uminus(dn)%UMINUS(DN) re tu rn s the unary minus o f dual number DN.[m, n ] = s ize (dn ) ;for i =1:m
for j =1:nmin dn ( i , j ) = dual(−dn( i , j ) . p,−dn( i , j ) . d ) ;
end
end
@dual/vers.m
function vers dn = vers (dn)%VERS(DN) i s the v e r s i n e o f dual−number DN.vers dn = dual(1−cos (dn . p ) , dn1 . d∗ sin (dn . p ) ) ;
51
