DIRECT AND INVERSE KINEMATICS
OF A
HIGHLY PARALLEL MANIPULATOR
Eric van Walsurn
B.Sc. (University of New Brunswick), 1989
Deparunent of Mechanical Engifieering
McGill Universiry
montrea al, Québec, Canada
A thesis submitted to the Faculty of Graduate Studies and Research
in partial fulfillrnent of the requirements for the degree of
Master of Engineering
January 27, 1997
O Eric van Walsum
National Library l*l of Canada Bibliothèque nationale du Canada
Acquisitions and Acquisitions et Bibliographie Services services bibliographiques
395 Wellington Street 395, rue Wellington OttawaON K1AON4 Ottawa ON K I A 0134 Canada Canada
The author has granted a non- exclusive licence allowing the National Library of Canada to reproduce, loan, distribute or seil copies of ths thesis in mirroform, paper or electronic formats.
The author retains ownership of the copyright in this thesis. Neither the thesis nor substantial extracts fiom it may be printed or othenvise reproduced without the author's permission.
L'auteur a accordé une licence non exclusive permettant a la Bibliothèque nationale du Canada de reproduire, prêter, distribuer ou vendre des copies de cette thèse sous la foxme de microfiche/nlm, de reproduction sur papier ou sur format électronique.
L'auteur conserve la propriété du droit d'auteur qui protège cette thèse. Ni la thèse ni des extraits substantiels de celle-ci ne doivent être imprimés ou autrement reproduits sans son autorisation.
Abstract
A model of a parailel rnechanism called the Double Tetrahedron was constructed. The
model displayed more mobility than predicted. The increased mobility of the mode1 was
attributed to extra rotary joints inadvenently created by the design of the mechanism.
The mobility and workspace of this newly created mechanism. dubbed Tetradon. are
examined qualitatively. The movement of Tetradon is described by the rigid body rotation
and translation of one tetrahedron relative to another. A method to calculate Tetradon's
joint coordinates based on this rotation and translation is presented. The underconstrained
solution for Tetradon and the constrained solution for a modified version of Tetradon are
given.
With a view toward applying Tetradon as a positioning mechanism. three different ways
in which to calculate the direct kinematics of Tetradon are presented.
Complementing the direct kinematic solutions, a solution to the inverse kinematics of
Tetradon is presented. A Newton-Gauss approximation scheme is applied to a set of
constrained objective functions. The objective functions are used to maintain the
intersection of the six edge pairs in the mechanism.
Résumé
Nous avons construit un modèle d'un mécanisme parallèle appelé Tétraèdre Double.
lequel démontra plus de mobilité que prévu. Cette mobilité accrue est attribuable à la
formation accidentelle d'articulations rotatives supplémentaires dues à la conception du
mécanisme.
Nous procédons à une analyse qualitative de la mobilité et de l'espace de travail de ce
nouveau mécanisme, nommé Tétradon. Le mouvement de Tétradon est décrit par la
rotation et translation du corps rigide d'un tétraèdre par rappon à un autre. Nous
présentons une méthode de calcul des coordonnées des articulations du Tétradon basée
sur cette rotation et translation. Une solution au problème non contraint pour Tétradon et
une autre au problème contraint pour la version modifiée sont exposées. Avec en vue
l'emploi de Tétradon comme mécanisme de positionnement, trois manières différentes de
calculer la cinématique directe de Tétradon sont proposées.
En complément de solution pour la cinématique directe, nous présentons une solution
pour la cinématique indirecte de Tétradon. Un système d'approximation de Newton-
Gauss est appliqué à un ensemble de fonctions cibles avec contrainte. Les fonctions
cibles sont utilisées pour maintenir l'intersection entre les six paires de la bordure du
mécanisme.
Acknowledgments
This work would not have been possible without the long endunng help and guidance of
Prof. Paul Zsombor-Murray and the ever-present administrative guidance of Barbara
Whiston. Thanks to NSERC for their part in fùnding the construction of the mode1 of the
Double Tetrahedron.
Thanks also to Leanne Brekke and Taylor Industrial Software for providing access to
hardware, software and resources. to Trevor Munk and Haydee Jalinski for their input and
editing and to Mom and Dad for providing the house and the financial support.
The support and humorous perspective always available h m Magda and Geoff
throughout this project were greatly appreciated. Thanks to Geoff for naming Tetradon
and to Magda for a place to crash and watch Star Trek when I was supposed to be in
class.
The love. patience, unswerving support and undeserved sacrifice given to me by my wife
Rhonda will never be forgotten and cannot be repaid. 1 love you Rhon.
Claim of Originality
The mechanism dubbed Tetradon is introduced here for the first time. In addition to the
introduction of the mechanism itself. the following contributions have been made in this
work:
The solution of the joint positions for Tetradon, given the rotation and translation of
its mobile tetrahedron.
A solution to the direct kinematics of the Double Tetrahedron.
Three approaches to the direct kinematics of Tetradon.
The application of objective functions and a weighting matrix to the inverse
kinematics of Tetradon.
Dedicated to:
Contents
.............................................................................................................................. Abstract
Résumé .................................................................................................................................
.............................................................................................................. Acknowledgements
............................................................................................. ............ Claim of Originality ..
List of Figures .....................................................................................................................
List of Tables .......................................................................................................................
List of S ym bols ....................................................................................................................
1 Introduction ..................................................................................................................... 1 .1 Background ............................................................................................................... 1.2 Goals .......................................................................................................................
iii
xiii
xvi
4 Direct Kinematics ...........................................................................................................
4.1 The Direct Kinematics of the DoubIe Tetrahedron ..................................................
................................................................. ...................... 4.1.1 Locating a Face ,..
...................................................................................................... 4.1.2 Finding Q4
4.1.3 Determining the Rotation and Translation ...................................... .. ..............
4.1.4 Example Kinematics for the Double Tetrahedron ...........................................
......................................................................... 4.2 The Direct Kinematics of Tetradon
4.2.1 Configuration of Tetradon ..............................................................................
......................................................................................... 4.2.2 T and Q Parameters
4.2.3 T and t Parame ters ...........................................................................................
4.2.5 T Parameters On1 y ...........................................................................................
.......................................................................................................... 5 Inverse Kinematics
.................................................................................... 5.1 Description of the Procedure
5.1.1 Variables .........................................................................................................
....................................................................................................... 5.1.2 Constraints
........................................................................................ 5.1.3 Objective Functions
.......................................................................................................... 5.1.4 Jacobians
........................................................................ 5.2 Applying the Procedure to Tetradon
5.2.1 Applied Constraints .......................................................................................
5.2.2 Applied Objective Functions ...........................................................................
5.2.3 Generating the Jacobian Matrices ....................................................................
5.2.3.1 Jacobian of Constraint Equations .................................................................
................................................................... 5.2.3.2 Jacobian of Objective Functions
..................................................................................................... 5.3 Example Solution
6 Conclusion .......................................................................................................................
6.1 Results ......................................................................................................................
................................................................................................. 6.1.1 Joint Positions
6.1.2 Direct Kinematics .................................... .. ...................................................
6.1.3 Inverse Kinematics ..........................................................................................
6.2 Contributions ............................................................................................................
................................................................................ 6.3 Opponunities for Further Study
References .............................................................................................................................
Appendices
A . The Mode1 of the Double Tetrahedron ~~e.....................................................................
B . Parametric Eguations for Joint Values ........................................................................
B . 1 Intersection of Edges P2P4 and QlQ3 .......................................................................
B . 1.1 Basic Position Coordinates and Vectors ................................. .. ...................
B . 1.2 Rotated Vectors ...............................................................................................
B . 1.3 Solution ...........................................................................................................
B.2 Intersection of Edges PIPJ and Q2Q3 .......................................................................
B.2.1 Basic Position Coordinates and Vectors .........................................................
B.2.2 Rotated Vectors ...............................................................................................
B.2.3 Solution ...........................................................................................................
B.3 Intersection of Edges PlP t and Q3Q4 .......................................................................
B.3.1 Basic Position Coordinates and Vectors ..................................... ... ................
B.3.2 Rotated Vectors ...............................................................................................
B.3.3 Solution ...........................................................................................................
..................................................................... B.4 Intersection of Edges P2P3 and QI Q4
...................... B.4.1 Basic Position Coordinates and Vectors ......................... ,..
..................... ........................................................ B.4.2 Rotated Vectors .. ............
B.4.3 Solution ...........................................................................................................
B -5 Intersection of Edges P3P4 and Qi Q2 ............................................... .................
B.5.1 Basic Position Coordinates and Vectors .........................................................
B.S.2 Rotated Vectors ...............................................................................................
B.5.3 SoIution ...........................................................................................................
B.6 Intersection of Edges P l Pl and Q2Q4 .......................................................................
......................................................... B.6.1 Basic Position Coordinates and Vectors
B.6.S Rotated Vectors ...............................................................................................
........................................................................................................... B.6.3 Solution
C Inverse Kinematic Equations and Their Jacobians .....................................................
C . 1 Cornrnon Components .............................................................................................
C . 1.1 Joint Positions on Q ........................................................................................
C . 1.2 Derivatives of the Transformation Matrix ......................................................
C.1.3 Derivatives of the Joint Positions on Q ..........................................................
C.2 Constraints ...............................................................................................................
C.2.1 Constra.int Equations ........................................ C.2.2 Jacobian of Constraint Equations ...................................................................
...................................................................... C.3 Objective Functions .................... .. C.3.1 Distance Between P2P4 and QVlQ; ..............................................................
C.3.2 Distance Between P I PA and QiQ; .................................................................
C.3.2 Distance Between P l Pz and Q'Q; ........................... ... ......................... C.3.2 Distance Between P2P3 and QflQ; .................................................................
C.3.2 Distance Between P3P4 and QtlQf2 .................................................................
C.3.2 Distance Between P& and QPz& .................................................................
List of Figures
1.1 The Double Tetrahedron .................................................................................................
1.2 The mode1 of the Double Tetrahedron ............................................................................
1.3 Tetradon in the basic position .........................................................................................
1.4 Edge dimensions for Tetradon ......................................................................................
1.5 Tetradon rotated for presentation ....................................................................................
2.1 Mapping of 0 and (I parameters ......................................................................................
2.2 Workspace of the DT expressed in 0 and 4 parameters .................................................
2.3 XY type-IV motion of the DT ....................... ..... .. .. .................................................
2.4 YZ type-N motion of the DT ........................................................................................
2.5 ZX type-IV motion of the DT ........................... .... ................................................. 2.6 Joint parameters of the DT .............................................................................................
2.7 Joint parameters on tetradon ...........................................................................................
2.8 Tetradon's workspace with a large inter-edge distance ................................................
2.9 Modified workspace of Tetradon ....................................................................................
3.1 Four parameters in a cylinder-cylinder intersection ..................................................
3.2 Locus of points on an edge pair intersection ..................................................................
.............................................................................................. 3.3 Parameters at an edge pair
.............................................................................. 3.4 Intersection of edges P2P4 and QIQ3
.. .................................................................... 3.5 Q3 rotated and translated to [10.5. 13 141
3.6 Detail of PI Pa x Q2Q3 and P I P x Q?QJ edge intersections ...........................................
3.7 Two possible solutions for the consuained equations ....................................................
* ............................................................. 3 -8 Second configuration with Q3 at [ 10.5. 13 - 141
3.9 Second detail of P l PJ x Q7Q3 and P l P 3 x Q2Q4 .............................................................
4.1 Planar double triangular manipulator .............................................................................
.............................................................. 4.2 Positioning a triangle using three given points
4.3 Positioning triangie QiQzQ3 ...........................................................................................
4.4 Locating a fourth vertex of the Q teuahedron ................................................................
4.5 First rotation - Q3 to Q "3 ................................................................................................
4.6 Second rotation - Q'? to Q " 2 ...........................................................................................
4.7 Two possible locations of QI Q2Q3 .................................................................................
4.8 Locating points Q5, e6 and Q7 .......................................................................................
4.9 Known triangle Q5Q6Q7 touching three known circles ..................................................
4.10 Partial faces on Q touching three known circles ..........................................................
5.1 Parameters used to calculate the distance between edges ...............................................
5.2 Trajectory of Q3 ..............................................................................................................
5.3 o values dong trajectory ................................................................................................
5.4 Cl values dong trajectory ................................................................................................
5.5 r values almg trajectory ................................................................................................
5.6 T values dong trajectory .................................................................................................
A . 1 The top of the slider .......................................................................................................
A.2 The bottom of the slider .................................................................................................
A.3 Steel plates for the thmst bearing ..................................................................................
A.4 The end spider ................................................................................................................ A S The end hinge .................................................................................................................
A.6 Bearing cap for end hinge ..............................................................................................
A.7 Connecting ann for end hinge ........................ .. ..........................................................
A.8 Assembly of dl pieces ...................................................................................................
............................................................................................................ A.9 Assembled slider
A . 10 Assembled end spider and hinges ................................................................................
A . 1 1 Fully assembled mode1 ................................................................................................
xiv
List of Tables
................ ............................. 3.1 Joint parameten for the underconstrained equations ... 34
3.2 Joint parameters for the constrained equations ............................................................... 39
List of Symbols
The magnitude of the rotation of the Q tetrahedron
An angle used to measure the orientation of the axis of rotation of the Q tetrahedron
Linear displacement dong an edge on the Q tetrahedron
Linear displacement dong an edge on the P tetrahedron
Rotation about and edge on the Q tetrahedron
Rotation about and edge on the P tetrahedron
The distance between two edges in an edge pair
X component of the Linear Displacement of the Q Tetrahedron
Y component of the Linear Displacement of the Q Tetrahedron
Z component of the Linear Displacement of the Q Tetrahedron
A vertex on the P tetrahedron
A vertex on the Q tetrahedron
A unit vector parallel to the axis of rotation of the Q tetrahedron
Vector of objective functions
Vector of constraint equations
A unit vector parailel to an edge on the Q tetrahedron
A unit vector parallel to an edge on the P tetrahedron
A unit vector mutually perpendicular to two edges in an edge pair.
Vector of design variables
Jacobian of Objective Functions
Jacobian of Constraint Equations
The Fixed Tetrahedron
The Mobile Te trahedron
xvi
Chapter 1
Introduction
Six serial chains connecting two rigid bodies can form an overdetennined and highly
redundant six degree of freedom mechanism. In this case, each serial chah has seven
lower pairs. referred to here as 7-DOF chains. The workspace and kinematics of a very
special configuration of such a device are described in detail in this work.
1.1 Background
The device studied in this work evolved from a mechanism known as the Double
Tetrahedron (DT). The simplest form of the DT is a line figure formed by joining al1
iwelve face diagonals of a cube. The two identical equilateral tetrahedra so formed
intersect each other at six points. See Fig. 1.1 below.
Note that the DT is an overconstrained mechanism with a topologically predicted
according to the well known Chebychev-Kutzback-Gruhler relationship. This enigmatic
nature of the DT has been discussed at length in [6 .... 101 and [12]. Nevertheless, if five
degrees of freedom are allowed at each of the intersections, the tetrahedra form a
CHAPTER 1. Introduction
mechmism with a maximum of two degrees of freedom. Holding one tetrahedron in
place. the other can perforrn pure rotations. or combinations of rotations and translations.
The advantage of such a rnechanisrn is that it provides structurai stiffness and good
redundancy for control purposes. The drawback is that it is restricted to a 2-DOF motion.
where a point on the moving tetrahedron moves only on narrowly defined cuwed
surfaces. shown in detail in chapter two.
The mechanism studied here is formed when two equilateral tetrahedra - one slightly
smaller than the other - are joined by six 7-DOF kinematic chains. This mechanism.
dubbed Tetradon, has al1 of the advantages of the DT. However. it is also more mobile
than the DT. Tetradon can rnove freely within a three dimensional workspace. It c m aiso
take advantage of six degrees of freedom within a much more limited workspace.
Figure 1.1 The Double Tetrahedron
CHAPTER 1. Introduction
1.2 Goals
As a positioning mechanism. Tetradon is more mobile. hence more useful. than the
Double Tetraheda. To exploit it, an understanding of its direct and inverse kinematics is
needed. A principal goal of this work is to generate the joint paths needed to move a
vertex on the mobile tetrahedron to a predetermined point in space.
Another goal is to examine a different approach to the inverse kinernatics of such parallel
mechanisms. This approach is to uncouple the motion of the major components of the
mechanism from that of the joint parameters. Chen [4] used this approach with his
"Double Tetrabot". He started with a given target point. Then, he found the rotation and
translation needed to place the mobile tetrahedron at the required position. Only once this
was accomplished did he calculate the joint positions. A similar approach is used here.
The vertex is placed at its target. then the mobile tetrahedron is rotated to a position
which satisfies the constraints of joint integrity. Only then are the joint positions
calculated.
1.3 Strategy
The first step in developing the kinematics of Tetradon is to understand its mobility . The
function of its individual joints and the form of its workspace are both integral to this
understanding. Chapter 2 uses the DT and its characteristics as laid out by Chen [4] to
introduce the mobility and workspace of Tetradon.
Second, a means by which to calculate the joint positions of the Tetradon is needed. The
nature of Tetradon, and of its components, allows its kinematic solutions to rely heavily
on very simple geometric relationships. The propenies of triangles, circles and tetrahedra
are used throughout, as are the formulations for the intersection of cylinders and the
distance between skew lines in space. To calculate the joint positions, Tetradon is
CHAPTER 1. Introduction 4
represented as six pain of intersecting cylinders. Each cylinder intersection is defined
parametricaily. Using the scheme given by Zsornbor-Murray [L3], the joint posiuons for
Tetradon are found.
If Tetradon is configured with seven joints at each edge intersection. the parametnc
equations for each cyiindedcylinder intersection are underconstrained. Consequenrly.
there will be a real continuum of possible solutions for the joint positions at each edge
intersection. A simple means to deal with this issue is suggested. An alternative
configuration of Tetradon reduces the number of joints at each edge intersection to six.
while maintaining Teuadon's mobility. This configuration, in mm, creates a consuained
set of equations which will yield a maximum of two possible solutions for the joint
positions at each edge intersection.
Three approaches to the direct kinematics are given. Each uses a different set of joint
variables to find the position and orientation of the mobile tetrahedron. In each case, the
solution is based on simple geometry. The first starts off with a known position of one
face of the mobile tetrahedron. Knowing the location of this face gives the location of the
rest of the tetrahedron. The second approach requires locating the vertices of a triangle of
known dimensions in three known circles. Once found. this triangle is used to locate a
face of the mobile tetrahedron and from there the whole tetrahedron. Finally. an approach
is given where only the positions of six circles are known. Among these six circles. six
uiangles must exist for Tetradon's constraints to be satisfied. These six triangles must
relate to each other in a specific way. This relationship defines the constraints to be
placed on a senes of six nonlinear equations in six unknowns.
The inverse kinematics are solved independently of the direct kinematics and of the joint
positions. The solution finds a position and orientation of the mobile tetrahedron that will
satisfj a given condition. Once the mobile tetrahedron has been so located. the joint
position solution is used to calculate the actual joint values needed to position it at that
location. Locating the mobile tetrahedron is accomplished by exploiting the objective
CHAPTER 1. Introduction 5
c functions in a Newton-Gauss least-square solution to a set of underconstrained nonlinear
equations.
1.4 Historical Sketch
The DT has been studied from a theoreticai perspective by a few researchers over the past
several years. The work to date has focused principally on proving that the DT actually
forms a mechanism (Tarnai and Makai [8.9,10], Stachel[7]), descnbing its particular
types of motion. and devising solutions for its inverse kinematics (Hyder and Zsombor-
Murray [6] , Zsombor-Murray and Hyder [12], Chen [4]).
Models were made to study the motion of the mechanism. The most advanced of these
was created by Hyder and Zsombor-Murray [6] . Cornputer animations of the tetrahedra
were also devised; a simple Iine-representation by Hyder and Zsombor-Murray [6] and a
more involved solids-based mode1 by Zsombor-Murray, Bulca and Angeles [ 1 1 1.
Chen 141 devised a ciosed-form solution for the joint coordinates associated with the DT.
In this work. one tetrahedron is seen as a fixed base. while the other is a moving end
effector. Al1 joint displacements are expressed in rems of rwo variables which
completely define the rotation and translation of the mobile tetrahedron. The axes of
rotation lie in three munially perpendicular planes. Each branch of the workspace is
idenUfied by the plane in which its rotation axis lies. The first variable. 8. defines the
orientation of the axis of rotation in one of these planes. The second, @, is the angle of
that rotation. The mobile tetrahedron's translation is a funetion of these two variables.
Chen [4] worked out the closed-form solution for the X. Y and Z components of this
displacement as functions of 8 and $. Individual solutions were derived for all three
branches of the device's motion.
Chen [4] also proposed the use of the DT as a pointing device or, with the addition of a
linear actuator. as a three dimensional positioning device.
CHAPTER 1. hcroduction
1.5 Tetradon
The concept for the more mobile Tetradon grew out of what was meant to be an improved
implernentation of the DT. The model was designed to emulate the DT in every way:
edges of each tetrahedron are of identical length and since the edges need only remain in
intersection. five degrees of freedom exist between edges. Five rolling element bearing
separate the hardened steel rods which make up the tetrahedron edges. These bearings
make the model rnove very smoothiy. A photograph of the mode1 is shown in Fig. 1.2.
The N 1 details of its design are given in appendix A.
Figure 1.2 The model of the Double Tetrahedron
CHAPTER 1. Introduction
c Despite the effort to emulate the M exactly, the physical mode1 does not behave as
intended. The predicted motions of the DT c m be readily d~~1icated.l However. the
mode1 can move in other ways not predicted for the DT. The mobile tetrahedron is able to
move about in a three DOF workspace. though it is still constrained by the requirements
of the six edge intersections. This work describes how this added rnobility came about.
and how to calculate Tetradon's joint positions within its workspace.
1.5.1 A Description of the Mechanism
The mechanism. as it is studied here, is a simplification of the original model. in the basic
position, Tetradon's two tetrahedra are centered on the origin and aligned with the
principal axes of a right-handed three dimensional coordinate system. One tetrahedron.
labeled P. remains fixed and is larger than the smaller. mobile Q tetrahedron. See Fig.
1.3.
A prismatic joint rides dong each edge of the tetrahedra. Mounted on this prismatic joint
is another joint able to rotate around the axis dong which the prismatic slides, therby
forming a cylindrical pair. From each of these cylindrical pairs, a short connecting rod
extends. The connecting rods from an edge on the P tetrahedron and from an edge on the
Q tetrahedron will meet in a sphericat joint, shown in detail in Fig. 1.4 below.
1.5.2 Dimensions
The dimensions of Tetradon were chosen to simpliS the calculations of its kinematics.
The difference in size between the P and Q tetrahedra was chosen specifically for this
purpose. However, as shown in Chapter 2, the relative dimensions of the two tetrahedra
can be changed. It is also shown that changing the relative sizes of the tetrahedra affects
Tetradon's mobility and workspace.
I It was. in fact. the ease of the motion. combined with fieshly machined edges and some misplaced ringers
that gave Tetradon its carnivorous sounding name.
CHAPTER 1. Introduction
Figure 1.3 Tetradon in the basic position
In the conceptual mode1 of Tetradon used throughout this thesis. vertices on the P
tetrahedron are located about an origin of [0.0,0] at the following points:
Pi = [ 11,-11.-Il] P2 = [-11, 1 1 . 4 l ] P3 = [-11.-l 1 , 1 1 1 P . $ = [ l l , 1 1 , I I ]
92 +22- or 31.1 127 This means that al1 edges on the P tetrahedron have a length of d- units. Vertices on the Q tetrahedron are al1 located opposite to their counter parts on the P
tetrahedron, and at a shoner distance from the origin:
CHAPTER 1. Introduction
J'- The length of edges on the Q tetrahedron is 18 + 18 or 25.4558 units. The connecting
rods that join the edges together each have a length of 1 unit. See Fig. 1.4
Figure 1.4 Edge dimensions for Tetradon
By using this design, it was intended that the mode1 be equivalent to two identical vinual
tetrahedra with vertices at [t 10, t 10. t 1 O]. However. this is valid only if the sphencal
joint remains at the midpoint of the comrnon perpendicular of fixed length that separates
the real tetrahedron edges, Le. if there is no rotation about the real edges.
CHAPTER 1. Introduction
1.5.3 Naming Conventions
The iabeling convention onginally introduced by Tarnai and Makai [8.9.1 O] for the DT is
employed for Tetradon. Naming conventions for new parameters introduced by Chen [4]
are also adhered to.
Vertices on the two tetrahedra will be identified as Pi and Qj i = 1. ..,4: j = 1 .... 4. In
keeping with these previous conventions, joint variables associated directly with the
fixed, or P, tetrahedron will be capitaiized, while their affiliates on the mobile, or Q,
tetrahedron will be in lower case. Thus. the parameter T, denotes the linear translation -
dong the edge P, P, , while t,, denotes the linear translation dong the edge Q, Q, . The
practice used by Chen [4] of superscripting joint parameters with the type of motion that
they are related to will not be used, as there are no distinctions between motion types for
Te tradon.
Because they are used frequently in the ensuing calculations, the trigonometxic functions
cos@ and sin@ will be abbreviated as ce and s@. respectively.
For clarity. images of Tetradon. will often be presented so that vertex Q3. Tetradon's end
effector. is pointing straight down. while vertex P3 is pointing straight up. as shown in
Fig. 1.5 below. In this position, the space diagonal of the cube in which the tetrahedra are
inscribed is vertical.
CHAPTER 1. Introduction
*\ ;. : t : /
Fixed P Tetrahedron :' ! ! i l I
i ! 1 '. 1 i
. Mobile Q Tetrahedron '.
Figure 1.5 Tetradon rotated for presentation
Chapter 2
Mobility and Workspace
Tetradon is much more mobile than the DT because of the extra two joints at each e d p
pair intersection. These two extra joints release Tetradon from the rigid constraint placed
on the motions of the tetrahedra in the DT. However, the mobility of Tetradon and of the
DT are still very much related. Therefore, as an introduction to the mobility and
workspace of Tetradon, the rnobility and workspace of the DT will first be explained.
This is followed by an explanation for Tetradon's added mobility and two examples of
how Tetradon's configuration affects its workspace.
2.1 The Mobility of the Double Tetrahedron
Tarnai and Makai [8,9.10] and Stachel [7] descnbed the distinct types of motion of
which the DT is capable. These include a single DOF screw type motion and a range of
two-DOF rotations. Only one of these motions. dubbed a type-1 rotation, is actually a
pure rotation of the mobile tetrahedron. The other motions, dubbed types II and III. are
combined rotations and translations.
Chen [4] expanded on this work and derived the mobility equations for the DT. One
finding of this work was that the rotation and translation of the mobile tetrahedron can
be fully defined by only two parameters. Chen went on to define the workspace of the
Chapter 2. Mobility and Workspace 13
mobile tetrahedron with respect to these two parameters and to map the workspace of a
point on the mobile tetrahedron.
2.2 Mobility Equations
The constraint forced upon the DT is that each pair of intersecting edges must remain
coplanar. This constraint was expressed by Chen (41 mathematically in the form of six
constraint equations: one equation for each edge pair.
He went on to identib a generalized rotation matrix and a translation vector that could
be applied to the mobile tetrahedron. The rotation matrix was derived from an axis of
rotation. given by the unit vector e, and the angle of the rotation about that axis. Q.
Applying this rotation matrix to the mobile tetrahedron and forcing it to meet the
constra.int conditions of edge intersection, Chen derived the translation vector for the
mobile tetrahedron erpressed in terms of e and @.
The six coplanar equations that Chen started with will only be satisfied under the
following conditions:
or one of
The motion which satisfies the first of these conditions was labeled by Tamai and Makai
as a type-iII motion. This is a corkscrew motion where one vertex of the mobile
tetrahedron moves in a straight line as the tetrahedron itself rotates about - and translates
dong - the same axis. That axis extends from the origin through the vertex that is
moving in the straight line.
Chapter 2. Mobility and Workspace 14
Motions which satisfy the other conditions are the three branches of what Chen [JI
referred to as rnired angle motion. For each branch of this mixed angle motion. the unit
vector e is parallel to one of the principal planes. Chen used the parameter 0 to place the
axis of rotation. 8 is the angle tumed by the axis of rotation about the normal to the plane
in which it lies (see Fig. 2.1). In this work, these motions will be referred to simply as
type-IV motions.
Maximum Value of Q
Figure 2.1 Mapping of 0 and @ parameters
Type-III motions, because they act dong a fixed axis. can be fully defined by the angle
of rotation of the mobile tetrahedron. $. Other motions are defined by the values of 0 and
0 -
Chapter 2. Mobility and Workspace
2.3 Workspace of the Double Tetrahedron
The workspace of the DT consists of the four rays of the type-ID motion. and the
surfaces for each of the three branches of the other motions. The four rays of the type-III
motion traced by the vertices of the Q tetrahedron mn along [-10.10.10]. [IO.-
10. IO], [IO. 10.- 101 and [- 10,- 10.-101.
Chen represented the workspace of the type-IV motion by displaying 8 and in polar
coordinates in each of the principal planes (see Fig. 2.7). Al1 points within the three
pointed figures represent an allowable axis and angle of rotation. The translation of the
Q tetrahedron is calculated directly from values of 0 and $ at that position.
XY Mixcd Angle Motion
/
YZ Mixai Anglc Motion -
ZX Mixed Angle Motion
Figure 2.2 Workspace of the DT expressed in 0 and 0 parameters
The three branches of the type-IV motion are illustrated in Figures 2.3 to 3.5. These
figures show al1 of the possible locations of the vertex Q3 as it performs the type-IV
motion. Each plot of a workspace is shown with the DT in a position representative of
that branch of the type-IV motion. Also shown in each figure is the portion of the 0 @
Chapter 2. MobiIir): and Workspace
workspace that applies to that branch of the motion. Sote that these three workspaces
only intersect where the planes in Fie. 2.2 inrersect. Thar is. where the axis of rotation
lies dong one of the coordinate axes.
Figure 2.3 XY type-IV motion of the DT
Chapter 2. Mobility and Workspace
2.4 YZ type-N motion of the DT
Figure 2.5 M type-IV motion of the DT
Chapter 2. Mability and Workspace
2.4 Joint Parameters
For the DT to perform the motions illustrated above. there must be five degrees of
freedom acting at each edge intersection. These five degrees of freedom are represented
as a serial PRRRP linkage. The elements of this linkage are as follows: a translation
dong an edge on the P tetrahedron: a rotation about that sarne edge: a rotation of the P
edge relative to its correspondhg Q edge: a rotation about the Q edge and finally a
translation dong the Q edge. These joint parameters were labeled by Chen [4] as
follows:
7'' - the distance traveled dong an edge of the P tetrahedron. Tp - the rotation about an edge on the P tetrahedron. y - the angle between paired edges on the P and Q tetrahedra. rq - the rotation about an edge on the Q tetrahedron.
tq - the distance traveled dong an edge of the Q tetrahedron .
These joint parameters and how they act are shown in detail in Fig. 2.6.
--_ --- _ -- - -- _ _ P edge
. - Q edge
Figure 2.6 Joint parameters of the DT
Chapter 2. Mobility and Workspace
2.5 Tetradon vs. the Double Tetrahedron
There are two fundamental differences between Tetradon and the DT. The first is that
Tetradon consists of tetrahedra of different sizes. This eliminates the restriction that pairs
of edges must intersect. The second difference is in the serial chah at the edge-pair
intersections. This senal chain contains seven degrees of freedom. The result of these
differences is that the motion of Tetradon is not subject to the same closed constraints as
those applied to the DT. Consequently, there are no mobiliry equations that can
completely define its motions and its workspace is much larger.
2.5.1 Joint Parameters
The serial link between edges on Tetradon contains seven joints in a PRRRRRP chain.
The extra joints are assigned the parameters R and o. Thus. for Tetradon. the order of
joint parameters is as follows. A prismatic joint uavels dong an edge on the fixed P
tetrahedron. A rotary axis acts about that e d g at the position of the linear actuator.
Attached to this rotary joint is a short connecting m. At the end of this connecting arm
is a sphencal joint where three rotational degrees of freedom act. Another short
connecting rod joins the spherical joint to an edge on the mobile Q tetrahedron. This
connecting rod is also attached to a rotary joint fixed to a linear actuator on the Q edge.
These parameters are labeled as follows:
Tp - the distance traveled dong edge P. S2 - the rotation about edge P Tp - the rotation about an axis that is parailel to edge P but passes through the
spherical joint that joins the connecting rods. Y - the rotation about the axis perpendicular to edges P and Q and passing
though the center of the spherical joint. Tq - the rotation about an axis that is parallel to edge Q but passes through the
spherical joint that joins the connecting rods. o - the rotation about edge Q lq - the distance traveled dong edge Q.
Chapter 2. Mobility and Workspace
Al1 of the previous joint parameters are illustratrd in Fig. 1.7.
Figure 2.7 Joint parameters on Tetradon
2.5.2 Tetradon in the Double Tetrahedron's Workspace
If the R and o angles on Tetradon are irnmobilized at zero degrees, Teetradon becornes a
DT mechanism. In this configuration. the two tetrahedra will represent two equally sized
tetrahedra intersecting through Tetradon's spherical joints. Furthemore, with these two
joints fixed, the joint order and the function of dl of the joints in Tetradon will be
identical to those in the DT.
However, because one of the tetrahedra is in fact smaller than the other, Tetradon cannot
duplicate the hiIl range of motion of the DT. The distance that the prismatic joints can
travel dong the Q edges will be lirnited by the length of those edges. The shorter the
edges are, the more restricted Tetradon will be.
Chapter 2. Mobility and Workspace
2.5.3 The Role of the R and w Joints
To release Tetradon from the two-dimensional workspace of the DT and allow it to
move about with its three degrees of positional freedom requires only the release of the
R and o joints. Allowing these joints to rotate creates the seven-DOF linkage between
edge pairs. More significantly. this is what eliminates the fundamental constraint on the
DT. namely that al1 edge pairs remain coplanar.
With the R and o angles active, the rigid constraint that edges must intersect is replaced
with the less restrictive requirement that edges remain within a certain distance of each
other. As long as each edge-pair remains within that distance. the seven-DOF linkage
will be able to fit between the two edges. This means that the Q tetrahedron in essence
floats about inside of the P tetrahedron. Its ability to make small compensations at the
joint intersections is what gives Tetradon its added mobility.
The effect of releasing the R and o joints is profound. Even if the distance between
edges is kept very small, Tetradon will still have a significant three-dimensiona!
workspace in which to rnove.
2.6 Tetradon's Workspace
The shape of Tetradon's workspace is determined by its configuration. A large
discrepancy in the sizes of the tetrahedra will limit the distance that the mobile
tetrahedron can travel. From the point of its greatest travel dong a type-III motion. the
vertex Q3 flares out quickly to rneet the bounds of the type-IV motion. This creates a
convex workspace. A smail difference in the sizes of the tetrahedra dlows the Q
tetrahedron to travel farther from the center. However, it must stay closer to the
workspace of the DT. This creates a concave workspace. Both of these workspaces are
illustrated below.
Chapter 2. Mobility and Workspace
2.6.1 Large Distance Between Edges
A large distance between edges restricts the motion of the linear actuators. creating a
small, rounded workspace. In this case. the two tetrahedra start at permutations of [k 1 1.
+ - I I , k 1 l ] for the P tetrahedron and [kg. 29, 291 for the Q tetrahedron. In this
configuration, the greatest travel dong the Q edges is 12.728 units from the midpoint of
the edge (4- ). This configuration limits the maximum distance that the vertex can
travel. However, it also provides for more freedom of movement of the Q tetrahedron
inside of the P tetrahedron. This gives the workspace a short and rounded appearance.
See Fig. 2.8.
Figure 2.8 Tetradon's workspace with a large inter-edge distance
Chapter 2. Mobility and Workspace
2.6.2 Smdl Distance Between Edges
Reducing the distance between edges makes Tetradon behave more like the DT. The Q
tetrahedron is not as free to move about withm the P tetrahedron. but there is more linear
travel ailowed dong the Q edges. This allows for a greater maximum distance traveled
along a type-III trajectory. and forces Tetradon to conform more closely to the workspace
of the DT. Figure 2.9 shows the workspace for Tetradon where the P venices are found
at permutations of [k 1 1. r 1 1. k 1 11 and the Q venices are found at permutations of
[f 10.9. t 10.9,110.9]. This allows for a maximum travel along the Q edges of 15.415
( Ji0.9' + 10.9' ) units from the midpoint of each edge on the Q tetrahedron.
Figure 2.9 Modified workspace of Tetradon
Chapter 2. Mobility and Workspace
2.6.3 Generation of Workspaces
The depictions of the workspace given here were generated using the inverse kinematics
described in chapter 5. The limits of the workspace were found by moving Tetradon
dong a set of trajectones emanating from a single point. When Tetradon had reached a
position where either the joint variables h d reached their maximum or a possible
position of the Q tetrahedron could no longer be found. it was assumed that the edge of
the workspace had been reached.
Chapter 3
Calculating Joint Positions
The edges on the Double Tetrahedra cross at simpie line intersections, each with only one
possible solution. Tetradon possesses extra rotary joints. Because of them. the edges on
the P and Q tetnhedra meet along cylinder intersections. These cylinder-cylinder
intersections have an infinite number of solutions, which complicates the calculation of
the joint variables considerably.
To simpiifj the calculation of the joint variables, the edge intersection c m be reduced to
the intersection of a line and a cylinder. This in turn reduces the number of possible
soIutions to two and in so doing simplifies the calculation of the joint variables.
3.1 Background
The calculation of Teuadon's joint variables requires some initial calculations to
determine the position of the Q tetrahedron. It also relies heavily on the solution of the
intersection of two cylinden. These two procedures will be explained first, followed by
their application to the calculation of Tetradon's joint positions.
Chapter 3. Calculating Joint Positions
3.1.1 Generalized Rotation and Translation
The solution of Tetradon's joint positions starts with the known positions of the fixed P
and the mobile Q tetrahedra. The position of the Q tetrahedron is given by a set of seven
rotation and translation parameters. These seven parameters. ex, eV, e,, @, X. Y and Z. are
the elements of a generalized rotation and translation matrix. This matnx is generated
from the seven parameters as follows:
Where. e a unit vector parallel to the axis of rotation. $J is the angle of rotation. k is equal
to I - cg and X, Y and Z are the components of the Q teuahedron's translation.
The elements of the transformation matrix to be applied to the Q tetrahedron, TQ . are
denoted as:
where the elements of the rotation portion of the matrix are al1 functions of e and $ as
described above.
Chapter 3. Calculating Joint Positions
< 3.1.2 Cylinder-Cylinder Intersections
Each tetrahedron edge is considered to be the axis of a cylinder. Tetradon's joint variables
are found through the solution of the intersection of cylinders on the P and Q tetrahedra.
The solution of the intersection of two cylinden folIows the procedure laid out by
Zsombor-Murray [Il]. Each cylinder is defined pararnetricaily. The definition s tms with
the known axis. base point and radius of a cylinder. The two parameters used to define
the cylinder's surface are the linear travel dong the axis. and the rotation of the radius
about the a i s . These two parameters are used in three equations to calculate the X. Y and
Z positions on the surface of the cylinder.
Two intersecting cylinders give three equations in four parameters. Fixing any pararneter
gives a quadratic solution for the other three parameters. Generally, for any value of one
parameter, there are two possible solutions for the other three. The pararnetric
representation of a cylinder-cylinder intersection is shown in Fig. 3.1.
_-- - -
---._ -+- - ..-'
Figure 3.1 Four parameters in a cylinder-cylinder intersection
Chapter 3. Calculating Joint Positions
3.2 Underconstrained Intersection Equations
In the case oPTetradon. the axes of the cylinders are the edges of the tetrahedra. The base
points are the vertices. The radii of the cylinders are the short connecting ams that join
edges on the P and Q teuahedra. The center of the sphencal joint berween the connecting
arms traces the surface of the cylinder. See Fig. 3.2.
Because al1 four joints are free to move. the set of possible joint positions at each edge
intersection is infinite. It includes al1 points on the cylinder-cylinder intersection. Note
that there are six such cylinder-cylinder intersection for Tetradon. Each one yields an
infinite number of solutions for four joint parameters for one position of the Q
te trahedron.
Figure 3.2 Locus of points on an edge-pair intersection
The intersection of these two cylinders can be expressed by the following set of
equations:
Chapter 3. Calculating Joint Positions 29
where P and Q are vertices on the fixed and mobile tetrahedra, respectively. W and rr* are
the lengths of the connecting rods on the P and Q tetrahedra. [I.J,K] and [ij.k] are the
unit vecton parallel to the connecting rods. A and )c are unit vectors that are parallel to
edges on the P and Q tetrahedra, respectively. See Fig. 3.3
Figure 3.3 Parameters at an edge-pair intersection
To put these equations into a more useful form, known attributes of the tetrahedra are
substituted where possible and the positions of the elements of the Q tetrahedron are
expressed in terms of the seven rotation and translation parameters. In the end. eqs. (3.3)
to (3.5) expand to three equations in four unknowns.
Chapter 3. Calculating Joint Positions 30
- - To illustrate this, the intersection of edges P2 p , and Q,Q3 is expressed. The coordinates
of points Pz, and P4 are (- 1 1.1 1 .- 1 1 ) and ( 1 1.1 1.1 1 ). The coordinates of Q1 and Q3 when Q
is in the basic position are (-9,9,9) and (9,9.-9).
Vector [ a ? ~ . a ] ~ is parallel to the edge f2 P, and [a,O,-a] is parailel to the edge a when Q is in the basic position. Throughout this exarnple, the lengths of the connecting
arms are omitted because they are each one unit long.
Vector [I,J,K] is equivalent to [crsRz~,-cRz~. -as&lT, while vector [i?j,k] is equivalent to
[-aso13,coi3, -aso131T when Q is in the basic position. The intersection of edges - - P2 P, and QiQ3 is illustrated in Fig. 3.4.
Figure 3.4 Intersection of edges P2P, and QI Q3
Chapter 3. Caiculating Joint Positions 3 1
- Rotating the edge QiQ3 and the vector [i,j,k] into their final positions gives the following:
and
This system of equations is underconstrained. To solve it, a value for w1, is assipned.
Then, by substitution, two possible values for sRz4 are found. For each value of SR?^,
values for T14 and t l 3 are then obtained. The full expressions for the values of fi4 and
t l 3 follow. A means by which to address the underconstrained nature of these equations is
given in the following section.
Subtracting eq. (3.10) from eq. (3.8) and solving for 113 yields
Chapter 3. CaIculating Joint Positions
Then. substituting the expression for t l l into eq. (3.9) and solving for cRZ4 gives
w here
Finally, squaring both sides and using the relationship ct!&j = 1 - S ' R ~ yields a quadratic
expression for SR^:
where
The value of sR34 is then found as
Chapter 3. Calculating Joint Positions 33
For each possible value of Rz4 the value for T2J is found by substituting into eq. (3.8) as
follows:
The value of t l s is found by back substitution. The parametric equations for each edge
intersection are expressed in detail in Appendix B.
3.3 Example Using the Underconstrained Equations
To illustrate the use of the underconstrained equations. venex Q3 is rnoved to a point
within Tetradon's workspace. The rotation and translation of the Q tetrahedron needed to
attain the position were found using the inverse kinematics described in chapter 5. The
joint parameters required to position Q3 are calculated from these rotation and translation
parameters. In this exarnple, vertex Q3 is moved to the point [10.5. 13, -141. The inverse
kinematic solution yields the following rotation and translation parameters:
From these parameten. the transformation rnatrix, TQ is calculated to be the following:
Chapter 3. Calculating Joint Positions 34
Using these values, the coefficients A through N in eqs. (3.1 1 ) to (3.13) are calculated.
Starting with an assumed value of 0" for o. the two possible values for the R. t and T
parameten are found. Note that for two edge intersections. an angle of wa will not
yield a viable solution. For these two cases. the value of o was incremented until a real
solution for i2 could be found.
Starting Value of o Two Solutions for $2
ml3 = 2.878O fiz4= -10. 191°
Clz4 = - 14.770'
= 0.000" = 36.808"
Ci14 = -30.528"
Q34 = 89.14 1 al2 = -13.596O
Ri2 = -18.527O
= 0.000" R 2 3 = 82.973'
Q3 =-4.117O
Clw =-13.168"
R34 = -75.552"
f i l 3 =84.856"
f i I 3 =-3.5M0
Two Solutions for t
t13 = 4.4533
213 = 4.548 1
t23 = 4.6233
t 23 = 3.3926
t3j = 17.6883
t 3 ~ = 17.5966
t1.+ = 33.1507
t l j = 31.5032
t l 2 = 1.8978
t l ~ = 0.7509
= 1.6283
tîj = 3.3824
Two solutions for T
T2j = 24.8804
TZ4 = 24.8295
T l j = 5.2102
Tl j = 4.6783
T l ? = 22.7862
Tl = 22.7545
TZ1 = 9.2780
TZ3 = 9.8507
T3j = 24.3824
T3j = 24.8747
T l z ~6 .1326
Tl7 = 7.0233
Table 3.1 Joint parameters for underconstrained solution
Tetradon is shown in Fig. 3.5 below with the first set of joint positions for R. t and T
applied to it. To better illustrate the edge intersections, a close-up of the intersections of - - P, p4 with Q2Q, is given in Fig. 3.6.
Chapter 3. Calculating Joint Positions
Figure 3.5 Q3 rotated and translated to
Figure 3.6 Detail of P l PJ x @Q3 and P I P x Q2Q4 edge intersections
Chapter 3. Calculating Joint Positions
3.4 Constrained Intersection Equations
As noted in the previous section. the solution for the joint variables on Tetradon is
underconstrained. Two connecting rods c m meet anywhere on a cylinder-cylinder
intersection. To derive the joint positions. a value for one of those joints was assumed
and the rest were derived based on that assumption.
Taking this one step further, the joint whose position was originally assurned is now
irnmobilized completely. Taking one joint out of the PRRRRRP joint sequence reduces
each intersection to a maximum of two possible solutions for R. For exarnple. if al1 o
angles are fixed at O", the edge intersection shown in Fig. 3.2 above c m only yield two
positions. as shown in Fig. 3.7.
Edge on the P tetrahedron , . . - . .
/
Edge on the Q tetrahedron --
Figure 3.7 Two possible solutions for the constrained equations
Because the angle o is now fixed at O*. eqs. (3.8) to (3.10) above simplify considerably.
Starting with the vector parallei to the connecting arm on the Q tetrahedron. [i,j.klT:
Chapter 3. CalcuIating Joint Positions
Upon substitution with the new values for i. j and k. eqs. (3 -3) to (3.5) become:
Now there is no need to assume a starting value of 0 1 3 . By substitution. two possible
values for sRtj are found directly. The simplified solution for Ru, fi4 and t l 3 follow:
Subtracting eq. (3.19) from eq. (3.17) and solving for t l 3 yields
w here
Substituting the expression for r 1 3 into eq. (3.18) and solving for cLb4 gives
Where
C = a . A ( R 7 , - R 3 )
D=-QI! + a . B ( R ? , - R ? 3 ) + R 2 2 - 1 1
Chapter 3. Calculating Joint Positions 38
Finally. squaring both sides and using the relationship = 1- sZ& yields a quadratic
expression for sfi24:
Where
G = D I - I
The vaiue of sQ24 is then found as
For both possible values of Rz4 the value for fi4 is found by substituting t i ~ and R24 into
eq. (3.17) to give
3.5 Exarnple Using Constrained Equations
In this example, the point Q3 is again moved to the point [10.5 13. -141. To do so. new
rotation and translation parameters are required. The reason is that an orientation mus< be
found where al1 of o angles remain at zero degrees. The new rotation and translation
parameters are:
Chapcer 3. Calcu1;tting Joint Positions
These parameten generate the following transformation matrix. TQ:
Using these values. the coefficients A through G from eqs. (3.20) to (3.12) above are
calculated. Starting with an imposed value of 0" for o. the two possible values for the S2.
r and T parameters are found.
Two Values of R Two Values of r Two Values of T
Starting Value of o
Table 3.2 Joint paramerers for the constrained equations
Chapter 3. Calculating Joint Positions
Table 3.2 Joint parameters for the constrained equations (cont'd)
Fig. 3.8 shows Teuadon in the configuration given above. Note that. although the joint
parameters are different. the vertex Q3 is still at the same location.
Figure 3.8 Second configuration with Q3 at [10.5. 13. - 14)
Chapter 3. Cdculaung Joint Positions
Fig. 3.9 again shows two joint intersections in detail.
Figure 3.9 Second detail of P I P J x QzQ3 and PIP3 x Q2Q4
Chapter 4
Direct Kinematics
The direct kinematics of Tetradon are closely related to those of the Double Tetrahedron.
To show this, the example of the Double Tetrahedron is again examined first. followed by
that of Tetradon. To find the position of a vertex on the Double Tetrahedron require only
three joint positions. In the example given. these are three T parameters on the P
tetrahedron.
Tetradon. with al1 of its angles active, poses a much more challenging problem. A special
configuration of Tetradon is examined. In this configuration the o angles are locked in
position. This reduces the kinematics to a problem with a finite number of solutions for
six given joint positions. In d l . three different approaches to the kinematics of this
modified Te tradon are examined.
4.1 The Direct Kinematics of the Double Tetrahedron
Since they are an integral part of the inverse kinematics of the Tetrahedra, the rotation
and translation applied to the Q tetrahedron are found as a part of the direct kinematics
solution. The solution is found by starting with a set of three joint coordinates. Using
these three parameters alone, one face of the Q tetrahedron is located. Using the three
vertices of this face, the remahing vertex is found. Finally, the rotation and translation
applied to the Q tetrahedron are calculated to complete the solution.
Chapter 3. Direct Kinematics
4.1.1 Locating a Face
Daniali [SI showed how three linear paraneters could be used to calculate the position of
one mobile triangular manipulator tied to another, fixed. triangular base. This same
principle is applied to the Double Tetrahedron.
In Daniali's example. actuation is provided by three linear actuators acting dong the
edges of the fixed triangle (see Fig. 4.1). His approach to the kinematics was to fit a
triangle of known dimensions (the movable viangle Q) around three known points in
space. The locations of the three points in space were given by the positions of the linear
actuators on the fixed triangle. The solution of the kinematics for this manipulator
reduces to a simple quadratic equation yielding two possible poses of the mobile triangle.
MobiIe Triangle Q 7 1- - . . _ _ _ - - - -
Figure 4.1 Planar double triangle manipulator
The equation is of the form:
Where
- 2d sin^-(^^^,( , C? - - - . C I = - ' sin Q3 -* cosC, c 3 = - -
sin QI sin Q3 sin Q3
Chapter 4. Direct Kinematics 44
The elements of this equation are shown below in Fig. 4.2. The complete solution is given
by Daniali 151.
Figure 4.2 Positioning a triangle using three given points
In the case of the Double Tetrahedron, the procedure works as follows: Three T --
parameters are given. These parameters descnbe the linear travel dong edges P: ~ 4 . P I P J
and P 3 P 4 , respectively. Using these three parameters, the points P5. P6 and P7 are found.
These three points coincide with three points on the Q tetrahedron which will be labeled -- -
Qs, Qs and Q7. These are points on the edges QI@, @Q3 and Pi&. respectively. Starting
with these three points, the two possible locations of the triangle QiQzQ3 can be found.
See Fig. 4.3.
Chapter 4. Direct Kinematics
Figure 4.3 Positioning triangle Q1 Q2Q3
4.1.2 Finding Q4
With one face of the tetrahedron known, the last vertex is found through the use of simple
geometry. The normal to the triangle is calculated. Q4 is found by starting from the center
of triangle QiQ2Q3 and travelling dong the normal for a distance equal to the height of the
tetrahedron. The centre of the Q tetrahedron can be found in a similar fashion. This is
illustrated in Fig. 4.4. The normal to triangle QiQZQ3 is calculated as: --
n = Q2Q3 x QrQ3 (4.2)
Chapter 3. Direct Kinematics
The point QJ is then found as:
Where Q5 is a point kalf way between QI and Q2 and k is the height of the tetrahedron.
See Fig. 4.4 below. Because Daniali's solution is quadratic. there are rwo possible
solutions for the location of the triangle QiQ2Q3. The correct position of the Q tetrahedron
cm be found by determining which location of QJ satisfies the three remaining edge
intersections.
Q3
Figure 4.4 Locating the fourth vertex of the Q tetrahedron
Chapter 4. Direct Kinematics
4.1.3 Determining the Rotation and Translation
The rotation applied to the Q tetrahedron is found by a simple back rotation scheme. The
Q tetrahedron is transiated back to the origin. Then. rotations are applied about two
known axes to bring it back to the basic position.
First, some t e m s will be defined. qi is the vector from the ongin to a vertex i on
tetrahedron Q in the basic posirio~i. Second. q,' is the vector from the origin to a vertex i
on teuahedron Q' . Q' is an intenm position of the mobile tetrahedron used for
calculation purposes only. and need not be a feasible solution. In this position. the center
of tetrahedron Q' is located at the origin. Finally. qi" is the vector from the origin to
vertex i on tetrahedron Q" . Q" is the mobile tetrahedron in its final orientation. Note that
it has undergone no translation. Its center has not moved.
The fxst rotation applied to the Q teuahedron rotates Q3 to Q3' . The second rotates Q' to
Q" . This rotation is made about the vector q3' . This means that the points Q3 * and Q3 "
are coincident.
The ax is of the first rotation. nl . is mutually perpendicular to the vectors q, and q3". The
angle of rotation is denoted Q I . See Fig. 4.5.
The cosine and sine of $I are found from the dot and cross products of q3 and q3".
respectively. The rotation matrix is calculated from the axis nl and the angle in the
same manner as that given in chapter 3. This rotation rnatrix will be cailed RI, where
Chapter 4. Direct Kinernatics
Q',
9 3
Figure 4.5 First rotation - Q3 to Q3"
- RI is a applied to Q to rotate it to Q'. A perpendicular QzV p is dropped from point QZ ' to
- vector q3'. A second perpendicular, Qrtl p is dropped from point QY to the sarne point.
See Fig. 4.6. The axis of rotation. nz, is paralle1 to the vector q3". The angle of rotation is
represented by Q2 . The cosine and sine of are calculated from the dot and cross -
products of Q2' p and QI" p . A second rotation matrix. Rt, is generated from nz and (05
Chapter 4. Direct Kinematics
Figure 4.6 Second rotation - Q2' to Qz"
The final rotation is then calculated as
R=R2RI
Chapter 4. Direct Kinematics
4.1.4 Example Kinematics for the Double Tetrahedron
The following example illustrates the kinematic scheme described above. The only input
for the problem is t hee T pararneters for the P tetrahedron. From these three T
parameters, the two possible locations of the face QiQ2Q3 on the mobile tevahedron are
found. From these, the correct face is detennined and from that. the position of point Q,.
Then, two back rotations are applied to the Q tetrahedron to obtain the matrix used to
rotate and translate the Q tetrahedron from the basic position to its final position.
To get the exarnple points needed. a rotation of 0=60°. O = G O in the ZX plane is applied
to the Q tetrahedron. The three 7 pararneters for this position are as follows:
The points Q5. Q6 and Q7 are then calculated as:
From these points, two possible locations of the face QiQ2Q3 are calculated. Both of these
faces are used to find a corresponding vertex Ql. The two possible positions of the
triangle QlQ2Q3 are found to be:
Chapter 4. Direct Kinematics
First Solution Second Solution
\ Second Solution \
Figure 4.7 Two possible locations of Ql Q2Q3
The valid position of the face Q1@Q3 is the one that yields a viable position for QJ. Thus.
for each face. a position for QJ is calculated and the edges QI Q,. Q2Q, and Q3Q, are
venfied to ensure that they al1 intersect the P tetrahedron.
First Solution Second Solution
QJ = [-9.833. -3.288, - 15.6591 QJ = [-6.475, -7.0 19. - 16.0 151
Once the correct position of the Q tetrahedron has k e n found - in this case. it is the first
solution - the back rotations are calculated to find the rotation that was applied to the Q
tetrahedron to get it into the current position. In this case, the rotation and translation of
the Q tetrahedron are found to match those predicted and are as follows :
Chapter 4. Direct Kinematics
4.2 The Direct Kinematics of Tetradon
As noted in Chapter 3, Tetradon's iunematics are much more clearly defined if one set of
the o angles is fixed. This discussion of Teuadon's direct kinematics will deal only with
this configuration.
Since Tetradon is a mechanism with six degrees of freedom. six pararneters are needed to
define the position and orientation of the mobile tetrahedron. Because there are 36 joints
to choose from, there are many ways in which to approach Tetradon's hnematics. Three
are presented here. The first makes use of three 7 and three R pararneters. The second
uses linear translations on both tetrahedra. The third makes use of linear actuators on the
fixed tetrahedron only.
4.2.1 Configuration of Tetradon
For the purposes of this section. Tetradon is given the configuration mentioned at the end
of Chapter 3. That is. al1 of the o pararneters are fixed at zero degrees. The connecting
rods from the Q tetrahedron to the points of intersection will not be shown because. for
al1 intents and purposes. rhey need not exist. The kinematic solutions to follow deal with
either points on this configuration of the Q tetrahedron, or with the intersection of its
edges with points on the cylinders created by the edges of the P tetrahedron.
4.2.2 T and Q Parameters
In a situation where three T and three Q parameters are known, the kinematics of
Tetradon are practically identical to those of the Double Tetrahedron. The only difference
is that the points used to define one face of the Q tetrahedron are found using one T and
one R parameter, rather than one T parameter alone.
Chapter 4. Direct Kinernatics
Figure 4.8 iliustrates the joint parameters as they are used in this case. Given the three T
and three ZL parameters. three points on the Q tetrahedron are known. These three points.
Q, Q6, and Q7 are al1 on face QlQ2Q3 of the mobile tetrahedron. Using Daniali's solution.
these three points yield two possible positions of the face QiQzQ3. From these two
possible faces, the correct one is found and the rotation and translation are calcuiated as
with the Double Tetrahedron.
,
Figure 4.8 Locating points Q5, Q6 and Q7
Chapter 4. Direct Kinematics
4.2.3 T and t parameters
Another way of approaching the problem of Tetradon's kinematics is to assume that three
T and three t parameters are known. In this situation. the kinernatics are not so well
defined. The reason is that the Q tetrahedron is not fixed to any known points. as it was in
the previous section.
The problem is approached in the following manner. First, the three t parameters on the Q
tetrahedron are used to describe a triangle of known dimension on a face of the Q
tetrahedron. If the parameters t 1 3 . r23 and tl? are used. this triangle is inscribed in
face QiQzQ3. See Fig. 4.9. This triangle wili be labeled QSQ& as shown in the figure.
Figure 4.9 Known triangle Q5QoQ7 touching three known circles
Chapter 4. Direct Kinematics 5 5
Second. three circies are defined about three P edges at points P5. f6 and P7. These points
are located at distances of -4. Ti4 and T3.: from vertices Pz. PI and P3 respectively. The
points Q5, e6 and Q7 each fa11 on one of these circles. The points Q5. Q6 and Q7 on the
resulting circles are defined as:
Where a is the constant I/& and P5. P6 and P7 are the points shown in the figure.
The distances between the points a, Q6 and Q7 are known. These three known distance
are the constraints placed upon the locations of these three points. Thus:
These equations reduce to three quadratic equations in three unknowns. R24. R14 and Q14.
Each real solution for the triangle can be used to position the Q tetrahedron. Each of these
positions cm, in mm. be tested against the remaining edge intersections to find the
solution that works for al1 edges.
Chapter 4. Direct Kinematics
4.2.4 T Parameters Only.
Finally, there is the case where no parameters on the Q tetrahedron are known. For this
particular example. the six T parameters are used. as they are most likely to be actuators.
The problern once again becomes more involved because less information about the Q
tetrahedron is known. This particular case requires finding a tetrahedron whose edges
intersect six circles in space.
The solution starts with the circles about the edges of the P tetrahedron. The centers of
these six circles are given by the T parameters and each is defined by an angle a. Lines
joining these circles can be defined as:
f .a Where
and
These six points are joined together to create four triangles. Each of these triangles is a
portion of a face of the Q tetrahedron. See Fig. 4.10.
Chapter 4. Direct Kinematics
Faces on Q Temhcdron
Figure 4.10 Partial faces of Q touching known circles on P
The fact that these four triangles are on the face of the Q tetrahedron imposes a constraint
on the locations of the six points. The four faces of the Q tetrahedron al1 intersect at a
known angle, 109.47". The four triangles wi11 have to have the sarne relationship.
The four Uhngles are, in order, A l = Q6Q7Q10: A 2 = Q5Q7Q8: A 3 = Q8Q9QI0; A4 = QsQ6Q9.
The normals used for each of these triangles are calculated using the cross products of
two of the triangle edges. For example, the normal to A , = Q,Q,g,, , nl is found as:
In component form:
Chapter 4. Direct Kinematics
c The three remaining nomals. nl, n3 and N are found in a similar manner. The constraint
placed on the triangles is expressed as:
By finding the intersections of ail nomals. six constraint equations are created. The only
unknowns in these equations are the six R values. Once these values are calculated. they
are used in the direct kinematics described in section 4.2 to find the possible orientations
of the Q tetrahedron.
Chapter 5
Inverse Kinematics
The function of Tetradon's inverse kinematics. as applied here. is no! to find Tetradon's
joint coordinates directly. Rather, it is to solve for seven rotation and translation
parameters. These parameters are needed to position the mobile tetrahedron where a
solution for the joint coordinates can be found. Once the mobile tetrahedron has k e n so
located. the procedure descnbed in Chapter 3 is used to find the required joint
coordinates.
To find the required rotation and translation. basic geometric relationshps are again
employed. Expressions to calculate the distance between each edge pair are created.
Des. a Using these expressions. and supplying the required maximum distance between ed,
position of the Q tetmhedron is found that satisfies the "soft" condition that al1 edges be
within a certain distance of each other.
A Newton-Gauss approximation is used to solve for the seven rotation and translation
parameten. The method uses a least-square solution to a constrained nonlinear system of
equations to find viable positions for the mobile teuahedron. A position is viable if each
e d p of the mobile teuahedron is within reach of its matching edge on the fixed
teuahedron. Once the edges are close enough to each other. the seven-joint linkage will
take care of any differences in orientation between them.
Chapter 5. Inverse Kinematics
5.1 Description of the Procedure
The algorithm chosen to deal with the iterative portion of the inverse kinematics is
contained in the QUADMIN software package developed at the McGill Centre for
Intelligent Machines (Angeles and Ma [ I l ) . It is based on a least-square solution to a
constrained nonlinear system of equations. The algorithm optimizes the solution of an
underconstrained set of nonlinear equations by minimizing the norm of the solutions of a
set of objective functions.
The approach is to start with a first guess at the solution. From this initiai guess. an
approximation of the final solution is made as:
where xk is the vector of design variables for the current iteration and xk-1 is the
approximation of the final answer. If the approximation does not satisfi the consuaint
conditions. it is used as the next guess. New approximations are made and the results are
adjusted iterativeiy untii the deviation from the objective is rninimized. or the procedure
is found to diverge.
The increment value ~r, is computed from the minimum norm solution to the following
system of equations:
In this case. f (xk is a set of objective functions used to guide the final solution to the
problem. F(xk) is the Jacobian. or matrix of panial derivatives with respect to x . of the
vector function f. The least square solution to the above set of equations is found subject
Chapter 5. Invene Kinernatics
c to the constraints placed on the whole problem. These constraint equations are
represented as g(xk ).
Where g(xk) is the vector of constraint conditions and G(xk) is the Jacobian mauix of the
vector function g(x). When the nom of Ax falls below a specified minimum value and
the constraint conditions are satisfied to within a given tolennce. the procedure stops. If
the procedure exceeds a specified number of iterations. it is considered to be divergent
and an error message is issued.
In order to apply the QUADMIN package to Tetradon's inverse kinematics. subroutines
had to be written which contained the constraint equations. the objective fumions and
their Jacobians pecuiiar to Tetradon. These subroutines were insened into the body of the
QUADMIN code and the program was then used to solve for a set of seven design
variables, given a desired location of the vertex Q3.
5.1.1 Variables
The design variables are the unknowns that will provide the solution to the problem ar
hand. They are represented as a vector x(n), where n is the number of design variables
with which the solution deals.
The seven design variables for the Tetradon's inverse kinematics are the components of
the rotation and translation of the mobile tetrahedron, e, , ey , e, . $, X. Y and 2. Once
these variables have been found. they are used to calculate the actuai joint positions for
the T, t, R and o parameters for Tetradon.
5.1.2 Constraints
Any valid solution to Tetradon's inverse kinematics must satisfy the constraint equations.
The constraint equations are expressed as a vector function of x and are referred to as the
Chapter 5. Inverse Kinematics 62
vector g(x). In Tetradon's case. the vector g(x) has four variables. as this is the number of
constraint equations used in the solution.
The goal of this inverse kinematics scheme is to find the joint coordinates needed to
produce a desired position of the vertex QS. The inverse kinematics of Tetradon cm be
expressed by three vector cornponent equations equating the X. Y and Z coordinates of
the vertex Q3 to those desired.
5.1.3 Objective Functions
The constraint equations provide a bounded but infinite set of solutions. Even if the
vertex (2) is locked in position. the rest of the Q teuahedron can be anywhere around that
point.
The set of solutions where al1 six edge pairs rernain intact is much more restricted. The
objective functions are used to satisfy the constraints. while maintaining the required six
joint intersections.
The objective functions are expressed as a vector function of x and are referred to as the
vector f(x). The vector f(x) has m elements. where m is the number of objective functions
needed for the solution. For Tetradon. six objective functions are required. Each function
ensures that one e d g pair will remain in intersection for any valid solution.
5.1.4 Jacobians
The Jacobian matrices are the partial derivatives of g(x) and f(x) with respect to the seven
elements of x. The two Jacobians are referred to as G(x) and F(x) and they have the same
row dimensions as their associated vector functions.
Chapter 5. Inverse Kinematics
5.2 Applying the Procedure to Tetradon
The seven design variables found in this procedure are the seven elements of the
transformation matrix T. T is found as:
where k= 1-CO.
A transformation matrix must satisfy the constraint
Where DP is the destination of the point Q 3 . This is the basic constra.int placed on
Tetradon, and it is handled by the constraint equations g(x). This ties the point Q3 to the
destination point. but it does not guarantee that ail six edge intersections will be
maintained. In order to accomplish this. the transformation rnauix must also mis@ the
requirement
bt., 1 2.0. i = 1 ro 6 (5.6)
Where wi is the distance between an edge on the P tetrahedron and its corresponding edge
on the Q tetrahedron. The objective functions f(x) are used to maintain the edge
intersections. For each edge intersection there is one objective hnction that equates the
distance between the two edges to the value of w,.
5.2.1 Applied Constraints
In dl, four constraint conditions are imposed. The first three are coordinate equations. the
fourth is required to maintain the integrîty of the algonthm and is unrelated to Tetradon.
The positional constraints are simply the X, Y and Z coordinates of the target position for
Chapter 5. Inverse Kinematics
the vertex Q3. The fourth constraint forces the algorithm to keep vector e of unit
rnagni tude.
The three position constraints relate the rotation and translation of the mobile vertex to its
desired position in space. Applying the rotation and translations to the vertex Q,
produces the following equations. where the Q tetrahedron's initiai position is assumed to
be the basic position:
where DP is the destination point. or the target position. for the Q3 vertex.
The components of the rotation vector ex, e,. e, and g wilt change with each iteration. As
they are recalculated, the magnitude of the vector e changes. For the algorithm to work.
this unit vector must be constrained to have a magnitude of one. Thus. the founh.
cornputational, constraint is as fol1ows:
5.2.2 Applied Objective Functions
The objective functions are used to impose bounds on the solution. Stated simply. these
objective functions try to keep each edge in a pair within a set distance of each other. The
expressions used for the objective functions are the expressions for the distance between
the edges and they are derived as follows:
Chapter 5. Inverse Kinernatics 65
Given the position and orientation of the Q tetrahedron. the distance between an ed, ~e on
the Q tetrahedron and its partner on the P tetrahedron are found through the solution of a
senes of parametric equations.
An edge on the Q tetrahedron is given as
Its associated edge on the fixed P tetrahedron. P? P4 is already known. The mutuai
perpendicular between these two edges is denoted as O,,, and it is found as
Figure 5.1 shows these three vectors for one intersecting edge pair.
Figure 5.1 Parameters used to calculate the distance between edges
The distance between the edges, measured dong the vector O, is calcuiated by solving
the following set of equations:
Chaprer 5. Inverse Kinematics 66
Where P is a venex of the fixed tetrahedron and Q is a vertex of the rnobiIe tetrahedron.
A is the unit vector paralle1 to the fixed e d g . )c is the unit vector paraIlel to the mobile
edge. o is the unit vector that is perpendicular to both edges. T is the linear displacement
from point P along the fixed edge. r is the linear displacernent from point Q along the
mobile edge and w is the distance between the edges.
The expressions above may be simplified by substituting the known coordinates of point
P and the components of the unit vector along the fixed edge. The components of the
other two unit vectors. as well as the coordinates of the point Q must ail be expressed in -
terms of the seven design variables. Computing the distance between edges P? P, and - Q, Q3 will serve as an exampie.
The coordinates of the points QI and Q3 are found by multiplying their basic position
coordinates by the rotation matnx and adding the displacement. Thus.
and
Chapter 5. Inverse Kinematics
The unit vector that is paralle1 to the mobile edge is found as:
The X. Y and Z elements of the mutual perpendicular are found by cross multiplying -
P2 P4 and QiQ3 . Thus.
Starting with eq. (5.14) and solving for the distance between the edges. rr* yields
i c .= A + B - r 1 3
Where
Substituting the value for w into eq. (5.13) and solving for 1 1 3 gives:
< Q I , + I ~ - ~ ~ ~ - A ) -a C = and LI=
O24. ' B - n 1 3 r %, - B 413c
Finally, substituting the expression for in eq. (5.21) into eq. (5.15) gives the following
expression for -4:
Chapter 5. Inverse Kinematics
Where E=Q,, +AI3: .C+ll-cr2-Iz.A-byi - B . C and F = a + a z 4 : - B - D - I , ~ : - D
The values for t i ~ and w are then found by back substitution.
5.2.3 Generating the Jaco bian Matrices
The Jacobians G(x) and F(x) are caiculated by finding the denvatives of their component
parts, then assernbling these as the derivatives of the full expressions. To begin with. the
partial derivatives of the transformation matrix, T are found as:
O ke, sd~ O d - T = [h-e.r 2k;, k;: Cl] d e ,
-s@ ke,
Chapter 5. Inverse Kinematics
From these derivatives of the transformation matrix. partial denvatives of the expressions
for each vertex on the Q teuahedron can easily be found. Following with the example
given, the derivatives of the equations to find points Q I and Qj are found as
and
Chapter 5. inverse Kinematics
5.2.3.1 Jacobian of Constraint Equations
The Jacobian of the constra.int eqs. (5.7) through (5. IO), G(x), is then calculated as
follows. For the first three constraints equating the coordinates of Q3 to the destination
point DP.
G ( x J ~ , =- (9 . (d Tl l /d X, +d Tlz/d X, -d TI3/d Y, )+d T14/d x , )
G ( x ) ~ =- (9 . (d TZl/d X, + d Tx/d X, - d TZ3/d X, )+d T3/d x , )
G(x)% = - ( 9 - t d TJl/d X, +d Ty/d X , - d T33/d I, )+d Ty/d x, I
where i=l to 7. The fourth consuaint equation includes only the first three design
variabies, thus
where i= 1 to 3.
5.2.3.2 Jacobian of Objective Functions
The Jacobian of the objective equations. F(x), is calculated by progressively finding the
derivatives of the elements of the objective functions. The process stans with the
elements of h13 . These are found using the partial derivatives of the expressions for the
points on Q. Thus, the partial derivatives of the elements of )ci3 are found as:
Chapter 5. Inverse Kinematics
Using these values. the derivatives of the vector oz4 c m be found as
where 1 = IIaII.
Generating the Jacobian F(x) is a rnatter of substituting the above partial derivatives into
the expressions for A. B. C. D. E and F given above . The final form of the Jacobian is
The full expansion of the objective functions, dong with al1 six sets of partial derivatives
are given in detail in Appendix B.
5.3 Exarnple Solution
Following is an exarnple of the application of the inverse kinematic solution given above.
For this example, a trajectory for point Q3 was chosen arbiuarily. The path starts at the
home position of (3,. [9,9.-91. and moves to a finai destination of [ 10.5.13.0.14.0]. The
trajectory was broken down into 100 steps. At each step on the trajectory the values of T.
t and R were calculated for each edge. based on a starting value of o. For each point on
the trajectory, the first value of o tried was 0'. If a solution for the other joint parameters
was impossible, the value of o was incremented until a solution could be found. Figure
5.2 shows Tetradon with the Q tetrahedron in the basic position and in its final position.
Chapter 5. inverse Kinernatics
To illustrate the movement of the Q tetrahedron. ai1 of the intermediate positions of one
of its edges are aiso shown. The joint paths for al1 six o. R. r and T parameters are shown
in Figs 5.3 to 5.6.
- . Q',
Tajectory of Q3
QI3
Figure 5.2 Trajectory of Q3
Chapter 5. Inverse finematics
100 -
Points on Trajecto-
Figure 5.3 o values dong trajectory
For most of the rotations and translations of the Q tetrahedron. a starting value of o = OC
was sufficient to find solutions for the remaining joint parameten. However. early on in
the trajectory there is a point where edges QIQz and Q,Q, are positioned in such a way that
this was not the case. These two edges are too close to their corresponding edges on the P
tetrahedron and both the o and R joints are needed to allow for this. Such a situation can
be avoided rhrough manipulation of the objective functions within the ai, oorithm.
Chapter 5. Inverse Kinematics 74
Later in the trajectory, mi2 and 0 2 4 both start to wander from O". This is because Q3 is
approaching the edge of its workspace. Manipulation of the objective functions might
have some effect on the o values. but the wandenng could not be avoided altogether.
Better management of the initial w values would avoid the large jump in 0 1 2 toward the
end of the trajectory. In the program used. the value of o was always incremented in
positive steps from the original value of O". For this joint. negative steps would have
produced rnuch less displacement and a much smoother joint path.
Points on Trajectory
Figure 5.4 S2 values dong trajectory
Chapter 5. Inverse Kinematics 75
The joint path for R is a reflection of the joint path for o. As long as CO remains constant.
the value of R will change smoothly. As soon as the value of o starts to change. R has to
compensate. This is shown in Fig. 5.4 above by the paths of Ri2 and R24. Again. different
management of the starting values of o could smooth out. but not eliminate. these
wrinkles in the joint path. In the program used. o was incremented by steps of 0.360"
degrees. Because of the amplification seen in the R values. this increment should be
reduced considerably.
50
Points on Trajectory
Figure 5.5 t values dong trajectory
Chapter 5. Inverse Kinematics 76
The translation dong the P and Q edges are relatively insensitive to changes in the values
of w and R. The only jump in values for either set of parameters for this trajectory is due
to the large jump in the value of ON late in the path.
50 Points on Trajectory
Figure 5.6 T values dong trajectory
As can be seen from the joint paths for this trajectory of Q3, the algonthm to calculate the
inverse kinematics and the program used to calculate the joint positions are not without
their quirks. However, the objective of being able to select a point within Tetradon's
workspace and then move the vertex Q3 to that point, calculating al1 necessary joint
positions dong the way, was achieved.
Chapter 6
Conclusions
While creating a worhng mode1 of the double tetrahedron - a two DOF paralle1 mecha-
nism - another, more complex and more mobile mechanism was created indvertently.
The new mechanism. dubbed Tetradon is a parallel rnechanism consisting of a fixed
tetrahedron. a mobile tetrahedron and 42 joints connecting the two. There are six serial
c chains, one for each edge pair intersection in the device. Each seriai chain contains seven
joints in a PRRRRRP configuration.
A means by which to calculate the joint positions for Tetradon was devised. The joint
position calculation requires as its input the rotation and translation of the Q tetrahedron.
The inverse kinematic solution for Tetradon is concemed only with finding the rotation
and translation of the Q tetrahedron. Once these have been found, the joint positions are
calculated separately.
The direct kinematics of Tetradon cm be calculated in many different ways. Three
possible alternatives are presented. The direct kinematic solutions build on the direct
kinematics of the Double Tetrahedra, which are also presented.
The solutions for the joint positions, direct and inverse kinematics al1 rely heavily on the
Q simple geometric relationships inherent in the design of Teuadon. The solution of the
Chapter 6. Conclusion 78
joint variables is based on the parametric represenration of intersecting cylinders. The
direct kinematic solutions use line intersections. equations of circles and the rules
goveming uiangles and tetrahedra. The inverse kinematic solution is based on the
distance between parametrically defined lines.
Al1 of these solutions supply multiple answers. No attempt was made to identify the best
configurations or to study mechanisms by which to optirnize these configurations. This
presents an opportunity for funher study.
6.1 Results
Three aspects of Teuadon's kinematics were dealt with: a means to calculate the joint
positions for Tetradon given the position and orientation of the Q tetrahedron: three
different approaches to the direct Iunematics of Tetradon given six joint variables; and
finally a scheme îo position the Q teuahedron given a required position of one of its
vertices.
6.1.1 Joint Positions
The centres of the spherical joints linking the connecting rods trace the surfaces of
cylinders. The edges of the tetrahedra are the axes of the cylinders and the short
connecting rod are their radii. Each of these cylinders is represented parametrically. with
one parameter being the linear travel dong an axis of a cylinder and the other being the
rotation of the shon connecting rod. These two pararnetea are represented as T and R on
the P tetrahedron and as t and o on the Q tetrahedron.
The parametric representation allows Tetradon's joint parameters to be solved in a set of
three simultaneous equations in four unknowns. Because these equations are
underconstrained, an assignment of one joint variable had to be given in order to fmd the
other three.
Chapter 6. Conciusion
;a A small change to the configuration of the joints in Tetradon allows for closed form
solution of the joint parameters. The change is to elirninate one joint from the redundant
seven joint chain. In this work. it was the o parameter that was eliminated. This left six
joints in a PRRRRP configuration at each edge pair. Eliminating the w parameter reduces
the parametric equations to three quadratic equations in three unknowns. yielding only
two possible solutions for the remaining joint positions.
6.1.2 Direct Kinematics
An approach to the direct kinernatics of the double tetrahedron was developed. This
approach was expanded upon to create three different methods to find the direct
kinematics of Tetradon.
The kinematics of the DT are used to find the rotation and translation of the Q
tetrahedron based on three joint coordinates. The three coordinates are T pararneters dong
edges on the P tetrahedron. These three T pararneters define the locations of points on one
triangle of the Q tetrahedron. Using the solution given by Daniali [5] the location of the
vertices of that triangle are found. Once that face of the Q tetrahedron has been located.
the fourth vertex is found. Once al1 of the points on the Q tetrahedron have been found.
the rotation and translation applied to it to get it into that position are easily calculated.
The first approach to the kinematics of Tetradon follows the same pattern as that of the
DT. Six joint positions give the positions of three points on a triangle of the Q
tetrahedron. The position and orientation of the Q tetrahedron are then found in the same
way as they were for the DT.
The second approach uses three T and three t pararneters on the P and Q tetrahedra. The
three t parameters on the Q tetrahedron describe a triangle of know dimension. The T
parameters on the P tetrahedron identify three circles in space. The vertices of the triangle
Chapter 6. Conclusion 80
must touch each of those three circles. Each configuration provides a possible solution to
the location of the Q te~ahedron. Once the three venices have been located. the position
and orientation of the Q tetrahedron are found as before. The correct orientation of the Q
tetrahedron is established by checking for intersection between the other three e d g s on
the P and Q tetrahedra.
The third approach uses six T parameters on the P tetrahedron only. These define six
circles in space. The six venices of a four sided figure must touch al1 six of those circles.
The restriction on this is that the normals of these four faces of the figure must be at an
angle equd to the angle between the faces on an equilateral tetrahedron. The possible
solutions are obtained from a series of six simultaneous equations in six unknowns.
6.1.3 Inverse Kinematics
The approach to the inverse kinematics of Tetradon was to uncouple the joint positions
from the position and onentation of the Q tetrahedron. To do this, the position and
onentation were found first, and then the joint positions were calculated aftenvards. based
on the location of the Q tetrahedron. The task of the inverse kinematics solution. then. is
to find a position and orientation for Q where al1 of the joint positions c m be solved.
This was accomplished by exploiting the objective functions in a least-square solution of
the position of the vertex Q3.
A set of four constraint equations are used in the inverse kinematics. They are functions
of seven design variables. These seven variables define the rotation and translation
applied to the Q teuahedron to place the vertex Q3 at a desipnated point in space. This
results in a set of four simultaneous non-linear equations in seven unknowns. To solve
these equations, a Newton-Gauss approximation is used. subject to a series of objective
functions. The objective functions are used to force the solution to give a viable solution
Chapter 6. Conclusion 8 1
The objective functions are the calculations of the distances between edges on the P and
Q tetrahedra. One objective function is used for each edge pair. It was found that setting
the target distance between edges to be slightly less than the maximum possible distance
between edges worked the best. An example trajectory and its associated joint paths
shows that the method works, but not without its peculiarities.
6.2 Contributions
Tetradon. as a mechanism, has not k e n studied before. Therefore. the treatment of its
joint positions. direct and inverse kinematics are al1 original work. Although several
people have worked on the double tetrahedron. no one has yet examined the question of
its direct kinematics. Thus. the work on the direct kinematics of the DT is new as wetl.
The parametric solution of the intersection of cylinders was introduced by Zsombor-
.c Murray [13]. However, the application of this formulation to the joint positions of
Tetradon is new to this work. Of more significance is the concept of cdculating position
and orientation of a portion of a mechanism independently of the actual joint positions.
Chen [4] used this approach for his Double Tetrabot. The approach is exploited much
more purposefully here.
Chen [4] used a Newton Gauss approximation in his inverse kinematic solution for the
Double Tetrabot. However. his solution was based on a constrained system of equations.
Because they are underconsuained. the inverse kinematics of Tetradon require the
addition of a set of objective functions to find a solution. The exploitation of these
objective hnctions to find a viable position of the Q tetrahedron is also new to this work.
Chapter 6. Conclusion
6.3 Opportunities for Further Study
Tetradon offers a more mobile alternative to the restricted workspace of the DT.
However, that extra flexibility cornes at the expense of greater computational complexity.
The calculation of the joint parameters yields at best two possible solutions for every edge
intersection. At worst. it generates infinite solutions which requires a predefined
mechanism to choose among the possible answers. More study into the issues of reducing
the possible solutions and choosing the optimum ones is needed.
It was demonstrated that the shape of Tetradon's workspace will be affected by the
relative sizes of the P and Q tetrahedra. A relatively large Q tetrahedron dlows greater
maximum travel distances dong a type-III motion. but also creates a workspace with a
smdler total volume. A relatively srnail Q tetrahedron reduces the maximum travel
distance in a type-LE rotation. but allows more flexibility overall. No effort was made
here to find the optimum relative size for the Q tetrahedron to maximize travel and
maneuverability .
The inverse kinematic solution relies very heavily on the objective functions given for the
aigorithm. This lirnits the solution in that possible poses may be rnissed if the wrong
target values are chosen for the objective functions. Either another approach to the
inverse kinernatics is needed. or a better way to manage the objectives and the weighting
matrix should be found to increase the reliability of the algonthm as it is applied to
Tetradon.
References
[ l ] Angeles. J. and 0. Ma. 1989. QUADMIN: An Integrated Package for Constrained Nonlirlear Least-Square Problems. Comp. in Eng. AmSoc. Mech. Eng. book no. G0502B.
[2] Angeles, J.. 1982. Spatial Kinematic Chains. Spnnger-Verlag. Berlin.
[3] Bulca, F.. J. Angeles and P.J. Zsombor-Murray. 1994. On the Kinernarics of Mechanisms with Redundant Loops. Proc. 12th Symp. on Eng. Applications of Mechanics (Interactions of Fluids, Structures & Mechanisms). Montreai. pp. 63-72.
[4] Chen. H-W. 199 1 . Kinemarics and introduction to dynamics o fa ntovable pair of tetrahedra. M.Eng . Thesis. McGill University. Dept. of Mechanical Engineering.
[5 ] Daniali, H.R.M. 1995. Contribittions to the Kinematic Synthesis of Parde l Manipulators. Ph. D. Thesis. McGill University. Dept. of Mechanical Engineering.
[6] Hyder. A. and P.J. Zsombor-Mumay. 1989. Design, m o b i l i ~ anaiysis and aninlatio>l of a double equilateral tetrahedral mechanism. CM-89- 15, McRCIM Int. Rep. McGill University, Montreal. 57pp.
[7] Stachel. H. 1988. Ein bewegliches Tetrahederpaar. Elemente der Mathematik v.43(3). pp 65-75.
[8] Tmai. T. and E. Makai. 1988. Physically inadmissable motions of a pair of tetrahedra. Proc. 3rd Int. Conf. Eng. Graph. & Desc. Geom. Vol. 2. Vienna. pp 3 4 - 2 7 1
[9] Tarnai. T. and E. Makai. 1989. A moveable pair of tetrahedra. Proc. R. soc. Lon. A. 423. pp 423-442
References 84
[ 101 Tarnai, T. and E. Makai. 1989. Kinernntical indeterminacy of a pair of retraliedral frames. Acta Tachnica. Acad. Sci. Hung. 102.( 1-2) pp 123- 145.
[ I l ] Zsombor-Murray. P.J., F. Bulca and J. Angeles. 1992. Kinemutics ofa hi&& parallel mechanism. Workshop on Kinematics and Robotics. Ebemburg 92-07-( 12- 17).
1 3 ~ ~ .
[12] Zsombor-Murray. P.J. and A. Hyder. 1992. An equilateral retrahedral nieclzanisnz. Robotics and Autonomous Systems. Elsevier Science Publishers B.V.
[ 131 Zsombor-Murray. P.J.. 1992. A Spherocylindrical Mechanism and Inrersection Among Other Qrdrat ic Surfaces. Proc. 5th Annual ASEE Int. Conf. ECGDC. Melbourne, Ausualia. v.2. pp. 370-376.
Appendix A
Components of the Physical Mode1 of the Double Tetrahedra
The following drawings show the components of the physical mode1 of the double
tetrahedra that was designed and built and constituted a major part of this work. Each
component is shown separately. followed by a composite drawing of the joint intersection
assembly and end pieces and some photographs of the mechanism itself.
The First series of drawings shows the components of the edge intersection mechanism.
The pnsmatic joints for the T and t parameters are roller bushings mnning on hardened
steel shaft. The R and o parameters result from the rotation of these roller bushings
around the shafts on which they are mounted. The rotary parameter Y revolves through
the radial bearing shown in the bottom of the slider.
The second set of drawings shows the assembly at the four vertices of each tetrahedron.
These end pieces consist of a three legged "spider" on which three arms are mounted.
Each arm houses a radial bearing that contributes one parameter. These radial bearings
are mounted in such a way that their axes intersect the vertical a i s of the top and bottom
slider, maintaining the geometry of the double teuahedra.
Appendix A. Components o f the Physical Mode1 of the DoubIe Tenahedra
Figure A. 1 The Top of the Slider
/ TETRAHEDRON SLlDER I
c'bore 7.00 dsep
bom 0.M dbsp
place: fila:
lower (fernale) slldarb. 0 ~ 2 : 02/02/92
material: ( Alurnlnurn
Appendix A. Components of the Physicai Mode1 o f the Double Teh-ahedra
Figure A.3 Steel Plates for the Thmst Bearing
1 1 CHAMFER EDGE AS ON HlNGE
0.125 dia. 3 HOLES IN SAME PLANE
TETRAHEDRON END PIECES
piece!
file:
end spider
spidermt. DC2 / 31 /O1 192
1 materiai: 1 aiurninurn '
Appendix A. Components of the Physical Mode1 of the Double Tetrahedra
Figure A.5 End Hinge
Appendix A. Components of the Physical Mode1 of the DoubIe Tetrahedra
Figure A.6 Bearing Cap for End Hinge
Appendix A. Components of the Physical Mode1 of the Double Ten-ahedra
Figure A.7 Comecting Arm for End Hinge
snap ring
ailder (top) end spider ,. Ml2 x 1.5 ," /' , bal1 bushlng
anap ring
"i bearlng plates
L
connecllng a m
snep ring
radial baarlng 10 mm bom Famir 200K or squlvalsnt
beerlng cap
thnid 18 mm bsarlng bore
Tonlngton NTA-1220 or equivalent
rodlal bearlng 8 mm bom Fahiir 38K or eguhralenl
END PIECES AND SLIDER piecer filer
assem bly allpcs.DC2 131 ~01192
materlalt Aluminum and Steel scalet- 1 10mm
Appendix A. Cornponents of the Physical Mode1 of the Double Tetraheda
Figure A.9 Assembled slider
Figure A. 10 Assembled end spider and hinges
Appendix A. Components of the Physical Mode1 of the Double Tetnhedn
Figure A. 1 1 Folly assembled mode1
Appendix B
Parametric Equations for Joint Values
The location of the center of the bal1 joint joining the connecting rods at each edge
intersection is expressed in a generd form as follows:
where Px, and Q., are vertices on the fixed and mobile tetrahedra. respective1 y. N and a.
are the lenb@s of the connecting rods on the P and Q tetrahedra. These values are omitted
because in this work. the lene@ of W and w are both one unit. [LJ-K] and [i.j .k] are the
unit vectors parallel to the connecting rods. A and h are unit vectors parallel to ed, =es on
the P and Q tetrahedra.
The paramemie equations for al1 six edge intersections are presented here. For each edge
pair, the coordinates of the vertices and the unit vectoa wili first be expressed for
Tetradon in the basic position. Then. the rotated and [i,jh] vectors are given. These are
then substituted into eqs. (B. 1) through (B.3) above. The solution is worked though to the
end, giving the solution for the T. r and R parameters. Recail that to accornmodate
Teuadon's redundancy, the value of o is assigned to each edge intersection.
Appendix B. Paramemc Equations - Solution of Joint Values
B . l Intersection of Edges G a n d Q,a,
B.1.1 Basic Position Coordinates and Vectors
B.l.2 Rotated Vectors
B.1.3 Solution
Upon subsûtution. eqs. (B. 1) to (B.3) become:
Subtracting eq. (B.6) from eq. (B.4) and solving for 113 yields
t13 =A-s inR2, + B - s i n u I I +C-coso13 + D
where
Appendix B. Paramecric Equations - Solution of Joint Values
Then, substituting the expression for t , , into eq. (B.5) and solving for c& gives
ci lIj = E.sQZJ + F .sw13 + G - c w t 3 + H (B.8)
Finally, squaring both sides and using the relationship c2n24 = 1 - s ~ R ~ ~ yields a quadratic for SR?^:
Where
The value of sn14 is then found as
(B. 10)
For each possible value of Rr4 the value for T24 is found by substituting into eq. (B.4) as follows:
The value of t l 3 is found by back substitution into eq. (B.7).
Appendix B. Parametric Equations - Solution of Joint Values
B.2 Intersection of Edges 4 P4 and Q2Q3
B.2.1 Basic Position Coordinates and Vectors
B.2.2 Rotated Vectors
B.2.3 Solution
Upon substitution, eqs. (B. 1 ) to (8 .3) becorne:
Subtrac ting eq. (B. 13) from eq. (B. 14) and solving for t z3 yields
r 3 = A.sinRf4 + B - s i n a ~ ? ~ + C - C O S ~ ~ ~ + D
where
Appendix B. Parameuic Equations - Solution of Joint Values 100
Then, substituting the expression for t23 into eq. (B. 12) and solving for cRi4 gives
Where
Finally. squaring bolh sides and using the relationship cT!& = 1 - S ' R ~ ~ yields a quadratic for sQi4:
Where
The value of ~ $ 2 ~ 1 is then found as
For each possible value of R14 the value for Tl4 is found by substituting into eq. (B. 13) as follows:
The value of is found by back substitution into eq. (B. 15).
Appendix B. Parametric Equations - Solution of Joint Values
B.3 Intersection of Edges %and Q,Q,
B.3.1 Basic Position Coordinates and Vectors
B.3.2 Rotated Vectors
B.3.3 Solution
Upon substitution. eqs. (B. 1) to (B .3) become:
Adding eq. (£3 20) and eq. (B -2 1 ) and solving for t~ yields
r34 = A-sinR12 + B - s i n ~ ~ ~ + C - c o s ~ ~ ~ + D
where
Appendix B. Paramevic Equations - Solution of Joint Values
Then, substituting the expression for t 3 into eq. (B.22) and solving for CR,? gives
cRI2 = E.sRII + F . s o W + G . c u ~ + H (B.24)
Where
Finally, squaring both sides and using the relationship C'R~Z = 1 - s'Cli2 yields a quadratic for SR 2:
Where
The value of sR12 is then found as
For each possible value of R12 the value for T12 is found by substituting into eq. (B.20) as follows:
The value of is found by back substitution into eq. (B.23).
Appendix B. Parameuic Equations - Solution of Joint Values
B.4 Intersection of Edges and a
B.4.1 Basic Position Coordinates and Vectors
Pz= [-11,11,-I 11 Q I = [-9.9.91 p3 = [-i1,-11.111 Q~ = [-9.-9,-91 1\23 = [O, -a,ajT Al4 = [O.-a.-a] [I.J,K] = [c&, -asRz3 , [i,j,k] = [-~o~.t,-aso~~, a s ~ ~ ~ ] T
B.4.2 Rotated Vectors
Upon substitution. eqs. (B. 1) to (8 .3) become:
Adding eqs. (B .29) and (B -30) and solving for tlj yields
r , ~ = A-sinR3 +B-sinulj +C.cosol, -t D where
Appendix B. Parameuic Equations - SoIution of Joint Values
Then, substituting the expression for r l ~ into eq. (B.28) and solving for cLh3 gives
~ $ 2 ~ ~ = E -sQt3 + F .sw14 + G . c w I J + H t B.32)
Where
Finally, squaring both sides and using the relationship c'fi2, = 1- S ' R ~ yields a quadratic for sRZ3 :
Where
The value of S& is then found as
For each possible value of RZ3 the value for Tz3 is found by substituting into eq. (B.29) as follows:
The value of ri4 is found by back substitution into eq. (B.3 1).
Appendix B. Parametric Equations - Solution of Joint Values
B.5 Intersection of Edges -and aial
B.5.1 Basic Position Coordinates and Vectors
P3= [-I 1,-I 1,111 Q 1 = [-99.91 Pj = [1 I,11,11] Q2= [9.-9.91
T A33 = [a, a, O] hl 2 = [a.-a$] [LJ,K] = [as!&. - c x s Q ~ . - C Q W I ~ [i.j.k] = [ aml2 , orsol,, coi2,] T
B.5.2 Rotated Vectors
B.5.3 Solution
Upon substitution, eqs. (B. 1 ) to (B.3) become:
Adding eq. (B.36) and eq. (B.37) and solving for t l z yields
r 1 2 = A - s i n % + B-sin + C + c o s w 1 2 + D
w here
Appendix B. Parametric Equations - Solution of Joint Values
Then. substituting the expression for r12 into eq. (B.38) and solving for c!& gives
c n 3 , = E - s f i 3 , + F.SOJ~: + G - c o 1 2 + H (B.40)
Where
Finally. squaring both sides and using the relationship = 1- s'aw yields a quadratic for SR^^:
Where
The value of is then found as
For each possible value of RH the value for T34 is found by substiniting into eq. (B.36) as follows:
The value of 112 is found by back substitution into eq. (8.39).
Appendix B. Paramecric Equations - Solution of Joint Values
B.6 Intersection of Edges sp' and e?a,
B.6.1 Basic Position Coordinates and Vectors
B.6.2 Rotated Vectors
B.6.3 Solution
Upon substitution, eqs. (B. 1 ) to (B.3) become:
Adding eqs. (B.44) and (B.46) and solving for 124 yields
ri4 = A.sinRI; + B . s i n ~ ? ~ + C . c o s ~ ~ ~ + D
w here
Appendix B. Parametric Equations - Solution of Joint Values 1 O8
Then. substituting the expression for r2, into eq. (B.45) and solving for cR13 gives
Where
Finally. squaring both sides and using the relationship C'RI = 1 - ~ 3 2 1 ~ yields a quadratic for sQ13:
Where
The value of sR13 is then found as
For each possible value of RI, the value for TI' is found by substiniting into eq. ( B . U ) as follows:
The value of tz4 is found by back substitution into eq. (B.47).
Appendix C
Inverse Kinematic Equations and Their Jacobians
The following sets of equations relate to the solution to the inverse kinematics of
Tetradon given in chapter 5. Common components of these equations. dong with al1 of
their partial derivatives, are given first, followed by the constraint equations and their
Jacobians. Finaily. the six sets of objective functions for the procedure are given. also
with their Jacobian matrices.
C.l Common Components
The seven design variables found in this procedure are the seven elements of the
transformation matrix T:
where e is the axis about which the Q tetrahedron rotates. 9 is the rotations about that
a i s , k= 1-ce and [X, Y,ZI is the translation of the Q tetrahedron. The matnx T is central to
most of the constraint equations and the objective functions as it is used to define the
positions of the four vertices of the Q tetrahedron after is has been rotated and translated.
The four points of the Q tetrahedron in their final positions are defined as follows:
Appendix C. Inverse Kinematic Equations and Their Jacobians
C.l.l Joint Positions on Q
C.1.2 Derivatives of the Transformation Matrix
The partial denvatives of T and of the matrices defining the four points on the Q
tetrahedron are given here, as they are also integral to the consuaint equations and
objective func tions that follow.
Zke, ke, ke, --
Appendix C. Inverse Kinematic Equations and Their Jacobians
C.1.3 Derivatives of the Joint Positions on Q
Appendix C. Inverse Kinematic Equations and Their Jacobians
Point Q3: dQ3,.' -- - -9 - (a Tz l /d r , + d T ï / d r , - d T u / d r , ) + d TZ4/d-cr d x i
(C.9b)
Point Q4 dQ4/ -- - 9 . ( - d T 2 1 / d x i - d T E / d r , - d T 3 / d x i ) + d T 2 , / d x j (C-lOb) d x i
C.2 Constraints
There are four constraints imposed on Tetradon, and on the equation parameters in this
solution. The fmt three constraints relate the rotation and translation of the mobile vertex
to its desired position in space. The last one forces the algorithm to maintain the axis of
rotation, e, as a unit vector. The notation used for these equations will adhere to that
introduced in chapter 5, where the constraint equations are represented as g(x). The
function vector g is the vector of constraint equations and x is the vector of d l seven
design variables.
Appendix C. Inverse Kinernatic Equations and Their Jacobians
C.2.1 Constraint Equations
Where DP is the Destination Point for Q3.
C.22 Jacobian of Constraint Equations
The Jacobian of the constraint equations is expresses as G(x). The first three constraint
equations contain al1 seven design variables. thus their partial denvatives can be
expressed as:
where i= 1 to 7.
The last equation makes use of only the first three design variables. Its Jacobian is
expressed as
where i= l to 3
(C. 15)
(C. 16)
(C. 17)
(C. 18)
Appendix C. Inverse Kinematic Equations and Their Jacobians 113
C.3 Objective Functions
The general form of the equations used to find the distance between two lines is as
follows:
Where Pi is a point on the fixed P tetrahedron. Qj' is a point on the rotated and uanslated
Q teuahedron, A is a unit vector parallel to an edge on the P tetrahedron. h is a unit
vector parallel to an edge on the Q tetrahedron. T is the distance traveled frorn point Pi
along the edge on the P tetrahedron, t is the distance traveled from point Q, along the
edge of the Q tetrahedron. is the unit vector mutualiy perpendicular to both edges and ir-
is the distance between the two edges, measured along the vector o. The general solutions
for the components of eqs. (C. 19) through (C.2 1 ) follow:
The objective functions used in the algorithm are denoted as f(x) where the function
vector f is the vector of the six objective functions and x is the vector of the seven design
variables. Each objective function equates the distance rv between two edges to a design
Appendix C. Inverse Kinematic Equations and Thcir Jacobians I l 5
distance specified at the beginning of the program. The general fom al1 six objective
fiinctions is as follows:
where d is the prescnbed distance between edges.
The Jacobian matnx F(x) consists of the seven partial derivatives with respect to the
elements of x for al1 six objective hinctions.
C.3.1 Distance Between and
The components of equations [C. 191 to fC.2 11 needed for this solution are given as:
Substituting known values for P and A into eq. (C.20) above and solvin, for r r yields
QI! - 1 1 A =
A13, and B=-
O24, O24,
Substituting the value for iv into eq. (C. 19) and solving for t 13 gives:
Appendix C. Inverse Kinematic Equations and Their Jricobians
Finally, substituting the expression for r13 in eq. (C.78) into eq. (C.3 1 ) gives the foilowing
expression for 7'23:
Where E =QI: +Al3: . C + 1 1 - 0 ~ ~ - .&O?+ - B . C and F =cr+a2 , : B D - A , " D
The d u e s for t l 3 and w are then found by back substitution.
The partial derivatives of the elements off(^)^ are given below.
Using these values, the derivatives of the vector a c m be found as
where 1 = l l q 4 l l .
Appendix C. Inverse Kinernatic Equations and ïheir Jacobians 117
Generating the Jacobian F(x), is a matter of substituting the above partial derivatives into
the expressions for A. B. C . D. E and F given above. The final form of the Jacobian is
C.3.2 Distance Between and p?.el.
The components of equations [C. 191 to [C.3 1 ] needed for this solution are given as:
Q'Q;' (C.38)
Substituting known values for P and A into eq. (C.20) above and solving for w14 yields
Where
Substituting the value for wl4 into eq. (C. 19) and solving for 123 $ives:
Where
Appendix C. Inverse Kinematic Equations and Their Jacobians
Finally, substituting the expression for t 2 3 in eq. (C.28) into eq. (C.2 1 ) gives the following
expression for Tir:
The values for 123 and rv14 are then found by back substitution.
The partial derivatives of the elements of f(x)? are given below.
Using these values, the derivatives of the vector q4 can be found as
where I = llaljll.
Appendix C. Inverse Kinematic Equations and Their Jacobians 119
Generaring the Jacobian F(x), is a matter of substituting the above partial derivatives into
the expressions for A. B. C. D. E and F given above . The final form of the Jacobian is
C.3.3 Distance Between and p,Q,
The components of equations [C. 191 to [C.2 11 needed for this solution are given as:
Substituting known values for P and A into eq. (C.20) above and solving for r t p yields
Substituting the value for w12 into eq. (C. 19) and solving for r34 gives:
= Ci- D . TI?
Where
Appendix C. Inverse Kinematic Equations and Their Jacobians
Finally, substituting the expression for 134 in eq. (C.28) into eq. (C.2 1) gives the following
expression for Ti?:
Where E =QIF +Au, . C + l l - ~ ~ ~ , A - c + , . B - C and F =a+a12, 4-D-E., , D
The values for rw and r v l l are then found by back substitution.
The partial derivatives of the elements of f(x), are given below.
Usine these values. the derivatives of the vector 0 1 2 can be found as
where 1 = 1101 :Il.
Generating the Jacobian F(x)~ is a matter of substituting the above partial derivatives into
the expressions for A. B. C. D. E and F given above . The final f o m of the Jacobian is
Appendix C. Inverse Kinematic Equations and Their Jacobians
C.3.4 Distance Between and Q,.Q,.
The components of equations [C. 191 to fC.7 11 needed for this solution are given as:
Substituting known values for P and A into eq. (C.70) above and solving for h'23 yields
Where
Substituting the value for ~ 2 3 into eq. (C.19) and solving for r1.4 gives:
Where
Finally. substituting the expression for t l ~ in eq. (C.65) into eq. (C.2 1 ) gives the following
expression for f i3:
Appendix C. Inverse Kinematic Equations and Their Jacobians
The values for t l ~ and IV-; are then found by back substitution.
The partial denvatives of the elements of f(x)a are given below.
Using these values, the derivatives of the vector 023 can be found as
where 1 = llaz311.
Generating the Jacobian F ( x ) ~ is a matter of substituting the above partial denvatives into
the expressions for A, B, C. D, E and F given above. The final form of the Jacobian is
Appendix C. Inverse Kinematic Equations and Their iacobians
C.3.5 Distance Between and * Q? *
The components of equations [C. 191 to [C.Z 11 needed for this solution are given as:
Substituting known values for P and A into eq. (C.20) above and solving for w14 yields
1 ~ 3 4 = A + B . ~ ~ ? (C.76)
Where
Substituting the value for WY into eq. (C. 19) and solving for r 1 2 gives:
Where
Finally, substituting the expression for in eq. (C.77) into eq. (C.2 1 ) gives the following
expression for T 3 ~ :
Appendix C. Inverse finematic Equations and Their Jricobians
Where E = Q , , + À i 2 , -C+ I I -oy , - A - q 4 , . B - C and F=a+oy, B - D - d l , , D
The values for t l z and w34 are then found by back substitution.
The partial derivatives of the elements off(x)5 are given below.
Using these values, the derivatives of the vector 034 cm be found as
where 1 = lI03.Jl.
Generating the Jacobian F(X)~ is a matter of substituting the above partial derivatives into
the expressions for A. B, C. D, E and F given above . The final form of the Jacobian is
d A d B F(x), = - r12 +-.r,-> + B . - d x , d x ; XI
Appendix C. Inverse Kinematic Equations and Their Jacobians
& C3.6 Distance Between qq and p,.p,.
The components of equations [C. 191 to [C.2 1 ] needed for this solution are given as:
Substituting known values for P and A into eq. (C.20) above and solving for i t t 1 3 yields
~ 1 3 = . 4 + B - t 2 4 (C.88)
Where
Q?,. + 1 1 A = -
Ar4, and B = -
O13, * 13,
Substituting the value for wl3 into eq. (C. 19) and solving for rzJ gives:
Where
Finally. substituting the expression for tz4 in eq. (C.89) into eq. (C.2 1) gives the following
expression for Tl 3:
Appendix C. Inverse Kinematic Equations and Their Jacobians
The values for t 2 ~ and w l 3 are then found by back substitution.
The partial derivatives of the elements off(^)^ are given below.
Using these values. the derivatives of the vector 013 c m be fo
where i = lloi311.
Generating the Jacobian F(x)~ is a matter of substituting the above partial denvatives into
the expressions for A, B, C, D, E and F given above . The final form of the Jacobian is
IMAGE EVALUATION TEST TARGET (QA-3)
APPLIED 2 IMAGE. lnc S 1653 East Main Street - -. - Rochester. NY 14609 USA -- -- - - Phone: 716i482-0300 -- -- - - Fax: 71 6/288-5989
Q 1993. Appiied Image. Inc.. Alt Rtghts Aeservecl