DIRECT AND INVERSE KINEMATICS OF Anlc-bnc.ca/obj/s4/f2/dsk2/ftp01/MQ29634.pdf · Rhonda will never be forgotten and cannot be repaid. 1 love you Rhon. Claim of Originality The mechanism

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


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


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.


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:


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.


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.


*\ ;. : 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


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


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.


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.


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.


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


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


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.


< 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


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.


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


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:


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.


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


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


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


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.


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)


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


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


Figure 4.6 Second rotation - Q2' to Qz"

The final rotation is then calculated as

R=R2RI


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:


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 :


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


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.


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.


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:


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.


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


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


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:


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,


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:


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.


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


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


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


B.3 Intersection of Edges %and Q,Q,



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


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


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


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


B.5 Intersection of Edges -and aial


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


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


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, --


C.1.3 Derivatives of the Joint Positions on Q


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


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


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


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:


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


& 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:


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

Documents

DIRECT AND INVERSE KINEMATICS OF Anlc-bnc.ca/obj/s4/f2/dsk2/ftp01/MQ29634.pdf · Rhonda will never be forgotten and cannot be repaid. 1 love you Rhon. Claim of Originality The mechanism