AN2013.01 - Calculating Angles Between Two YEI 3-Space Sensor !

e"ices on a #u$an Bo%&'

1. Intro%uction
This application note provides the mathematics and reference source code for calculating the angle between two YEI 3-Space Sensor devices. This is especially useful for organic motion-capture, biomechanics studies, range-of-motion studies, sports studies, and ergonomics studies since it is possible to e tract human !oint-angles from body-worn sensors. The YEI 3-Space Sensor is a miniature, high-precision, high-reliability, "ttitude and #eading $eference System %"#$S& ' Inertial (easurement )nit %I()& in a single low-cost end-use-ready device. The "ttitude and #eading $eference System %"#$S& ' Inertial (easurement )nit %I()& uses tria ial gyroscope, accelerometer, and compass sensors in con!unction with advanced processing and on-board *uaternion-based +alman filtering algorithms to determine orientation relative to an absolute reference in real-time. ,rientation can also be returned relative to a designated reference orientation. This ma-es 3-Space Sensor placement and alignment easier since the devices can ma-e use of arbitrarily defined .ero-identity orientations which ma-es perfect physical alignment unnecessary and reduces the difficulty in e tracting desired output angles. The 3-Space Sensor devices can return orientation in a number of formats, including as forward and down vectors, thus ma-ing it simple to calculate the angle between two of these devices. #owever, many surfaces, such as those of the human body, may not be flat or smooth, and, thus, we must be able to compensate for the possibility of imperfect sensor placement and alignment. /e can use the devices0 *uaternion orientation output and *uaternion operations to account for the human body0s irregularities and obtain more accurate forward and down vectors. The 1ython language source code listed within this document is in three parts and contains crossreferences to the e*uations used. These listings contain all the code needed to return the angle or angles %in radians and degrees& between two 3-Space Sensor devices for a variety of possible !oint configurations. 2or convenience, this document assumes the use of two 3-Space Sensor /ireless 3.45#. 6SSS devices that are being communicated via )S7. Since calculating different !oint angles re*uires different sensor placement, this application note uses two possible sensor placement configurations. In the first configuration, the devices are mounted on the right upper-arm and fore-arm and are positioned to be lined up with the 8E6 lights towards the shoulder. In the second configuration, one device is mounted in the middle of the bac- with the 8E6 light up towards the head and the other device is mounted on the right upper-arm with the 8E6 light towards the shoulder. "dditionally, prior to running the code, both configurations re*uire the sensors to be properly mounted and the person to be in the standard T-pose, arms straight out to the side of the body and the palms facing forward. "ll vectors used are unit vectors and all *uaternions used are unit *uaternions.

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<'34

3-Space Sensor Commands Used

There are four 3-Space Sensor commands used in the 1ython source code. Co$$an% 0(00= This command gets the filtered tared orientation of the device as a *uaternion. Co$$an% 0(0)= This command gets the filtered orientation of the device as a *uaternion. Co$$an% 0(0C= This command gets the filtered orientation of the device as two vectors, where the first vector refers to >orth and the second refers to 5ravity. These vectors are given in the device0s reference frame and not the global reference frame. Co$$an% 0()1= This command sets the tare orientation of the device to be the same as the supplied orientation, which should be passed as a *uaternion.

2. T&pes o* +oints in t,e #u$an Bo%&

There are five types of !oints in the human body. 3-Space Sensor devices can be used to detect the motions of and e tract angles from each of these !oint types.

2.1 Hinge Joint

" hinge !oint acts much li-e a hinge on a door. They allow for bac- and forth movement around the a is of the !oints, but do not allow side to side or lateral movements. #inge !oint e amples in the human body are the elbows, -nees, and the middle and end !oints of the fingers and toes.

-igure 1 . T&pical #inge +oint

2.2 Ball and Socket Joint

" ball and soc-et !oint %or spheroidal !oint& is a !oint in which a ball-shaped surface of one bone is connected to a corresponding soc-et-shaped recess of another bone. This configuration allows for movement around multiple a es in almost any direction. 7all and soc-et !oint e amples in the human body are the hip !oints and the shoulder !oints.

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

3'34

-igure 2 . T&pical Ball an% Soc/et +oint

2.3 Ellipsoid Joint

"n ellipsoid !oint %or condyloid !oint& allows for angular, bending movements but with limited rotation. So it is similar to the movements of a ball and soc-et !oint with lesser magnitude. Ellipsoid !oint e amples in the human body are the wrist !oints and an-le !oints.

2.4 Pivot Joint

" pivot !oint allows for rotation around a single a is. 1ivot !oint e amples in the human body are the necand forearms.

-igure 3 . T&pical 0i"ot +oint

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

3'34

2. Saddle Joint
" saddle !oint allows for the same movements as the ellipsoid !oint. " saddle !oint e ample in the human body is the carpometacarpal !oint of the thumbs.

3. 1at,e$atical an% Algorit,$ic -oun%ations

3.1 !lgorit"ms #or Calc$lating t"e !ngle Bet%een &%o 'ectors
!ot 2Inner3 0ro%uct
)sing the properties of vectors, the dot product can be used to calculate the angle between two vectors. v 0 = (i x : , ( y : , k z: ) v 1 = ( i x < , ( y < , k z <) v 0v 1 = x : x < + y : y < + z : z <
= v :v <cos

E)n. 1 E)n. 2 E)n. 3 E)n. 4

>ow using some algebra we can combine E)$ation 3 and E)$ation 4 into an e*uation defining ?.
v :v <cos = x : x < y : y < z : z < x x y: y< z: z< = arccos : < v :v <

E)n. E)n. *

/e can simplify E)$ation * -nowing that the lengths of the vectors are <. = arccos x : x < y : y < z : z < E)n. +

So ? represents the angle between these two vectors. This algorithm can be applied to the forward and down vectors received from the 3-Space Sensor devices to calculate the angle between them.

Cross 24uter3 0ro%uct

)sing the properties of vectors, the cross product can be used to calculate a vector perpendicular to two vectors and the angle between the two vectors. v 0 v 1 = ( i ( y : z < z : y < ) , ( ( z : x < x: z < ) , k ( x : y < y : x< ) ) =v 0v1sin n E)n. , E)n. -

/here n is a unit vector perpendicular to v0 and v1, and where ? is the angle between them. To calculate ?, remember that the vectors are unit vectors so the lengths of the vectors are <. Ta-e note however, that the cross vector may not have a length of < due to ?. v 0 v 1 =v 0v 1sin n =v 0v 1sin n 93::;-3:<3 Yost Engineering Inc. 1atents 1ending E)n. 1. E)n. 11 4'34

sin =v 0 v 1 = arcsin v 0v 1

E)n. 12 E)n. 13

So ? represents the angle between these two vectors. This algorithm can be applied to the forward and down vectors received from the 3-Space Sensor devices to calculate the angle between them.

3.2 !lgorit"ms #or /$aternion 0perations

" *uaternion, ), is a fourth dimensional vector that can be interpreted as a third dimensional rotation.
) = ( i x , ( y , k z , w) = $ sin / 3 , cos / 3 = (v , w)

E)n. 14 E)n. 1 E)n. 1*

2or E)$ation 1 , $ is a vector defined as= $ = (i , ( , k ) E)n. 1+

5uaternion Con6ugate an% In"erse

2inding the con!ugate of a *uaternion, )1, is easily done by negating the imaginary numbers or the vector part of the *uaternion. "nd since all *uaternions in this application note are unit *uaternions, the con!ugate of a *uaternion is e*ual to the inverse of the *uaternion, )-1. ) ' = )1 = (i x , ( y , k z , w ) E)n. 1,

5uaternion 1ultiplication
) 0 = ( v 0 , w: ) ) 1 = ( v 1 , w< ) ) 0 )1 = ( w : v1 +w < v 0 +v 0 v1 , w :+w <v 0v 1) E)n. 1E)n. 2. E)n. 21

5uaternion 7ector 1ultiplication

To rotate a vector by a *uaternion, we must use pure *uaternions, p, which are *uaternions with its real part as : and con!ugacy. So any vector can be made into a pure *uaternion by putting the vector in the vector part and : in the real part of a *uaternion. The return value of the con!ugacy is a pure *uaternion, which can be interpreted as a vector by ignoring the real part.
p = (v , :) vr = ) p ) '

E)n. 22 E)n. 23

3.3 Compensation #or t"e H$man Bod2

"s mentioned earlier the human body is not flat nor smooth, and ma-es perfect placement and alignment of 3-Space Sensor devices difficult. Thus, a method of correction is needed to accommodate imperfect sensor alignment issues. To do this we are going to use the device0s >orth and 5ravity vectors, specifically the 5ravity vector. The 5ravity vector denotes the direction in which the device thin-s gravity is pulling on it. If the human body was flat and smooth, the 5ravity vector would line up with one of the @artesian coordinate a es of the device0s reference frame when in the starting position. So in order to fi this we 93::;-3:<3 Yost Engineering Inc. 1atents 1ending A'34

must calculate the 5ravity vector0s offset from the @artesian coordinate a is we denote as the true gravity vector and correct for it.
= arccos g dg a = g dg ) o = a sin / 3 , cos / 3

E)n. 24 E)n. 2 E)n. 2*

/here gd is the 5ravity vector from the device and g is the gravity vector we want, a is a unit vector that denotes the a is of rotation from gd to g, and )o is the rotational offset as a *uaternion. +nowing this, all we need to do is offset gd so it is lined up with the g. This is done by multiplying the filtered orientation of the device by )o and set the result as the tare orientation of the device. >ote that we must retain this offset so that we can post-multiply the orientation of the device by it later.

3.4 Calc$lating t"e 'ectors o# a 3-Space Sensor 3evice

The following are the vectors that will be used to calculate the angles of the !oints and the methods used to calculate these vectors.

-orwar% 7ector
To calculate the forward vector of a device, we are going to use the filtered tared orientation of the device and a vector in the device0s reference frame that will be denoted as the forward vector. $emember the rotational offset of the device must be applied to the filtered tared orientation before calculating the forward vector of the device. v 4 = )t )o v E)n. 2+

/here )t is the filtered tared orientation of the device, v4 is the device0s forward vector in global-space, and v is the unrotated forward vector in the sensor0s space.

!own 7ector
To calculate the down vector of a device, we are going to use the filtered tared orientation of the device and a vector in the device0s reference frame that will be denoted as the down vector. $emember the rotational offset of the device must be applied to the filtered tared orientation before calculating the down vector of the device.
v 3 = )t )o v

E)n. 2,

/here )t is the filtered tared orientation of the device, v3 is the device0s down vector in global-space, and v is the unrotated down vector in the sensor0s space.

8p 7ector
To calculate the up vector of a device, a simple negation of the calculated down-vector is all that is necessary.

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

B'34

v U = v 3

E)n. 2-

9ig,t 7ector
To calculate the right vector of a device, we are going to use the forward and down vectors of the device and perform the cross product on them.
v 5 = v 4 v 3

E)n. 3.

/here v5 is the right vector of the device. "lso, note that the forward and down vectors already have the compensation applied to them so v5 will also already compensated and that the 3-Space Sensor uses a lefthanded space by default.

3. 6et"ods #or Calc$lating t"e !ngle7s8 o# t"e Joints

Calculating t,e Angle o* a #inge +oint
This section will discuss how to calculate the angle of the hinge !oint of the right arm using the first setup of the 3-Space Sensor devices. /e are going to use the forward vector of the devices, where v4. is the forward vector of the first device and v41 is the forward vector of the second device. "fter calculating the forward vectors, we will also need to calculate the up vector from the device that is being used as the reference device, in this case the first device. This vector, vU., will help in determining the sign of the angle. >ow using the forward vectors from the devices and the up vector we can calculate the angle between the devices.
= arccos v 41v 4. a = v 41 v 4. = copysign , vU.a

E)n. 31 E)n. 32 E)n. 33

Ta-e note that arccos will always return a positive value, so we must use the dot product of a and vU. to calculate the sign of ?. The function copysign, is a function that returns the first parameter with the same sign as the second parameter, so using the dot product is perfect because it ranges from -< to <.

Calculating t,e Angle o* a 0i"ot +oint

This section will discuss how to calculate the angle of the pivot !oint of the right arm using the first setup of the 3-Space Sensor devices. /e are going to use the down vector of the devices, where v3. is the down vector of the first device and v31 is the down vector of the second device. "fter calculating the down vectors, we will also need to calculate the forward vector from the device that is being used as the reference device, in this case the first device. This vector, v4., will help in determining the sign of the angle. >ow using the down vectors from the devices and the forward vector we can calculate the angle between the devices.
= arccos ( v 31v 3. ) a = v 31v 3.

E)n. 34 E)n. 3 1atents 1ending ;'34

93::;-3:<3 Yost Engineering Inc.

 = copysign , v 4.a

E)n. 3*

Ta-e note that arccos will always return a positive value, so we must use the dot product of a and v4. to calculate the sign of ?. The function copysign, is a function that returns the first parameter with the same sign as the second parameter. )sing the dot product is perfect because it ranges from -< to <.

Calculating t,e 0itc,: Yaw: an% 9oll Angles o* 1ulti-A(is +oints

This section will discuss how to calculate the pitch, yaw, and roll angles of a multi-a is !oint such as the ball and soc-et !oint of the right arm using the second setup of the 3-Space Sensor devices. /e will be using the method described in C<D and the forward, down, and right vectors of the devices to derive these angles. /here v4., v3., and v5. are respectively the forward, down, and right vectors of the first device which will be the reference device and v41, v31, v51 are respectively the forward, down, and right vectors of the second device. The method ta-es the orientation of the arm and performs rotations in a precise order to move the arm bac- to its initial orientation to derive the pitch, yaw, and roll angles. 7efore we start deriving the angles, we must transform v41 to be on the same transverse plane as v4.. To do this we will use the right vector of each device and calculate a *uaternion that will rotate v51 to line up with v5.. = arccos v 51v 5. a = v 51v 5. ) 5o = a sin / 3 , cos / 3 E)n. 3+ E)n. 3, E)n. 3-

The *uaternion )5o, is the rotational offset for the right vectors. >ow we can start decomposing the angles. The first step in decomposing the angles is to undo any rotations on the vertical a is %this will be the yaw angle&. So we will be using the forward vector of each device to calculate the yaw angle. #owever, we must first apply the offset )5o to v41. v &41 = ) 5o v 41 The vector v&41, is the transformed vector of v41. >ow calculate the angle between v&41 and v4..
yaw = arccos v &41v 4.

E)n. 4.

E)n. 41

The angle ?yaw, is the yaw angle of the !oint. >ow we must undo this rotation by calculating a *uaternion and transforming v41 again, but this time to be on the same frontal plane as v4.. /e must also transform v31 to be used for a later calculation. a = v &41v 4. ) = a sin yaw / 3 , cos yaw / 3 v &41 = ) v 41 v &31 = ) v 31 E)n. 42 E)n. 43 E)n. 44 E)n. 4

The *uaternion );, is the rotational offset of ?yaw. >ow ta-e note that arccos will always return a positive value, so we must use the dot product of a and v5. to calculate the sign of ?yaw.

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

E'34

 yaw = copysign yaw , av 5.

E)n. 4*

The function copysign, is a function that returns the first parameter with the same sign as the second parameter, so using the dot product is perfect because it ranges from -< to <. >e t we must undo any rotations on the frontal hori.ontal a is %this will be the pitch angle&. So we will be using v&41 and v4. to calculate the pitch angle.
pitch = arccos v &41v 4.

E)n. 4+

The angle Fpitch, is the pitch angle of the !oint. >ow we must undo this rotation by calculating a *uaternion and transforming v&31 again, but this time so it will be on the same sagittal plane as v3.. a = v &41v 4. ) = a sin pitch / 3 , cos pitch / 3 v &31 = ) v &31 The *uaternion )<, is the rotational offset of Fpitch. >ow we must use the dot product of a and v3. to calculate the sign of Fpitch.
pitch = copysign picth , av 3.

E)n. 4, E)n. 4E)n. .

E)n. 1

2inally calculate any rotations on the sagittal hori.ontal a is %this will be the roll angle&. /e will be using v&31 and v3. to calculate the roll angle. roll = arccos v &31v 3. E)n. 2

The angle Groll, is the roll angle of the !oint. >ow we must calculate the sign of Groll using the cross product of v&31 and v3., a, and the dot product of a and v4.. a = v &31v 3. E)n. 3 roll = copysign roll , av 4. E)n. 4 The planes and a es mentioned are described in C3D. "nd as mentioned in C<D, there are two positions in which the so called Hgimbal-loc-I will occur. Those positions are when the arm is straight up or straight down, ma-ing ?yaw and Groll ambiguous.

=. So*tware I$ple$entation
The reference 1ython code in this documentation is written in 1ython 3.;, uses the internal libraries math and struct, and the e ternal library, serial, which is provided by 1ySerial 3.B. " custom library, threespace, has functions for calculating the angles and performing vector ans *uaternion operations. @ommunication with the 3-Space Sensor devices is done with 1ySerial0s Serial class. It ta-es the @,( port name the device was given by the computer when plugged in to connect to it for writing and reading data to and from the device.

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

J'34

4.1 Connecting to a 3-Space Sensor 3evice P2t"on So$rce Code

To connect a 3-Space Sensor devices, you need to -now the @,( port name the device is on. This can be found in the 6evice (anager or in the 3-Space Sensor Software Suite. This @,( port name is used for creating a Serial class ob!ect to communicate to the 3-Space Sensor device.
def open1ort%comKport&= try= serialKport L serial.Serial%comKport, timeoutL:.<, writeTimeoutL:.<, baudrateL<<A3::& return serialKport e cept E ception as e = print M2ailed to create a serial port on port=M, comKport raise e

4.2 Comm$nicating %it" a 3-Space Sensor 3evice P2t"on So$rce Code

The 3-Space Sensor device has a list of commands for getting and setting data. These commands must be sent to the 3-Space Sensor device through a command pac-et that must be created, and the serial port must read the full amount of bytes returned from the 3-Space Sensor device. The command pac-et is comprised of a header byte, the command byte%s&, input data byte%s&, and a chec-sum byte. The construction of these command pac-ets and the amount of bytes to be read can be found in the 3-Space Sensor device0s )ser (anual.
def command/rite$ead%serialKport, command, byteKsi.eL:, dataKformatL00, inputKdataLCD&= MMM /rites and reads data to and from a serial port given. "rgs= serialKport= " Serial ob!ect that is communicating with a 3-Space Sensor device. command= " char string of one of the 3-Space Sensor device0s commands. byteKsi.e= The number of bytes to read from the serial port. dataKformat= The format for which struct is to pac- or unpac- data. inputKdata= 6ata to be sent to the 3-Space Sensor device. MMM dataKstr L 00 if len%inputKdata& N := dataKstr L struct.pac-%dataKformat, OinputKdata& commandKdata L ST"$TK7YTE P command P dataKstr P create@hec-Sum%command P dataKstr& try= serialKport.write%commandKdata& e cept E ception as e = print MThere was an error writing command to the portM, serialKport.name raise e if byteKsi.e N := try= dataKstr L serialKport.read%byteKsi.e& e cept E ception as e = print MThere was an error reading from the portM, serialKport.name raise e outputKdata L list%struct.unpac-%dataKformat, dataKstr&& return outputKdata return >one

4.3 !ngle Calc$lation P2t"on So$rce Code

The angle calculation function is listed below and returns the angle in radians. It uses the methods stated in the sections Calc$lating t"e !ngle o# a Hinge Joint and Calc$lating t"e !ngle o# a Hinge Joint.
def calculate"ngle%vec:, vec<, vec3L>one&= MMM @alculates the angle between the two given vectors using the dot product. "rgs=

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<:'34

vec:= " unit vector. vec<= " unit vector. vec3= " unit vector perpendicular to vec: and vec<. MMM QQ The ma and min is used to account for possible floating point error dotKproduct L ma %min%vector6ot%vec:, vec<&, <.:&, -<.:& angle L math.acos%dotKproduct& if vec3 is not >one= a is L vector>ormali.e%vector@ross%vec:, vec<&& angle L math.copysign%angle, vector6ot%vec3, a is&& return angle

4.4 Pitc"9 :a%9 and 5oll Calc$lation P2t"on So$rce Code

The function below is used to calculate the pitch, yaw, and roll angles in radians. It uses the method stated in section Calc$lating t"e Pitc"9 :a%9 and 5oll !ngles o# 6$lti-!;is Joints.
def calculate1itchYaw$oll%forward:, down:, forward<, down<&= MMM @alculates the pitch, yaw, and roll angles using the forward and down vectors calculated from two 3-Space Sensor devices. "rgs= forward:= " unit vector that denotes the forward vector of the first 3-Space Sensor device. down:= " unit vector that denotes the down vector of the first 3-Space Sensor device. forward<= " unit vector that denotes the forward vector of the second 3-Space Sensor device. down<= " unit vector that denotes the down vector of the second 3-Space Sensor device. MMM QQ "ssumes the devices0 a is directions are default %RYS& and are positioned or has had its QQ orientation manipulated so that the $ight a is is up QQ 2irst, calculate the right vector for both devices using the forward and down vectors right: L vector>ormali.e%vector@ross%forward:, down:&& right< L vector>ormali.e%vector@ross%forward<, down<&& QQ Second, calculate the angle between the right vectors and a vector perpendicular to them angle L calculate"ngle%right<, right:& a is L vector>ormali.e%vector@ross%right<, right:&& QQ Third, create a *uaternion using the calculated a is and angle that will be used to transform the QQ forward vector of the second device so that it is on the same hori.ontal plane as the forward QQ vector of the first device *uat L createTuaternion%a is, angle& transformedKforward< L vector>ormali.e%*uaternionUector(ultiplication%*uat, forward<&& QQ 2ourth, calculate the angle between the transformed forward vector and the forward vector of the QQ first device QQ This angle is the yaw yaw L calculate"ngle%transformedKforward<, forward:& QQ 2ifth, calculate a vector perpendicular to the transformed forward vector and the forward vector QQ of the first device a is L vector>ormali.e%vector@ross%transformedKforward<, forward:&& QQ Si th, create a *uaternion using the calculated a is and yaw angle that will be used to transform QQ the forward vector of the second device so that it is on the same vertical plane as the forward QQ vector of the first device and to transform the down vector of the second device to be used in a QQ later calculation *uat L createTuaternion%a is, yaw& transformedKforward< L vector>ormali.e%*uaternionUector(ultiplication%*uat, forward<&& transformedKdown< L vector>ormali.e%*uaternionUector(ultiplication%*uat, down<&& QQ Set the sign of yaw using the a is calculated and the right vector of the first device yaw L math.copysign%yaw, vector6ot%a is, right:&& QQ Seventh, calculate the angle between the transformed forward vector and the forward vector of QQ the first device QQ This angle is the pitch pitch L calculate"ngle%transformedKforward<, forward:&

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<<'34

QQ Eighth, calculate a vector perpendicular to the transformed forward vector and the forward vector QQ of the first device a is L vector>ormali.e%vector@ross%transformedKforward<, forward:&& QQ >inth, create a *uaternion using the calculated a is and pitch angle that will be used to transform QQ the transformed down vector so that it is on the same vertical plane as the down vector of the first QQ device *uat L createTuaternion%a is, pitch& transformedKdown< L vector>ormali.e%*uaternionUector(ultiplication%*uat, transformedKdown<&& QQ Set the sign of pitch using the a is calculated and the down vector of the first device pitch L math.copysign%pitch, vector6ot%a is, down:&& QQ Tenth, calculate the angle between the transformed down vector and the down vector of the first QQ device QQ This angle is the roll roll L calculate"ngle%transformedKdown<, down:& a is L vector>ormali.e%vector@ross%transformedKdown<, down:&& QQ Set the sign of roll using the a is calculated and the forward vector of the first device roll L math.copysign%roll, vector6ot%a is, forward:&& return Cpitch, yaw, rollD

4. Compensation 0##set P2t"on So$rce Code

The function below is used to calculate the offset of the 3-Space Sensor device on the human body. The function returns the offset orientation as a *uaternion.
def offsetTuaternion%serialKport, gravityLC-<.:, :.:, :.:D, initKoffsetL>one&= MMM @alculates the offset of the 3-Space Sensor device on the human body. "rgs= serialKport= " Serial ob!ect that is communicating with a 3-Space Sensor device. gravity= " unit vector that denotes the gravity direction the 3-Space Sensor device should be reading. initKoffset= " unit *uaternion the denotes a rotational offset for a 3-Space Sensor device. MMM QQ 2irst, find what the device reads as the gravity direction using the read >orth 5ravity command QQ The command returns B floats, the first 3 ma-e the >orth vector and the last 3 ma-e the 5ravity QQ vector northKgravity L command/rite$ead%serialKport, $E"6K>,$T#K5$"UITYK@,((">6, byteKsi.eL34, dataKformatL0Nffffff0& sensorKgravity L northKgravityC3=D QQ Second, read the current filtered orientation as a *uaternion from the device using the read QQ 2iltered Tuaternion command filtKdata L command/rite$ead%serialKport, $E"6K2I8TKT)"TK@,((">6, byteKsi.eL<B, dataKformatL0Nffff0& QQ Third, using the gravity vector given and the 5ravity vector from the device, calculate the angle QQ between them and a vector perpendicular to them angle L calculate"ngle%sensorKgravity, gravity& a is L vector>ormali.e%vector@ross%sensorKgravity, gravity&& QQ 2ourth, create a *uaternion using the calculated a is and angle that will be used to offset the QQ filtered *uaternion of the device so the gravity vectors will line up QQ "lso apply the initial offset if any offset L createTuaternion%a is, -angle& if initKoffset is not >one= offset L *uaternion(ultiplication%offset, initKoffset& tareKdata L *uaternion(ultiplication%filtKdata, offset& QQ 2ifth, set the offset filtered *uaternion as the tare orientation for the device using the set Tare QQ Tuaternion command command/rite$ead%serialKport, SETKT"$EKT)"TK@,((">6, dataKformatL0Nffff0, inputKdataLtareKdata& QQ The calculated offset *uaternion is returned because it needs to be applied to the filtered tared

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<3'34

QQ *uaternion received from the device return offset

4.* 4$ll P2t"on So$rce Code

-ull Source Co%e o* t,e Custo$ >i?rar&
QV'usr'bin'env python3.; QQ QQ Script >ame= threespace.py QQ QQ "pplication >ote= @alculating "ngles 7etween Two YEI 3-Space Sensor 6evices using Two Uectors QQ on a #uman 7ody QQ QQ 6escription= #elper functions for calculating the angles between two YEI 3-Space Sensor devices in QQ 1ython 3.; QQ QQ /ebsite= www.3-space-sensor.com QQ QQ @opyright= @opyright %@& 3:<3 Yost Engineering, Inc. QQ 1ermission is hereby granted, free of charge, to any person obtaining a copy of this software QQ and associated documentation files %the MSoftwareM&, to deal in the Software without QQ restriction, including without limitation the rights to use, copy, modify, merge, publish, QQ distribute, sublicense, and'or sell copies of the Software, and to permit persons to whom the QQ Software is furnished to do so, sub!ect to the following conditions= QQ The above copyright notice and this permission notice shall be included in all copies or QQ substantial portions of the Software. QQ T#E S,2T/"$E IS 1$,UI6E6 M"S ISM, /IT#,)T /"$$">TY ,2 ">Y +I>6, QQ ER1$ESS ,$ I(18IE6, I>@8)6I>5 7)T >,T 8I(ITE6 T, T#E /"$$">TIES QQ ,2 (E$@#">T"7I8ITY, 2IT>ESS 2,$ " 1"$TI@)8"$ 1)$1,SE ">6 QQ >,>I>2$I>5E(E>T. I> >, EUE>T S#"88 T#E ")T#,$S ,$ @,1Y$I5#T QQ #,86E$S 7E 8I"78E 2,$ ">Y @8"I(, 6"("5ES ,$ ,T#E$ 8I"7I8ITY, QQ /#ET#E$ I> "> "@TI,> ,2 @,>T$"@T, T,$T ,$ ,T#E$/ISE, "$ISI>5 QQ 2$,(, ,)T ,2 ,$ I> @,>>E@TI,> /IT# T#E S,2T/"$E ,$ T#E )SE ,$ QQ ,T#E$ 6E"8I>5S I> T#E S,2T/"$E. QQ import serial import struct import math QQ Static Uariables QQ ST"$TK7YTE L chr%: f;& $E"6K2I8TKT"$E6KT)"TK@,((">6 L chr%: ::& $E"6K2I8TKT)"TK@,((">6 L chr%: :B& $E"6K>,$T#K5$"UITYK@,((">6 L chr%: :c& SETKT"$EKT)"TK@,((">6 L chr%: B<& def vector@ross%vec:, vec<&= MMM 1erforms the cross product on the two given vectors. "rgs= vec:= " unit vector. vec<= " unit vector. MMM :, y:, .: L vec: <, y<, .< L vec< return Cy: O .< - .: O y<, .: O < - : O .<, : O y< - y: O <D def vector6ot%vec:, vec<&= MMM 1erforms the dot product on the two given vectors. "rgs= vec:= " unit vector. vec<= " unit vector. MMM :, y:, .: L vec:

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<3'34

<, y<, .< L vec< return : O < P y: O y< P .: O .< def vector8ength%vec&= MMM @alculates the length of a vector given. "rgs= vec= " vector. MMM return %vector6ot%vec, vec& OO :.A& def vector>ormali.e%vec&= MMM >ormali.es the vector given. "rgs= vec= " vector. MMM length L vector8ength%vec& , y, . L vec return C ' length, y ' length, . ' lengthD def createTuaternion%vec, angle&= MMM @reates a *uaternion from an a is and an angle. "rgs= vec= " unit vector. angle= "n angle in radians. MMM QQ Tuaternions represent half the angle in comple space so the angle must be halved , y, . L vec tmpK*uat L C:.:D O 4 tmpK*uatC:D L O math.sin%angle ' 3.:& tmpK*uatC<D L y O math.sin%angle ' 3.:& tmpK*uatC3D L . O math.sin%angle ' 3.:& tmpK*uatC3D L math.cos%angle ' 3.:& QQ >ormali.e the *uaternion * , *y, *., *w L tmpK*uat length L %* O * P *y O *y P *. O *. P *w O *w& OO :.A tmpK*uatC:D 'L length tmpK*uatC<D 'L length tmpK*uatC3D 'L length tmpK*uatC3D 'L length return tmpK*uat def *uaternion(ultiplication%*uat:, *uat<&= MMM 1erforms *uaternion multiplication on the two given *uaternions. "rgs= *uat:= " unit *uaternion. *uat<= " unit *uaternion. MMM :, y:, .:, w: L *uat: <, y<, .<, w< L *uat< Kcross, yKcross, .Kcross L vector@ross%C :, y:, .:D, C <, y<, .<D& wKnew L w: O w< - vector6ot%C :, y:, .:D, C <, y<, .<D& Knew L < O w: P : O w< P Kcross yKnew L y< O w: P y: O w< P yKcross .Knew L .< O w: P .: O w< P .Kcross return C Knew, yKnew, .Knew, wKnewD def *uaternionUector(ultiplication%*uat, vec&= MMM $otates the given vector by the given *uaternion.

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<4'34

"rgs= *uat= " unit *uaternion. vec= " unit vector. MMM QQ 1rocedure= *uat O vecK*uat O -*uat * , *y, *., *w L *uat v , vy, v. L vec vw L :.: negK* L -* negK*y L -*y negK*. L -*. negK*w L *w QQ 2irst (ultiply Kcross, yKcross, .Kcross L vector@ross%C* , *y, *.D, vec& wKnew L *w O vw - vector6ot%C* , *y, *.D, vec& Knew L v O *w P * O vw P Kcross yKnew L vy O *w P *y O vw P yKcross .Knew L v. O *w P *. O vw P .Kcross QQ Second (ultiply Kcross, yKcross, .Kcross L vector@ross%C Knew, yKnew, .KnewD, CnegK* , negK*y, negK*.D& w L wKnew O negK*w - vector6ot%C Knew, yKnew, .KnewD, CnegK* , negK*y, negK*.D& L negK* O wKnew P Knew O negK*w P Kcross y L negK*y O wKnew P yKnew O negK*w P yKcross . L negK*. O wKnew P .Knew O negK*w P .Kcross return C , y, .D def calculate"ngle%vec:, vec<, vec3L>one&= MMM @alculates the angle between the two given vectors using the dot product. "rgs= vec:= " unit vector. vec<= " unit vector. vec3= " unit vector perpendicular to vec: and vec<. MMM QQ The ma and min is used to account for possible floating point error dotKproduct L ma %min%vector6ot%vec:, vec<&, <.:&, -<.:& angle L math.acos%dotKproduct& if vec3 is not >one= a is L vector>ormali.e%vector@ross%vec:, vec<&& angle L math.copysign%angle, vector6ot%vec3, a is&& return angle def calculate1itchYaw$oll%forward:, down:, forward<, down<&= MMM @alculates the pitch, yaw, and roll angles using the forward and down vectors calculated from two 3-Space Sensor devices. "rgs= forward:= " unit vector that denotes the forward vector of the first 3-Space Sensor device. down:= " unit vector that denotes the down vector of the first 3-Space Sensor device. forward<= " unit vector that denotes the forward vector of the second 3-Space Sensor device. down<= " unit vector that denotes the down vector of the second 3-Space Sensor device. MMM QQ "ssumes the devices0 a is directions are default %RYS& and are positioned or has had its QQ orientation manipulated so that the $ight a is is up QQ 2irst, calculate the right vector for both devices using the forward and down vectors right: L vector>ormali.e%vector@ross%forward:, down:&& right< L vector>ormali.e%vector@ross%forward<, down<&& QQ Second, calculate the angle between the right vectors and a vector perpendicular to them angle L calculate"ngle%right<, right:& a is L vector>ormali.e%vector@ross%right<, right:&& QQ Third, create a *uaternion using the calculated a is and angle that will be used to transform the QQ forward vector of the second device so that it is on the same hori.ontal plane as the forward QQ vector of the first device

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<A'34

*uat L createTuaternion%a is, angle& transformedKforward< L vector>ormali.e%*uaternionUector(ultiplication%*uat, forward<&& QQ 2ourth, calculate the angle between the transformed forward vector and the forward vector of the QQ first device QQ This angle is the yaw yaw L calculate"ngle%transformedKforward<, forward:& QQ 2ifth, calculate a vector perpendicular to the transformed forward vector and the forward vector QQ of the first device a is L vector>ormali.e%vector@ross%transformedKforward<, forward:&& QQ Si th, create a *uaternion using the calculated a is and yaw angle that will be used to transform QQ the forward vector of the second device so that it is on the same vertical plane as the forward QQ vector of the first device and to transform the down vector of the second device to be used in a QQ later calculation *uat L createTuaternion%a is, yaw& transformedKforward< L vector>ormali.e%*uaternionUector(ultiplication%*uat, forward<&& transformedKdown< L vector>ormali.e%*uaternionUector(ultiplication%*uat, down<&& QQ Set the sign of yaw using the a is calculated and the right vector of the first device yaw L math.copysign%yaw, vector6ot%a is, right:&& QQ Seventh, calculate the angle between the transformed forward vector and the forward vector of QQ the first device QQ This angle is the pitch pitch L calculate"ngle%transformedKforward<, forward:& QQ Eighth, calculate a vector perpendicular to the transformed forward vector and the forward vector QQ of the first device a is L vector>ormali.e%vector@ross%transformedKforward<, forward:&& QQ >inth, create a *uaternion using the calculated a is and pitch angle that will be used to transform QQ the transformed down vector so that it is on the same vertical plane as the down vector of the first QQ device *uat L createTuaternion%a is, pitch& transformedKdown< L vector>ormali.e%*uaternionUector(ultiplication%*uat, transformedKdown<&& QQ Set the sign of pitch using the a is calculated and the down vector of the first device pitch L math.copysign%pitch, vector6ot%a is, down:&& QQ Tenth, calculate the angle between the transformed down vector and the down vector of the first QQ device QQ This angle is the roll roll L calculate"ngle%transformedKdown<, down:& a is L vector>ormali.e%vector@ross%transformedKdown<, down:&& QQ Set the sign of roll using the a is calculated and the forward vector of the first device roll L math.copysign%roll, vector6ot%a is, forward:&& return Cpitch, yaw, rollD def create@hec-Sum%charKdata&= MMM @alculates the chec-sum for the given data. "rgs= charKdata= " string of data. MMM chec-sum L : for byte in charKdata= chec-sum PL ord%byte& return chr%chec-sum W 3AB& def offsetTuaternion%serialKport, gravityLC-<.:, :.:, :.:D, initKoffsetL>one&= MMM @alculates the offset of the 3-Space Sensor device on the human body. "rgs= serialKport= " Serial ob!ect that is communicating with a 3-Space Sensor device. gravity= " unit vector that denotes the gravity direction the 3-Space Sensor device should be reading. initKoffset= " unit *uaternion the denotes a rotational offset for a 3-Space Sensor device.

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<B'34

MMM QQ 2irst, find what the device reads as the gravity direction using the read >orth 5ravity command QQ The command returns B floats, the first 3 ma-e the >orth vector and the last 3 ma-e the 5ravity QQ vector northKgravity L command/rite$ead%serialKport, $E"6K>,$T#K5$"UITYK@,((">6, byteKsi.eL34, dataKformatL0Nffffff0& sensorKgravity L northKgravityC3=D QQ Second, read the current filtered orientation as a *uaternion from the device using the read QQ 2iltered Tuaternion command filtKdata L command/rite$ead%serialKport, $E"6K2I8TKT)"TK@,((">6, byteKsi.eL<B, dataKformatL0Nffff0& QQ Third, using the gravity vector given and the 5ravity vector from the device, calculate the angle QQ between them and a vector perpendicular to them angle L calculate"ngle%sensorKgravity, gravity& a is L vector>ormali.e%vector@ross%sensorKgravity, gravity&& QQ 2ourth, create a *uaternion using the calculated a is and angle that will be used to offset the QQ filtered *uaternion of the device so the gravity vectors will line up QQ "lso apply the initial offset if any offset L createTuaternion%a is, -angle& if initKoffset is not >one= offset L *uaternion(ultiplication%offset, initKoffset& tareKdata L *uaternion(ultiplication%filtKdata, offset& QQ 2ifth, set the offset filtered *uaternion as the tare orientation for the device using the set Tare QQ Tuaternion command command/rite$ead%serialKport, SETKT"$EKT)"TK@,((">6, dataKformatL0Nffff0, inputKdataLtareKdata& QQ The calculated offset *uaternion is returned because it needs to be applied to the filtered tared QQ *uaternion received from the device return offset def open1ort%comKport&= try= serialKport L serial.Serial%comKport, timeoutL:.<, writeTimeoutL:.<, baudrateL<<A3::& return serialKport e cept E ception as e = print M2ailed to create a serial port on port=M, comKport raise e def close1ort%serialKport&= try= serialKport.close%& e cept E ception as e = print M2ailed to close the port=M, serialKport.name raise e def command/rite$ead%serialKport, command, byteKsi.eL:, dataKformatL00, inputKdataLCD&= MMM /rites and reads data to and from a serial port given. "rgs= serialKport= " Serial ob!ect that is communicating with a 3-Space Sensor device. command= " char string of one of the 3-Space Sensor device0s commands. byteKsi.e= The number of bytes to read from the serial port. dataKformat= The format for which struct is to pac- or unpac- data. inputKdata= 6ata to be sent to the 3-Space Sensor device. MMM dataKstr L 00 if len%inputKdata& N := dataKstr L struct.pac-%dataKformat, OinputKdata& commandKdata L ST"$TK7YTE P command P dataKstr P create@hec-Sum%command P dataKstr& try= serialKport.write%commandKdata& e cept E ception as e = print MThere was an error writing command to the portM, serialKport.name raise e

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<;'34

if byteKsi.e N := try= dataKstr L serialKport.read%byteKsi.e& e cept E ception as e = print MThere was an error reading from the portM, serialKport.name raise e outputKdata L list%struct.unpac-%dataKformat, dataKstr&& return outputKdata return >one def calculate6eviceUector%serialKport, vec, offset&= MMM @alculates a vector in a 3-Space Sensor device0s reference frame. "rgs= serialKport= " Serial ob!ect that is communicating with a 3-Space Sensor device. vec= " unit vector. offset= " unit *uaternion that denotes the offset of the 3-Space Sensor device. MMM QQ 5et the filtered tared orientation of the device data L command/rite$ead%serialKport, $E"6K2I8TKT"$E6KT)"TK@,((">6, byteKsi.eL<B, dataKformatL0Nffff0& QQ "pply the offset for the device *uat L *uaternion(ultiplication%data, offset& QQ @alculate a vector for the device with its orientation vector L *uaternionUector(ultiplication%*uat, vec& return vector

=.@ -ull Source Co%e *or Calculating t,e Angle o* a #inge +oint
QV'usr'bin'env python3.; QQ QQ Script >ame= hinge.py QQ QQ "pplication >ote= @alculating "ngles 7etween Two YEI 3-Space Sensor 6evices using Two Uectors QQ on a #uman 7ody QQ QQ 6escription= @alculates the hinge angle between two YEI 3-Space Sensor devices in 1ython 3.; QQ QQ /ebsite= www.3-space-sensor.com QQ QQ @opyright= @opyright %@& 3:<3 Yost Engineering, Inc. QQ 1ermission is hereby granted, free of charge, to any person obtaining a copy of this software QQ and associated documentation files %the MSoftwareM&, to deal in the Software without QQ restriction, including without limitation the rights to use, copy, modify, merge, publish, QQ distribute, sublicense, and'or sell copies of the Software, and to permit persons to whom the QQ Software is furnished to do so, sub!ect to the following conditions= QQ The above copyright notice and this permission notice shall be included in all copies or QQ substantial portions of the Software. QQ T#E S,2T/"$E IS 1$,UI6E6 M"S ISM, /IT#,)T /"$$">TY ,2 ">Y +I>6, QQ ER1$ESS ,$ I(18IE6, I>@8)6I>5 7)T >,T 8I(ITE6 T, T#E /"$$">TIES QQ ,2 (E$@#">T"7I8ITY, 2IT>ESS 2,$ " 1"$TI@)8"$ 1)$1,SE ">6 QQ >,>I>2$I>5E(E>T. I> >, EUE>T S#"88 T#E ")T#,$S ,$ @,1Y$I5#T QQ #,86E$S 7E 8I"78E 2,$ ">Y @8"I(, 6"("5ES ,$ ,T#E$ 8I"7I8ITY, QQ /#ET#E$ I> "> "@TI,> ,2 @,>T$"@T, T,$T ,$ ,T#E$/ISE, "$ISI>5 QQ 2$,(, ,)T ,2 ,$ I> @,>>E@TI,> /IT# T#E S,2T/"$E ,$ T#E )SE ,$ QQ ,T#E$ 6E"8I>5S I> T#E S,2T/"$E. QQ import time import threespace QQ @hange the @,( port names as needed QQ 2irst device serialKport: L threespace.open1ort%M@,(<:M&

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<E'34

QQ Second device serialKport< L threespace.open1ort%M@,(<<M& print MTo *uit, hold down XM@trlXM -ey and press XM@XM -eyM print M---------------------------------------------------M print M5et into the starting position.M time.sleep%3& print M1lease hold for <: seconds to compensate for device positioning.M time.sleep%A& QQ @alculate the rotational offset of the compensation for the the first device offset: L threespace.offsetTuaternion%serialKport:& QQ @alculate the rotational offset of the compensation for the the second device offset< L threespace.offsetTuaternion%serialKport<& time.sleep%3& while True= QQ @alculate the forward vector of the first device QQ The initial forward vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading up the arm forward: L threespace.calculate6eviceUector%serialKport:, C:.:, :.:, <.:D, offset:& QQ @alculate the forward vector of the second device QQ The initial forward vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading up the arm forward< L threespace.calculate6eviceUector%serialKport<, C:.:, :.:, <.:D, offset<& QQ @alculate a vector perpendicular to the forward vectors and parallel to the a is of rotation to use QQ for determining the sign of the angle QQ )sing the first device0s orientation will give the best results QQ The initial vector to use depends on the initial forward vector up: L threespace.calculate6eviceUector%serialKport:, C:.:, <.:, :.:D, offset:& QQ @alculate the angle between the two devices angle L threespace.calculate"ngle%forward<, forward:, up:& QQ 1rint as radians and degrees print M#ingeM print M$adians= W:.4fXt6egrees= W:.4fM W %angle, threespace.math.degrees%angle&& print MLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLM QQ @lose the serial ports threespace.close1ort%serialKport:& threespace.close1ort%serialKport<&

=.A -ull Source Co%e *or Calculating t,e Angle o* a 0i"ot +oint
QV'usr'bin'env python3.; QQ QQ Script >ame= pivot.py QQ QQ "pplication >ote= @alculating "ngles 7etween Two YEI 3-Space Sensor 6evices using Two Uectors QQ on a #uman 7ody QQ QQ 6escription= @alculates the pivot angle between two YEI 3-Space Sensor devices in 1ython 3.; QQ QQ /ebsite= www.3-space-sensor.com QQ QQ @opyright= @opyright %@& 3:<3 Yost Engineering, Inc. QQ 1ermission is hereby granted, free of charge, to any person obtaining a copy of this software QQ and associated documentation files %the MSoftwareM&, to deal in the Software without QQ restriction, including without limitation the rights to use, copy, modify, merge, publish, QQ distribute, sublicense, and'or sell copies of the Software, and to permit persons to whom the QQ Software is furnished to do so, sub!ect to the following conditions= QQ The above copyright notice and this permission notice shall be included in all copies or QQ substantial portions of the Software. QQ T#E S,2T/"$E IS 1$,UI6E6 M"S ISM, /IT#,)T /"$$">TY ,2 ">Y +I>6, QQ ER1$ESS ,$ I(18IE6, I>@8)6I>5 7)T >,T 8I(ITE6 T, T#E /"$$">TIES

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

<J'34

QQ QQ QQ QQ QQ QQ QQ

,2 (E$@#">T"7I8ITY, 2IT>ESS 2,$ " 1"$TI@)8"$ 1)$1,SE ">6 >,>I>2$I>5E(E>T. I> >, EUE>T S#"88 T#E ")T#,$S ,$ @,1Y$I5#T #,86E$S 7E 8I"78E 2,$ ">Y @8"I(, 6"("5ES ,$ ,T#E$ 8I"7I8ITY, /#ET#E$ I> "> "@TI,> ,2 @,>T$"@T, T,$T ,$ ,T#E$/ISE, "$ISI>5 2$,(, ,)T ,2 ,$ I> @,>>E@TI,> /IT# T#E S,2T/"$E ,$ T#E )SE ,$ ,T#E$ 6E"8I>5S I> T#E S,2T/"$E.

import time import threespace QQ @hange the @,( port names as needed QQ 2irst device serialKport: L threespace.open1ort%M@,(<:M& QQ Second device serialKport< L threespace.open1ort%M@,(<<M& print MTo *uit, hold down XM@trlXM -ey and press XM@XM -eyM print M---------------------------------------------------M print M5et into the starting position.M time.sleep%3& print M1lease hold for <: seconds to compensate for device positioning.M time.sleep%A& QQ @alculate the rotational offset of the compensation for the the first device offset: L threespace.offsetTuaternion%serialKport:& QQ @alculate the rotational offset of the compensation for the the second device offset< L threespace.offsetTuaternion%serialKport<& time.sleep%3& while True= QQ @alculate the down vector of the first device with its orientation QQ The initial down vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading into the arm down: L threespace.calculate6eviceUector%serialKport:, C:.:, -<.:, :.:D, offset:& QQ @alculate the down vector of the second device with its orientation QQ The initial down vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading into the arm down< L threespace.calculate6eviceUector%serialKport<, C:.:, -<.:, :.:D, offset<& QQ @alculate a vector perpendicular to the down vectors and parallel to the a is of rotation to use for QQ determining the sign of the angle QQ )sing the first device0s orientation will give the best results QQ The initial vector to use depends on the initial down vector forward: L threespace.calculate6eviceUector%serialKport:, C:.:, :.:, <.:D, offset:& QQ @alculate the angle between the two devices angle L threespace.calculate"ngle%down<, down:, forward:& QQ 1rint as radians and degrees print M1ivotM print M$adians= W:.4fXt6egrees= W:.4fM W %angle, threespace.math.degrees%angle&& print MLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLM QQ @lose the serial ports threespace.close1ort%serialKport:& threespace.close1ort%serialKport<&

=.B -ull Source Co%e *or Calculating t,e 0itc,: Yaw: an% 9oll Angles o* a 1ulti-A(is +oint
V'usr'bin'env python3.; QQ QQ Script >ame= multiKa is.py QQ QQ "pplication >ote= @alculating "ngles 7etween Two YEI 3-Space Sensor 6evices using Two Uectors QQ on a #uman 7ody

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

3:'34

QQ QQ 6escription= @alculates the pitch, yaw, and roll angles between two YEI 3-Space Sensor devices in QQ 1ython 3.; QQ QQ /ebsite= www.3-space-sensor.com QQ QQ @opyright= @opyright %@& 3:<3 Yost Engineering, Inc. QQ 1ermission is hereby granted, free of charge, to any person obtaining a copy of this software QQ and associated documentation files %the MSoftwareM&, to deal in the Software without QQ restriction, including without limitation the rights to use, copy, modify, merge, publish, QQ distribute, sublicense, and'or sell copies of the Software, and to permit persons to whom the QQ Software is furnished to do so, sub!ect to the following conditions= QQ The above copyright notice and this permission notice shall be included in all copies or QQ substantial portions of the Software. QQ T#E S,2T/"$E IS 1$,UI6E6 M"S ISM, /IT#,)T /"$$">TY ,2 ">Y +I>6, QQ ER1$ESS ,$ I(18IE6, I>@8)6I>5 7)T >,T 8I(ITE6 T, T#E /"$$">TIES QQ ,2 (E$@#">T"7I8ITY, 2IT>ESS 2,$ " 1"$TI@)8"$ 1)$1,SE ">6 QQ >,>I>2$I>5E(E>T. I> >, EUE>T S#"88 T#E ")T#,$S ,$ @,1Y$I5#T QQ #,86E$S 7E 8I"78E 2,$ ">Y @8"I(, 6"("5ES ,$ ,T#E$ 8I"7I8ITY, QQ /#ET#E$ I> "> "@TI,> ,2 @,>T$"@T, T,$T ,$ ,T#E$/ISE, "$ISI>5 QQ 2$,(, ,)T ,2 ,$ I> @,>>E@TI,> /IT# T#E S,2T/"$E ,$ T#E )SE ,$ QQ ,T#E$ 6E"8I>5S I> T#E S,2T/"$E. QQ import time import threespace QQ @hange the @,( port names as needed QQ 2irst device serialKport: L threespace.open1ort%M@,(<:M& QQ Second device serialKport< L threespace.open1ort%M@,(<<M& print MTo *uit, hold down XM@trlXM -ey and press XM@XM -eyM print M---------------------------------------------------M print M5et into the starting position.M time.sleep%3& print M1lease hold for <: seconds to compensate for device positioning.M time.sleep%A& QQ @reate a *uaternion to be used to orient the first device to the same orientational space as the second QQ device *uat L threespace.createTuaternion%C:.:, <.:, :.:D, -threespace.math.pi ' 3.:& QQ @alculate the rotational offset of the compensation for the the first device offset: L threespace.offsetTuaternion%serialKport:, C:.:, :.:, -<.:D, *uat& QQ @alculate the rotational offset of the compensation for the the second device offset< L threespace.offsetTuaternion%serialKport<& time.sleep%3& while True= QQ @alculate the forward and down vectors of the first device with its orientation QQ The initial forward vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading to the left of the body forward: L threespace.calculate6eviceUector%serialKport:, C:.:, :.:, <.:D, offset:& QQ The initial down vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading into the body down: L threespace.calculate6eviceUector%serialKport:, C:.:, -<.:, :.:D, offset:& QQ @alculate the forward and down vectors of the second device with its orientation QQ The initial forward vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading up the arm forward< L threespace.calculate6eviceUector%serialKport<, C:.:, :.:, <.:D, offset<& QQ The initial down vector to use depends on the orientation and a is direction of the device QQ The resultant vector must be heading into the arm down< L threespace.calculate6eviceUector%serialKport<, C:.:, -<.:, :.:D, offset<&

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

3<'34

QQ @alculate the 1itch Yaw and $oll between the two devices angleKlist L threespace.calculate1itchYaw$oll%forward:, down:, forward<, down<& QQ 1rint as radians and degrees print M1itchM print M$adians= W:.4fXt6egrees= W:.4fM W %angleKlistC:D, threespace.math.degrees%angleKlistC:D&& print MLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLM print MYawM print M$adians= W:.4fXt6egrees= W:.4fM W %angleKlistC<D, threespace.math.degrees%angleKlistC<D&& print MLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLM print M$ollM print M$adians= W:.4fXt6egrees= W:.4fM W %angleKlistC3D, threespace.math.degrees%angleKlistC3D&& print MLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLM QQ @lose the serial ports threespace.close1ort%serialKport:& threespace.close1ort%serialKport<&

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

33'34

C. 9e*erences
<. 6oorenbosch, #arlaar, and Ueeger. The globe system= "n unambiguous description of shoulder positions in daily life movements http=''www.rehab.research.va.gov'!our':3'4:'3'162'doorenbosch.pdf 3. )nderstanding E ercise Y 1lanes, " es and (ovement http=''www.todaysfitnesstrainer.com'understanding-e ercise-planes-a es-movement' 3. The Zoints http=''www.shoc-family.net's-eleton'Z,I>TS.#T(8 4. 2ive 6ifferent Types of Zoints http=''www.livestrong.com'article'<<AEEJ-five-different-types-!oints' A. ,pen, Sesamoid= Types of Zoints http=''science.howstuffwor-s.com'life'human-biology'bone<<.htm

93::;-3:<3 Yost Engineering Inc.

1atents 1ending

33'34

YEI Technology
B3: Second Street 1ortsmouth, ,hio 4ABB3

Toll-2ree= EEE-3JA-J:3J 1hone= ;4:-3AA-J:3J

www.YeiTechnology.com www.3SpaceSensor.com

1atents 1ending 93::;-3:<3 Yost Engineering Inc. 1rinted in )S"