1 TRIGONOMETRY (Most Frequent Formu-

las Used in Robotics)

1
1.1 Some Trigonometric Identities
Notation used in robotics:

cos( i ) = ci cos( i + j) = cij cos( i + j + k) = cijk

sin( i ) = si sin( i + j) = sij sin( i + j + k) = sijk

Identities:

sij = si ci + ci sj s2 + c2 = 1
cij = ci cj si sj
q
1 cos( )
sin( =2) = 2 sin( ) = ptan( )
1+tan( )2

q
1+cos( ) 1
cos( =2) = 2 cos( ) = p
1+tan( )2

d
dx sin(x) = cos(x)
d
dx cos(x) = sin(x)

sin( ') ' ( ')3 =6 '

for ' 0
cos( ') 1 ( ')2 =2 1

2
1.2 About atan() and atan2()
Functions atan(x) (also called arctan(x)) and atan2(y; x) (also called arctan 2(y; x))
are standard mathematical library functions included in most of the high-level
programming languages.

=atan(x) is an inverse of tan( ) = x, which returns a value in the range

2 2

y
=atan2(y; x) is a qudrant sensitive inverse of tan( ) = x which returns
a value in the range :

The relationship between atan(x) and atan2(y; x) is as follows:

8
>
> y>0 1st
>
> atan( xy ) 2 [0; 2 ] if
>
> x>0 quad.
>
>
>
> y y>0 2nd
< atan( jxj ) 2 [2; ] if
x<0 quad.
atan2(y; x) =
>
> jyj y<0 3rd
>
> +atan( jxj ) 2[ 2; ] if
>
> x<0 quad.
>
>
>
> y<0 4th
: atan( jyj
x ) 2 [0; 2] if
x>0 quad.

3
Examples:
3 1
atan2(5; 5) = 4 atan2(5; 5) = 4

3 1
atan2( 5; 5) = 4 atan2( 5; 5) = 4

atan2(0; 5) = atan2(0; 5) = 0

1 1
atan2( 5; 0) = 2 atan2(5; 0) = 2

An important identity:

atan2(y; x) = 2 atan2(x; y) (1)

Proof:
' = atan2(y; x)
sin(') cos( =2 ')
tan(') = xy = cos(') = sin( =2 ')
sin( =2 ') x
tan( 2 ') = cos( =2 ') = y
atan2(x; y) = 2 '= 2 atan2(y; x)

4
More examples:

0 if x > 0
atan2(0; x) =
if x < 0

2 if y > 0
atan2(y; 0) =
2 if y < 0

atan2(y; x) if y > 0
atan2(y; x) =
+atan2(y; x) if y < 0

atan2( y; x) = atan2(y; x)

atan2(0; 0) UNDEFINED!

5
1.3 Equation cos(') a - sin(') b = p

In inverse kinematics there is very often a need to solve the transcedental

equation:
cos(') a sin(') b = p (2)
given the scalar parameters a; b and p; and to evaluate afterwards the ex-
pression:
q = sin(') a + cos(') b (3)

The problem can also be stated in a reverse form, solve:

sin(') a + cos(') b = q

then evaluate:
p = cos(') a sin(') b

6
1.3.1 Traditional solution
A traditional way to solve these equations is by substitutions:
a=p r sin( ); and b = r cos( ); where = atan2(a; b) and
r = a2 + b2 , so that (2) becomes:

cos(') r sin( ) sin(') r cos( ) = r sin ( ') = p

or:
p
sin ( ') = (4)
r
Hence: q r
p 2
2
cos ( ') = 1 sin ( ') = 1 (5)
r
By combining (4) and (5), follows:
p
tan( ') = p
r2 p2
or
p
' = atan2( r2 p2 ; p) (6)
p
= atan2(a; b) atan2( a2 + b2 p2 ; p)

Using the identity (1), the equation (6) can also be written as:
p
' = atan2(p; a2 + b2 p2 ) atan2(b; a): (7)

In order to evaluate (3) we use again the substitutions above:

q = sin(') a + cos(') b
= sin(') r sin( ) + cos(') r cos( )
= r cos ( ')
p (8)
=r 1 sin( ')2
p
= pr2 p2
= a2 + b2 p2

p
Note that that the square root a2 + b2 p2 needs to be evaluated only
once and than be used in (7) and (8).

7
1.3.2 An Alternative Approach

(p,q)
y

q (a,b)
ϕ b
x

p
a

A more compact and structured approach is to consider the two equations

(2)-(3) as two indivisible companion equations. Written in matrix form:

cos(') sin(') a p
= (9)
sin(') cos(') b q
they clearly suggest a 2D rotation of the vector (a; b) by the angle '; giving
the new vector (p; q): Since the rotation does not change the size of vectors, we
have:

a2 + b2 = p2 + q 2 (10)
It is clear from the …gure above that:

' = atan2(q; p) atan2(b; a) (11)

The last two equations we can use now to solve both problems stated above:

p
solve: cos(') a sin(') b = p q= a2 + b2 p2
evaluate: q = sin(') a + cos(') b ' =atan2(q; p) atan2(b; a)

and

8
 p
solve: sin(') a + cos(') b = q p= a2 + b2 q 2
evaluate: p = cos(') a sin(') b ' =atan2(q; p) atan2(b; a)