ROBOTICS 1

Direct and Inverse Kinematics of Serial

Manipulators (Nyrio One 6-axis Robotic Arm)
ALMEIDA, João∗ ROSA, Daniel† VIEGAS, Guilherme‡
[email protected] [email protected] [email protected]
IST id: 90119 IST id: 90041 IST id: 90090

May 29, 2021

Abstract—This paper presents the development of a 6-axis corresponding joint variables to move in order to achieve the
rigid body kinematics. A rigid body in three dimensions has six desired position and/or orientation.
degrees of freedom composed by three translatory DOF and three The two problems are solved using distinctive mathematical tools
rotational DOF. The multiples revolute joints of the rigid body which, although explained in more detail later on, it can be said that
form a serial chain, which allows successive rotations about the for solving the direct kinematics problem the need to specify positions
x, y and z axes accomplishing a specific 3D position, this method and orientations of many referentials is very important. Thereafter a
is the so called direct kinematics. In addition, it is also designed composition of transformations between those specified referentials
the reverse procedure, inverse kinematics, that places the end is computed. On the other hand, for the inverse kinematics both
of the kinematic chain (e.g. gripper) in an exact place with an geometric and algebraic methods are used to solve specific problems
specific orientation. that, altogether, resolve the more complex problem of the inverse
kinematics.
Keywords— Rigid Body, 6DOF, Serial Chain, Niryo One, Direct
Kinematics, Inverse Kinematics
III. BACKGROUND /C ONCEPTS∗
the shield of the arm, the rigid body can be described as
II. I NTRODUCTION‡ B EHIND
a series of mathematical calculations. Each joint varies from the
previous by one transformation that can be expressed as a rotation
can be studied from different perspectives. Perhaps one of
R OBOTS
the most important parts in the study of a robot is understanding
its geometrically possible motion without considering the forces
plus a translation. In 3D coordinates (X-Y-Z), the T-matrix is a
linear transformation mapping R4 to R4 , so a basic knowledge
about this mapping is required (see more in [1]). Another method to
involved. This is called the robot kinematics, often referred as the describe the multiple transformations between joints is using Denavit-
geometry of motion, and aims to provide a description of the spatial Hartenberg parameters, although it is not used in this application, it
position of systems (and also their velocity and acceleration). might help understanding the construction of the skeleton. In addition,
Within this subject, two forms of kinematics can be distinguished: throughout the report it is mentioned the orientation of the rigid body,
• The direct kinematics that enables to determine the robots’ which is computed with the Euler angles and describes the orientation
configuration in the work space, given a set of values/angles of the current frame according the world frame. This application uses
for that the robots’ joints can move; the Z-Y-Z convention, however other conventions can be studied in
• The inverse kinematics that solves the inverse problem, with [2]. Moving to the inverse kinematics process, there are mainly two
the goal of, given a work space configuration, determine the threads to bear in mind. Firstly the properties of the rotation matrix
2 ROBOTICS

such as the determinant equal to 1 and the inverse matrix means

a rotation by the symmetric angle (transpose). Secondly, a general
knowledge in trigonometry involving the law of cosines and planar
geometric (e.g. 2D double pendulum equations) is necessary.
Throughout the report, the scale of the images showed are in
centimetres, even though the calculus of the position were made in
millimetres.

IV. S TATE OF THE ART∗

paper explains, step by step, the kinematics of the robot
T HIS
alongside examples. Moreover this paper does not go into much
detail, however it is a plus for whom wants to be familiar with 6DOF
Robotic Arm movements. For a more detailed approach see [3].

V. E ASE OF USE∗
implementation consists on two contrary processes. Firstly
T HE
it is computed the position of the end effector and the Euler
angles with the call function: direct kinematics(A:1x6 matrix), from
the vector of angles of each joint. This step is super fast, since it
is a ”simple” series of matrix operations. On the other hand, the
inverse kinematics(O:1x6 matrix) function behaves as the inverse,
since it transforms X-Y-Z end effector’s coordinates and Z-Y-Z Euler
angles into multiple solutions for the angles of the joints. This
approach is slower once it compares and validates each possible
solution after every step. The metrics are millimetres (mm) for the Fig. 1. Coordinate Frames for the 6 joints and World Frame
position and radians (rad) for the joint angles.

VI. D EVELOPMENT∗†‡ Then, for the pair between frame 1 and frame 2 (so link 1 that
connects joint 1 and joint 2), because there’s only a rotation through
A. Direct Kinematics‡ the y axis, it originates
1) Choice of the Coordinate System‡ : As with many robotic    
manipulators, the robot described in this report is considered a serial 0 cos(θ2 ) 0 sin(θ2 )
1 1
 0  2 Ry = 
manipulator. These types of manipulators are constituted by a set of 2P = 0 1 0 , (3)
rigid bodies, called links, connected by joints that enable different 80 − sin(θ2 ) 0 cos(θ2 )
kinds of movements and that have different types of restrictions, and
contrasting to parallel manipulators, there are no closed loops. leading to the transformation matrix
In the direct kinematics problem, where the goal is, given a set of 
cos(θ2 ) 0 sin(θ2 ) 0

values for the joint variables, compute the robot configuration (posi- 0 1 0 0
1
tion + orientation), the solution for the end effector position is found T = . (4)

2 − sin(θ ) 0 cos(θ )
2 2 80
through a series of multiplications between transformation matrices. 0 0 0 1
For this, a crucial initial step is to define a set of referentials capable
of defining the entire motion of the manipulator. Although possible to For the next pair, between frame 2 and frame 3 (link 2 connecting
define those referentials according to multiple conventions, some of joints 2 and 3) there’s the same rotation around the y axis as
them enable not only an easier understanding of the robot movement previously described, originating
but are also easier to compute the needed chain of transformations.    
For this specific project, it was defined a direct coordinate system for 0 cos(θ3 ) 0 sin(θ3 )
2  0  23 Ry = 
each joint, with the particularity that all of the systems had the same 3P = 0 1 0 , (5)
orientation, as seen in Figure 1, where (for the reader’s view) every x 210 − sin(θ3 ) 0 cos(θ3 )
axis was pointing left, every z axis was pointing upwards and every
y axis pointing towards the ”viewer”, orthogonal to the x and y axes, leading to the transformation matrix
as in the referential in Figure 3.
cos(θ3 ) 0 sin(θ3 ) 0
 
With this the only need was to take care of the rotation that each
joint made and the change in position for each and every frame. 2 0 1 0 0 
3T =  . (6)

Knowing the manipulator specifications it could then be built the − sin(θ3 ) 0 cos(θ3 ) 210
matrix transformations for each consecutive pair of frames, noting 0 0 0 1
the choice of using millimetres for translations. For the pair between
The next pair, where frame 4 is positioned in fourth joint, a rotation
the world frame and frame 1 (the so called link 0), because it only
around x axis is formed, leading to
rotates through the z axis, it originates
   
41.5 1 0 0
   
0 cos(θ1 ) − sin(θ1 ) 0 3
0  0  01 Rz =  sin(θ1 ) 4P =
 0  34 Rx = 0 cos(θ4 ) − sin(θ4 ) , (7)
1P = cos(θ1 ) 0 , (1)
103 0 0 1 30 0 sin(θ4 ) cos(θ4 )

leading to the transformation matrix leading to the transformation matrix

cos(θ1 ) − sin(θ1 ) 0 0 1 0 0 41.5
   
0  sin(θ1 ) cos(θ1 ) 0 0  3 0 cos(θ4 ) − sin(θ4 ) 0 
1T =  . (2) 4T =  . (8)
0 0 1 103 0 sin(θ4 ) cos(θ4 ) 30 
0 0 0 1 0 0 0 1
 3

Then another rotation around y axis for the transformation between For the cases where β was ±π/2 there was the need in Matlab
frame 4 and 5 (link 4) and the rotation that happens in fifth joint, to define the values for α and γ by
leading to 
    α = 0

180 cos(θ5 ) 0 sin(θ5 ) γ = arctan2(r21 , r11 ) , if β = π2 (17)
4  0  45 Ry = 
5P = 0 1 0 , (9) γ = − arctan2(r , r ) , if β = − π .

21 11 2
0 − sin(θ5 ) 0 cos(θ5 )
With this, a final output is given in vector form with the first
leading to the transformation matrix three elements being the final end effector position, with respect to

cos(θ5 ) 0 sin(θ5 ) 180
 the world frame, given in millimetre, and the last three elements
0 1 0 0  being the end effector orientation, according to the Z-Y-Z Euler angles
4
5T =  . (10) convention, in radians.

− sin(θ5 ) 0 cos(θ5 ) 0 
0 0 0 1

For the last transformation, there is the connection between frame B. Inverse Kinematics∗†
5 and 6 and the rotation around the x axis in joint 6 that leads to 1) Translatory and Rotational DOF∗ : The inverse kinematics
    can be divided into two groups of links: translation and orientation of
23.7 1 0 0 the end effector. Considering rotations about the x, y, and z axes, it is
5 5
 0  6 RX = 0 cos(θ6 ) − sin(θ6 ) ,
6P = (11) evident that there are three rotational degrees of freedom. The other
−5.5 0 sin(θ6 ) cos(θ6 ) 3 DOF correspond to the translation of the rigid body. The handicap
of this division is that the group of orientation also has influence on
leading to the transformation matrix the translation of the end effector.
Due to the fact that every rotation affects the orientation of the rigid
1 0 0 23.7
 
body and since the range is proportional to the length of the joints
5 0 cos(θ6 ) − sin(θ6 ) 0 
6T =  . (12) therefore to the weight not only of the links but also the motors, it is
0 sin(θ6 ) cos(θ6 ) −5.5 defined the translation group of links closer to the base, composed by
0 0 0 1 the three first joints. Hence, the orientation group involves the joints
4, 5 and 6, and make the bridge between the third orientation and
Now that all the frames and transformations are defined, the direct
the final joint (end effector).
kinematics problem may be divided into two parts, one for finding the
end effector final position using a serial composition of the previously Consequently, it is defined a point as closest to the end effector as
defined matrices and the other for finding the end effector orientation, possible without being affected by the last 3 rotations. Accordingly to
according to the Z-Y-Z Euler Angles convention, making use of some the skeleton of the rigid body, the fourth joint rotates according x axis
geometric properties. corresponding to the axis where the fourth link lies, so any rotation
2) Solving the Position : Starting by the first problem of will only be evident after the fifth joint (because the rotation axis
‡
is different). Moreover, according to the rigid body in analysis, the
finding the end effector final’s position in space, according to the
second and third joints rotates according the y axis, thus, forming
defined world frame, it can be said that this position, in homoge-
a plan (xy, z) ∈ R2 which rotates through the first joint (z axis),
neous coordinates, is the last column of the matrix formed by the
making a 3D translation from the first joint to the fifth.
multiplication series of the previously defined matrices, as in
Finally the 5.5mm offset on the sixth joint in the z direction, imply
0 0 1 2 3 4 5 that the last three joints do not intersect at one point, thus creating
6 T =1 T ∗2 T ∗3 T ∗4 T ∗5 T ∗6 T. (13)
infinity admissible solutions to define the end effector. Therefore,
3) Solving the Orientation‡ : For the second part of the direct in order to solve the inverse kinematics easily it is not taken into
kinematics problem, which is finding the end effector orientation, account the 5.5mm length, so the algorithm can generally have up to
according to a Z-Y-Z convention using Euler angles, it’s started by eight different solutions. Furthermore, a six-degree-of-freedom robot
making a correlation between the previously defined rotation matrix arm with this particularity is referred to as wrist-partitioned or as
inside (13) between frame 6 and the world frame, 06 R, and the generic having inline wrists. At the end, it is considered the 5.5mm length
rotation matrix defined by the three Euler angles, α, β and γ and the problem is iterative solved for multiples values of the angle of
rotation at the sixth joint. To conclude, the real solutions are obtained
    from the two combinations of the trunk (joint 1) rotation, from the
cα cβ cγ − sα sγ −cα cβ sγ − sα cγ cα sβ r11 r12 r13 two combinations of the planar chain and finally for both translatory
sα cβ cγ + cα sγ −sα cβ sγ + cα cγ sα sβ  = r12 r22 r23  . combinations.
−sβ cγ sβ sγ cβ r31 r32 r33 . 2) Position of the Fifth Joint∗ : The input of the inverse
(14) kinematics are the X-Y-Z position and Z-Y-Z Euler angles of the end
Therefore, from inspection and adequate choice of the equations effector. Additionally, in order to compute a middle joint position,
to solve it is obtained the transformation from the world frame to the end effector can
 p be split in the selected joint. In other words, the view of the fifth
2 2
β = arctan2( r13 + r23 , r33 )
 joint in the reference frame is equivalent to compute the end effector
−r31
γ = arctan2( rs32
β
, s β
) , if s β 6= 0 (15) transformation and then reverse one step. This is illustrated in (18).
α = arctan2( r23 , r13 ) , if s 6= 0.

sβ sβ β 0 0 0 5
5P = 6P − 5R 6P (18)
First, the β angle is computed because both the other angles The same reasoning is applied to 05 R, rotating from frame 5 to 6
depend on its value. If sβ 6= 0 then solving the other two angles and then from frame 6 to 0, obtaining
is easy by applying (15). If sβ = 0 then cβ = ±1 which leads to
α ± γ = atan2(r21 , r11 ) (other rotation matrix elements could be 0 0 6
chosen) and so it is needed to disambiguate their values, solved by 5R = 6 R 5 R. (19)
stating ( Substituting (19) in (18), it is obtained (20).
γ=0
(16) 0 0
α = cβ · atan2(r21 , r11 ). 5P = 6P − 06 R 65 R 56 P (20)
4 ROBOTICS

The rotation matrix 06 R is computed from the input Euler angles axis between the two solutions, β = arctan2(z/xy). Therefore, the
(see (21)), and the notation 06 P is simply queuing the 3D end expressions are
effector’s position. (
β = θ2 + ψ , (θ3 < 0)
. (25)
0 β = θ2 − ψ , (θ3 ≥ 0)
6R = Rz (α)Ry (β)Rz (γ)
With this being said, both θ2 and θ3 are easily computed using
 
ca ∗ cb ∗ cg − sa ∗ sg −ca ∗ cb ∗ sg − sa ∗ cg ca ∗ sb
= sa ∗ cb ∗ cg + ca ∗ sg −sa ∗ cb ∗ sg + ca ∗ cg sa ∗ sb the cosine formula.
−sb ∗ cg sg ∗ sb cb c2 = a2 + b2 − 2ab cos(γ) , γ=6 ab. (26)
w/ ca = cos(α), cb = cos(β), cg = cos(γ),
Applying (26) in the inside upper triangle in 2, it is clear that
sa = sin(α), sb = sin(β), sg = sin(γ). 2 l2 +xyz 2 −l2
l34 = l22 + xyz 2 − 2 · l2 · xyz · cos(ψ) ↔ ψ = arccos 2 2·l2 ·xyz 34 ,
(21) p
with xyz = xy 2 + z 2 . Analysing the same triangle but another
At this point, the only variable missing is 65 R, which is written by angle, the problem is written by:
l2 +l2 −xyz 2 xy 2 +z 2 −l2 2
34 −l2
  π − θ3 = arccos 34 2·l22 ·l34 ↔ θ3 = arccos 2·l2 ·l34
.
1 0 0
6
cos(θ) − sin(θ) . To conclude the reasoning, using (25) it is computed θ2 (θ3 , Ψ) for
5 Rx (θ) = 0 (22)

0 sin(θ) cos(θ) the 2 solutions.
Lastly, it is mapped the angles from the 2D geometry for the 3D.
At this point there are only two missing variables: 05 P and 65 R(θ). Since it is used the direct configuration for the axes, the direction
Considering the home position conditions to facilitate the calculus of the rotation between 2D and 3D is contrary, therefore the angles
and 56 P = L5 = (L5x , 0, 0), in other other words the link distance are symmetric. The final step is to subtract the angles of the home
between the fifth and sixth joint in the home position just differs in position (see figure 3) because in 2D the home position is lied in the
the x coordinate. Consequently, for any value of θ, (20) is given by horizontal plane. With this being said, the mapping is written by
(
θ23D = −θ22D − (− π2 )
   
X ca ∗ cb ∗ cg − sa ∗ sg
0 (27)
5P =
Y  − L5x · sa ∗ cb ∗ cg + ca ∗ sg  (23) θ33D = −θ32D − (α = arctan L3xL+L3z
4x
).
Z −sb ∗ cg
end ef f ector

3) Planar Chain∗ : By definition, a planar chain has all the links

constrained to move in parallel to the same plane. This means that
this type of chains only includes prismatic joints where their axes
must be parallel or lie in the plain of the chain, and revolute joints
that are perpendicular to it.
For each wrist position there are two symmetric planar geometries
θ1 = {−γ, γ}, although there is only one option of slope (forward
or backwards) to reach the 05 P = (xy, z). On the other hand, for
each slope, there are two symmetric possibilities as it is shown in
the figure 2.

Fig. 3. Planar Chain in 3D view

4) Segmented Transformation† : At this point the orientation of

the rigid body might be different from the home position’s orientation.
According to what is done in the direct kinematics, 03 T is computed,
and with the same reasoning as (18), transforming from frame 6 to
0 to 3.
3 0 3
6 T = 6 T 0 T. (28)
Now, it is computed the transformation matrix 36 T analytically,
with (12) (10) (8), in (29) and (30), where it is used the same notation
as in (21) with θ4 , θ5 and θ6 represented only with the respective
Fig. 2. Planar Chain in 2D view number.
3 5 4 3
6 T = 6 T 5 T 4 T. (29)
According to the figure 2, the planar geometry is written as
(
xy = l2 cos(θ2 ) + l34 cos(θ2 + θ3 ) 
c5 s5 s6 s5 c6 c5 L6x + L4x + L5x

(24)
z = l2 sin(θ2 ) + l34 sin(θ2 + θ3 ). 3  s4 s5 c4 c6 − s4 c5 s6 −c4 s6 − s4 c5 c6 s4 s5 L6x
T =

6 −c s
4 5 s4 c6 + c4 c5 s6 −s4 s6 + c4 c5 c6 −c s L
4 5 6x

Secondly it is computed the angle between the horizontal plan 0 0 0 1
(z = 0) and the 05 P . This angle, titled β, represents the symmetric (30)
 5

From 36 T11 = cos(θ5 ) it is computed the angle of the fifth joint, it [rad]. From this, it can be seen that the joint 1 leads the manipulator
is important to notice that the arccos(.) function has 2 solutions in to be in the fourth quadrant (according to the defined referentials,
[−π, π]. Knowing that, from each solution it is computed where Y is pointing towards the reader) due to the -π/4 rotation,
the joint 2 rotates π/3 from its initial position and joint 3 -π/6, so
θ4 = arcsin(36 T21 / sin(θ5 )), (31) considering a planar 2D geometry the arms look above a diagonal
traced from joint 2 to joint 5. Then the last three joints lead to the
that also gives 2 solutions for each θ5 , because arcsin(.) gives 2
manipulator’s orientation, seen in the augmented circle of Figure 4.
solutions in [−π, π]. However, for each θ5 there is only 1 value for
Being these Euler angles in a Z-Y-Z convention it means, from the
θ4 , so those values need to be verified with
starting frame defined in Figure 5(a), that there was a rotation of
3 ≈ -108º along the Z axis, then a rotation of ≈ 156.7º along the Y
6 T31 = −c4 s5 , (32)
axis and finally a rotation of ≈ 50.8º along the Z axis as showed in
where only 1 solution for each θ5 pass. Figures 5 (b), (c) and (d), respective.
With the same reasoning, θ6 is computed by
θ6 = arcsin(63 T12 /sin(θ5 )), (33)
giving 2 solutions for each value of θ5 that are verified with
3
6 T13 = −s5 c6 , (34)
discarding 2 solutions. Finally, for each θ5 there is only one value of
θ4 and θ6 , so there are only two combinations of the angles of the
fourth, fifth and sixth joints that are feasible.
Now, if cos(θ5 ) = 1, it means that θ5 is 0, therefore the fourth
joint’s x axis is coincident with the sixth joint’s x axis, which leads
to a singularity, that is, there are infinite combinations of θ4 and θ6
that give the same end effector orientation. In this specif case, it is
considered θ6 = 0, so it is possible to simplify 36 T22 and 36 T32 to
what is represented in (35).
(
3 Fig. 4. Experimental Result - Direct kinematics
6 T22 = c4
3 (35)
6 T32 = s4

Finally, θ4 can be computed with θ4 = arctan2(36 T32 ,36 T22 ).

From now on, the 5.5mm offset in the z direction will be
considered. The way to solve the inverse kinematics at this point
is to iterate for different values of θ6 in the interval [−π, π[, in this
π
case the program starts with the value −π and increments 36 in each
π
iteration until the value is π − 36 , and then proceed in the same way
as without the offset. Firstly, it is computed 05 P with (20) and then
the first 3 angles, θ1 , θ2 and θ3 , as shown before, after that, 03 T is
computed. Now, with (28), one gets 36 T and then can compute the (a) (b)
last 3 joints angles showed before.
Lastly, to inspect if the computed angles of each of the eight
solutions are correct, the difference between the two angles of the
sixth joint, the one given by the iteration and the other computed
π
with (33), is computed to see if it is within a threshold (e.g. 36 ) in
order to identify if it is a good solution or not. It is good to notice
that with this method, it will have multiple solutions with a tiny error
and, sometimes, with no error.

VII. E XPERIMENTAL R ESULTS∗†‡ (c) (d)

A. Solutions for the Inline Wrist Experiment Fig. 5. Euler Angles rotation [-1.8925, 2.7352, 0.8861], with Z-Y-Z
(w/o 5.5mm)‡∗ Being the end effector starting position as in Figure 3, one can be
1) Direct Kinematic‡ : To validate the work done, an important certain that by moving these angles it gets to the orientation showed
step is to validate the direct kinematics problem due to its importance in the focused circle in Figure 4, because the end effector ”plane”
for the correct testing of the inverse kinematics problem. Therefore, was perpendicular to the X axis and pointing contrary to the Z axis
although tested several times and with several different sets of values direction, and in Figure 5(d) it is seen that the end effector would be
for each of the six manipulator joints, in this section a simple pointing upwards (contrary to the Z axis) and perpendicular to the X
simulation is described, with different than zero values for each joint axis, which is precisely the orientation seen in Figure 4.
variable and so that it does not coincide with any singularity. To note 2) Inverse Kinematic∗ : Regarding the inverse kinematic solu-
that it’s possible, using a flag in the direct and inverse kinematics tions, for the purpose of exemplification, it is tested the software
MATLAB functions, to solve both direct and inverse problems with with the position and orientation of the previous section. As it was
the manipulator’s joint constraints, altough being set as false by being said throughout the paper, an inline wrist robot arm may have
default. With this being said, the direct kinematics’ input is defined to up to eight solutions. Analysing in a top-down reasoning, the figure
be the set of angles [− π4 , π3 , − π6 , π2 , π3 , π4 ], where the simulated result 6 can be studied in blocks. First things first, it is evident that each
of position plus orientation is seen in Figure 4, leading the position of line contains a different direction for the elbow. This happens due
the end effector to be, approximately, (295.64, -292.73, 19.97) [mm] to the combination of three different factors that will be discussed
and the orientation to be, approximately, (-1.8925, 2.7352, 0.8861) next. The first bifurcation is in the shoulder (yellow): the sub figures
6 ROBOTICS

(a) (b) (c) (d)

(e) (f) (g) (h)

Fig. 6. Inline Wrist

{a, b, e, f} represents the arm with the θ1 in the fourth quadrant, TABLE II
on the other hand the sub figures {c, d, g, h} show the robot with S OLUTIONS FOR THE INVERSE KINEMATICS FOR THE HOME POSITION
the opposite slope because the θ1 is rotated by plus π. The second
divergence is related with the planar geometry and it is evident in θ1 θ2 θ3 θ4 θ5 θ6
-3.1416 -1.4905 0.0002 3.1416 1.6513 0
each column pair (sub figure {a-e, b-f, c-g, d-h}). This occurs once
0 1.4905 -2.8726 0 1.3821 0
the elbow (light blue) has to adapt his angle (θ3 ) to the angle of the 0 0.0002 0.0002 0 0 0
arm (light green) (θ2 ), in order to reach the same position for the -3.1416 -0.0002 -2.8726 3.1416 0.2688 0
fifth joint. On the other hand, for each line pair (sub figure {a-b, c-d, 3.1416 -1.5429 0.0555 0 -1.6542 -3.1416
e-f, g-h}), is evident the change in the orientation of the links from 3.1416 0.0054 -2.9279 0 -0.2191 3.1416
the third joint to the end effector (hand). This is notorious in the -0.0022 1.5429 -2.9279 3.1394 -1.3850 3.1412
head of the forearm (heavy blue) or in the body of the wrist (pink). -0.0022 -0.0055 0.0555 3.0983 0.0500 -3.0984
Lastly a quick note regarding the real situation of this arm, due to
the physical limitations on each joint just the combination of angles
on the sub figures {a,b} would be possible to achieve.

TABLE I
S OLUTIONS FOR THE INVERSE KINEMATICS IN FIGURE 6 IN RAD .

Subfigure θ1 θ2 θ3 θ4 θ5 θ6
(a) -07854 1.0472 -0.5236 1.5725 1.0472 0.7854
(b) -0.7854 1.0472 -0.5236 -1.5691 -1.0472 -2.3562
(c) 2.3562 -1.0472 -2.3488 -1.7233 1.0679 1.0938
(d) 2.3562 -1.0472 -2.3488 1.4183 -1.0679 -20477
(e) -0.7854 1.9904 -2.3488 1.1515 1.2474 1.7368
(f) -0.7854 1.9904 -2.3488 -1.9901 -1.2474 -1.4048
(g) 2.3562 -1.9904 -0.5236 -2.056 1.3657 1.9875
(h) 2.3562 -1.9904 -0.5236 1.0856 -1.3657 -1.154

B. Solutions for the not Inline Wrist Experiment

(w/ 5.5mm)∗†‡
1) Inaccurate Solutions† : With the objective of exemplifica- Fig. 7. Solution with less error in relation to the home position
tion, it is tested the inverse kinematics solutions, but with the hand
not aligned with the wrist. For this example it is used the home
position, where the given solutions are represented in the table II, In table III, there are roughly all the solutions to the fully stretched
with 8 different solutions. In figure 7, it is represented the best out robot. For this position, there are only 3 solutions, which are
solution, that is, the solution with the less error in relation to the represented graphically in the figure 8.
home position, with θ1 = −3.1416, θ2 = −1.4905, θ3 = 0.002, 2) Accurate Solutions (Bonus Point)‡ : Now, using MATLAB’s
θ4 = 3.1416, θ5 = 1.6513 and θ6 = 0. Regarding this example, the function fmincon, a minimisation problem is solved, using as objec-
best solution, theoretically, would be all the joints with value 0, but tive variable the Θ6 angle, so that the best solutions are found, as
there are slightly differences due to approximated values and that the showed in Table IV. To get these results, the regions for the fmincon
third combination of angles in the table II just zeros. function to search for the best solution need to be coded by hand to
 7

TABLE III
S OLUTIONS FOR THE INVERSE KINEMATICS FOR THE STRETCHED OUT
ROBOT

Subfigure θ1 θ2 θ3 θ4 θ5 θ6
(a) 0 0 -1.4362 0 0 0
(b) -0.6151 -00426 -1.3096 2.5189 0.1332 -1.9122
(c) 0.6151 -0.0426 -1.3096 -2.5189 0.1332 1.9122

(a) Fig. 9. θ6 error throughout the trials

the clustering threshold it is only displayed 16 solutions, overlapped

at Figure 10.

TABLE V
S OLUTIONS FOR THE INVERSE KINEMATICS WHEN A SINGULARITY
HAPPENS

θ1 θ2 θ3 θ4 θ5 θ6
-3.0543 0.4627 -2.3046 -3.1416 -0.2711 -0.0873
(b) (c) 1.6581 0.4627 -2.3046 3.1416 -0.2711 1.4836
-1.6581 0.4627 -2.3046 -3.1416 -0.2711 -1.4836
Fig. 8. Inaccurate Solutions for the rigid body maximum stretched out
-0.0873 0.4627 -2.3046 -3.1416 -0.2711 -3.0543
0.8727 -0.4347 -0.5677 -3.1416 0.5684 2.2690
2.4435 -0.4347 -0.5677 -3.1416 0.5684 0.6981
the function, so to get those lower and upper search regions, the plot -2.4435 -0.4347 -0.5677 3.1416 0.5684 -0.6981
showed in Figure 9 (generated in the inverse kinematics.m file) is -0.6981 -0.4347 -0.5677 -3.1416 0.5684 -2.4434
used, where it is shown the difference between the Θ6 that is being -1.5708 -0.4627 -0.5677 0 -0.5404 1.5708
iterated, or said otherwise, the proposed Θ6 , and the generated θ6 0 -0.4627 -0.5677 0 -0.5404 0
from the inverse kinematics. This method generates the so called error 1.5708 -0.4627 -0.5677 0 -0.5404 -1.5708
that depends on the iterated Θ6 , where only the values lower than the 3.1416 -0.4627 -0.5677 0 -0.5404 -3.1416
π -1.5708 0.4347 -2.3046 0 0.2991 1.5708
36
threshold (and nearer than 2mm from the direct kinematics end
effector position) are seen as acceptable values. With this, one can 0 0.4347 2.3046 0 0.2991 0
define certain bounded regions to apply in the fmincon region. As 1.5708 0.4347 -2.3046 0 0.2991 -1.5708
seen in Table IV, the accurate solutions are similar to the inaccurate 3.1416 0.4347 -2.3046 0 0.2991 -3.1416
ones seen in Table II, although, with some minor differences, which
lead to a decrease in the order of magnitude of the error of around
10−3 .

TABLE IV
ACCURATE S OLUTIONS FOR THE H OME P OSITION

θ1 θ2 θ3 θ4 θ5 θ6
-3.1416 -1.3858 -0.2606 3.1416 1.6513 0
0 1.4905 -2.8725 0 1.382 0
0.00003 0.002 0.002 0 0 0
-3.1416 -0.0002 -2.8726 3.1416 0.2688 0
3.1416 -0.0054 -2.928 0 -0.2191 -3.1416
3.1416 0.0054 -2.9279 0 -0.2191 3.1416
0.0022 1.5429 -2.928 3.1390 -1.3850 3.1416
-0.0022 -0.0055 0.0555 3.098 0.0500 -3.0982 (a) Top View (b) Side View
Fig. 10. Shoulder Singularity; X=Y=0, Z=600mm, α = γ = 0,
3) Singularity at z axis∗ : One particularity about the movement
β = − arctan L3xL+L4x rad
of the rigid arm, that is clearly seen when there are more than 3z

eight cluster regions, happens when one or more degrees of freedom

are lost. According to the author in [5], ”A robot singularity is a For this family of rigid bodies, there are three well-known singu-
configuration in which the robot end-effector becomes blocked in larities: Wrist, Elbow and Shoulder. The last singularity is the one
certain directions.” This results can be seen in the Table V where explained above when the axes of the last joint lie in the plane passing
for each solution display in each row, the joint angles θ1 and θ6 are through the axes of joints 1 and 2, and a DOF is lost in the Z axis
symmetric since they cancel each other. Due to the sampling rate and rotation. On the other hand, when the axes of the last joint lie on the
8 ROBOTICS

plane passing through the axes of joints 2 and 3 results on a DOF VI Development∗†‡ 2
lost (Elbow singularity). Finally the Wrist rotation occurs when the VI-A Direct Kinematics‡ . . . . . . . . . . . . . . . 2
axes of the fourth and sixth joints are coincident, and it happens in VI-A1 Choice of the Coordinate System‡ . 2
the wrist inline architecture (whitout the L5z =5.5m). VI-A2 Solving the Position‡ . . . . . . . . 3
VI-A3 Solving the Orientation‡ . . . . . . 3
VI-B Inverse Kinematics∗† . . . . . . . . . . . . . 3
VIII. N OTES† VI-B1 Translatory and Rotational DOF∗ . 3
VI-B2 Position of the Fifth Joint∗ . . . . . 3
There were a few improvements from the first version (submitted): VI-B3 Planar Chain∗ . . . . . . . . . . . 4
• In the inverse kinematics.m file, when the point and orientation VI-B4 Segmented Transformation† . . . . 4
are not possible and the program has not any solution, it crashes.
The solution is to change the lines 78 to 89 after the if statement VII Experimental Results∗†‡ 5
to the lines 96 to 100. This part of the code is a simple plot VII-A Solutions for the Inline Wrist Experiment
that could even be commented; (w/o 5.5mm)‡∗ . . . . . . . . . . . . . . . . . 5
• In the file bonusPoint.m, the functions in inverse kinematics.m VII-A1 Direct Kinematic‡ . . . . . . . . . 5
need to be imported in order for it to work; VII-A2 Inverse Kinematic∗ . . . . . . . . . 5
• The input vector of the clustering algorithm is flipped, in order VII-B Solutions for the not Inline Wrist Experiment
to preserve the solution with less error, since when two solutions (w/ 5.5mm)∗†‡ . . . . . . . . . . . . . . . . . 6
are recognised to be at the same group the second is privileged. VII-B1 Inaccurate Solutions† . . . . . . . . 6
VII-B2 Accurate Solutions (Bonus Point)‡ 6
The Repository of this project including the code is available here. VII-B3 Singularity at z axis∗ . . . . . . . . 7

VIII Notes† 8
IX. C ONCLUSION†
IX Conclusion† 8
main focus of this work was to understand the calculations
T HE
and the principles behind the direct and inverse kinematics for
the Nyrio One robot. First, it is important to define the world frame,
X References 8

its position and orientation, as well as the auxiliary frames to ease L IST OF F IGURES
this process. Note that one has to be very careful with the frames that
he chose with respect to the world frame, because a small mistake 1 Coordinate Frames for the 6 joints and World Frame . 2
may lead to a lot of hours wasted in order to solve it! With the frames 2 Planar Chain in 2D view . . . . . . . . . . . . . . . . 4
set, the direct kinematics and inverse kinematics are easy to solve, 3 Planar Chain in 3D view . . . . . . . . . . . . . . . . 4
because both just need to use the transformation matrixes between 4 Experimental Result - Direct kinematics . . . . . . . . 5
frames. For the direct kinematics, it gets the point of the end effector 5 Euler Angles rotation [-1.8925, 2.7352, 0.8861], with
and orientation of the robot from given joint rotation angles, while for Z-Y-Z . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
the inverse kinematics, it gets the joint rotation angles from a given 6 Inline Wrist . . . . . . . . . . . . . . . . . . . . . . . 6
point of the end effector and the orientation of the robot. However, 7 Solution with less error in relation to the home position 6
for the inverse kinematics it is important to notice when the robot 8 Inaccurate Solutions for the rigid body maximum
loses degrees of freedom, singularities, that gives an infinite number stretched out . . . . . . . . . . . . . . . . . . . . . . . 7
of solutions. 9 θ6 error throughout the trials . . . . . . . . . . . . . . 7
With that being said, it is possible to understand the importance 10 Shoulder Singularity; X=Y=0, Z=600mm, α = γ = 0,
of the kinematics for robotics manipulators, which do not consider β = − arctan L3xL+L3z
4x
rad . . . . . . . . . . . . . . . 7
the physics aspects of it, that is, its mass, the material of which it
is made, etc., the kinematics not only allow one to know the actual L IST OF TABLES
position and orientation of the robot, but also they can compute the I Solutions for the inverse kinematics in figure 6 in rad. . 6
decisions to make for the robot to be in certain position with a certain II Solutions for the inverse kinematics for the home position 6
orientation. III Solutions for the inverse kinematics for the stretched out
One way to improve the work developed in this laboratory could robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
be to minimize the energy used in the movements for the inverse IV Accurate Solutions for the Home Position . . . . . . . 7
kinematics, which gives more than one solution, so the program V Solutions for the inverse kinematics when a singularity
chooses the solution that consumes the minimum energy. Other way happens . . . . . . . . . . . . . . . . . . . . . . . . . . 7
to improve it is to consider not only physical constrains in the joints,
but also constrains in the environment where the robot is. For example
it could consider the floor or an obstacle, in order to restrict the robot R EFERENCES
movements. [1] S. Ganapathy, ”Decomposition of transformation matrices for
robot vision,” Proceedings. 1984 IEEE International Conference
on Robotics and Automation, Atlanta, GA, USA, 1984, pp. 130-
C ONTENTS 139, doi: 10.1109/ROBOT.1984.1087163.
[2] F. Yang, Y. Zhu and P. B. Kingsley, ”A further inves-
I Abstract∗ 1 tigation of the Euler angle calculation in diffusion ten-
sor imaging,” 2016 IEEE 13th International Conference on
II Introduction‡ 1 Signal Processing (ICSP), Chengdu, 2016, pp. 39-43, doi:
10.1109/ICSP.2016.7877792.
[3] Kumar. V. ”Introduction to Robot Geometry and Kinematics”.
III Background/Concepts∗ 1
[4] ”Niryo One Mechanical Specifications”
[5] B. Ilian ”What are Singularities in a Six-Axis Robot Arm?”.
IV State of the art∗ 2

V Ease of use∗ 2