DERIVATION OF INVERSE KINEMATIC EQUATIONS

FOR THE RV-M1 ROBOT MANIPULATOR

Inverse kinematics deals with the calculation of the joint angles given the position
and orientation of the end-effector. To determine the joint angles, the following
computations are followed.
The calculation of each joint angle starts by realizing that:

5
0

AAAAA

(1)

and knowing that

nx
n
5
y
T 0 = nz

sx
sy
sz
0

px
p y
pz

ax
ay
az
0

px
where p0 = p y is the desired position of the end-effector referenced to the
p z
nx s x a x
world coordinate frame and R = n y s y a y is the desired orientation of the end nz s z a z

effector, also referenced to the world coordinate frame. The right-hand side of equation
(1) contains only the unknown joint angles. To determine each unknown, equation (1)
will be used repeatedly to isolate the unknown variable to be determined to the left side
of the equation.
Proceeding, we premultiply both sides of equation (1) by the

(A )

1 1
0

5
0

nx
s
1 1
1 1
Note that the A0 = A0 = x
ax

( ) ( )

AAAA
ny
sy
ay
0

nz
sz
az
0

p0 ' n
p0 ' s
.
p0 ' a

(A )

(2)

1 1
0

, thus we get,

Upon premultiplication, we equate the corresponding entries (3, 4) of each side.

Doing this, we get:
sin 1 * p x = cos 1 * p y
py
px

1 = arctan

Also, we equate the corresponding entries (3, 3).

cos 5 = s1a x c1a y

5 = arccos(s1a x c1a y )
We now set entries (1, 3) equal and see that:
c1a x + s1a y = s234

234 = arcsin (c1a x + s1a y )

To determine the elbow angle, we set entries (1, 4) and (2, 4) equal on both sides
of equation (1).
a3c23 + a2 c2 = k1

(1,4)

a3 s23 + a2 s2 = k 2

(2,4)

where k1 = c1 p x + s1 p y d 5 s234 and k 2 = p z d1 + d 5 c234 . Squaring both sides of

the equations above and summing the resulting equations, we arrive at an equation
containing only the unknown joint angle, the elbow angle.

(a3 )2 + (a2 )2 + 2a2 a3c3 = (k 2 )2 + (k1 )2

(k )2 + (k1 )2 (a3 )2 (a2 )2

= arccos 2
3

2a2 a3

To solve for the shoulder angle, we expand entries (1, 4) and (2, 4).
a3 (c2 c3 s2 s3 ) + a2 c2 = k1

a3 (s2 c3 + c2 s3 ) + a2 s2 = k 2

We then group and rearrange terms and put the unknown joint angle at the
lefthand-side of the equation. We come up with:
c2 =
s2 =

k1 (a3c3 + a2 ) + k 2 a3 s3
(a3c3 + a2 )2

k 2 (a3c3 + a2 ) k1a3 s3
(a3c3 + a2 )2

2 = arctan 2(s2 , c2 )

To determine the wrist pitch angle, the following equation is used.

4 = 234 2 3