Kinematic Modeling with Experimental Validation of a KUKA^®–Kinova^® Holonomic Mobile Manipulator

Abstract

We have proposed an open-source holonomic mobile manipulator composed of the KUKA youBot holonomic mobile platform with four Swedish wheels and a stationary aboard six-degrees-of-freedom Kinova Jaco Gen 2H manipulator, and we have developed corresponding kinematic problems. We have defined forward and inverse analytic Jacobians and designed Jacobian algorithms of forward and inverse mobile manipulator kinematics. An experimental test conducted with the designed laboratory prototype of the investigated mobile manipulator with the described kinematics was used to verify the obtained theoretical results. The goal of the test was to keep constant the position of the gripper in 3D space while the mobile platform is moving to some extend in the 2D workspace.

1. Introduction

In the present day, robots with various configurations (mobile robots, robot manipulators, mobile manipulators, etc.) are very rapidly advancing in performing different application tasks in our everyday lives. There are also many companies that offer on the market service robots dedicated to helping disabled people in their daily activities. Currently, various approaches for human–robot interactions are gaining popularity, including, for example, manual guidance, among others. It should be noted that such machines, especially in their recently investigated collaborative implementation, must correspond to the safety standards developed in the EU [1,2,3]. The widespread use of robots also requires a high level of understanding of the problems involved into this field of science [4,5,6,7,8]. In the present work, we describe existing research into the mathematical modeling of the kinematics of a holonomic mobile manipulator that has remarkable mobility and handling capabilities. The combined machinery contains two devices: a mobile platform and a 6 DoF manipulator. The chosen mobile platform, KUKA youBot, has four omnidirectional wheels, and it is widely used for scientific research tasks [9]. Installed on the top of the mobile platform robot manipulator, Kinova Jaco Gen2 is often used to assist disabled people by helping them with their daily activities, such as drinking water, holding a spoon, etc. The manipulator has also been selected for the experiments because it is supported by numerous software tools that make its control open to users and provide an opportunity to explore different control approaches. Such a combination of mobile platforms and manipulators widens the fields of application of the resulting mobile manipulators. Examples of similar assemblies can be found in [10,11,12]. All those efforts aimed to extend the working space of manipulators. However, the assembly of a mobile platform and a manipulator requires relevant kinematic models. An interesting solution can be found in [13], where authors present a configuration of a mobile manipulator based on Boston Dynamics Spot that is additionally equipped with a robotic arm. Some of the mobile manipulators with open-chain kinematics have been studied and are presented in [14,15,16,17,18,19,20]. More recent studies on the inverse kinematics frameworks of mobile manipulators are given in [21,22,23,24].

In the present work, we derive the forward kinematics, inverse kinematics, Jacobian and inverse Jacobian of the investigated mobile manipulator. The obtained mathematical models are then applied to control the movement of the laboratory prototype mobile manipulator in the task of maintaining a constant position and orientation of the manipulator’s end-effector in 3D space while the mobile platform is moving to some extent in the 2D workspace. This is achieved by controlling the speed of each mobile manipulator link and the speed of each mobile platform wheel. Afterwards, the efficacy of the proposed approach is evaluated by running a real-world experiment.

The article is structured in the following way: In Section 2, we describe the components of the mobile manipulator and derive their kinematic models. Then, the forward and inverse kinematics of the mobile manipulator and Jacobian algorithms are derived. In Section 3, the achieved results from the conducted experimental test carried out with the constructed laboratory prototype are presented. Section 4 contains conclusions.

2. Devices and Mathematical Models

2.1. The Mobile Manipulator

The assembled structure consists of two essential devices: a holonomic mobile platform and a six-degrees-of-freedom (6 DoF) robot manipulator (Figure 1). The mobile platform is a KUKA youBot holonomic platform, and the implemented spherical wrist robot is a Jaco Gen2 manipulator produced by the Canadian company Kinova.

Figure 1. Mobile manipulator and velocities.

This paper discusses the mathematical derivation of many important aspects that are related to the designed mobile manipulator: the forward and inverse kinematics of the mobile platform, forward and inverse kinematics of the manipulator Jaco Gen2, combined homogeneous transformation between the mobile platform and the end-effector, manipulator’s Jacobian matrix, manipulator’s inverse Jacobian matrix, and synchronization between the velocities of these two individual components. The obtained theoretical results have been verified via the conducted experimental test using a laboratory prototype of the above mobile manipulator. The implemented control software of the prototype uses ROS Melodic (Robot Operating System)-implemented packages, which were either provided by the manufacturers of the devices or are freely available from the web. The transfer of the control signals and feedback values is provided by the ROS’s topics, the ROS’s actions, and the ROS’s services. Each one of these types of communication is used depending on the existing situation. The presented results show the velocities of the mobile platform ${\mathrm{V}}_{\mathrm{mp}}$ and the robot manipulator ${\mathrm{V}}_{\mathrm{man}}$. These velocities allow for achieving a stable positioning of the end-effector during a movement of the mobile platform. The resulting velocity is ${\mathrm{V}}_{\mathrm{res}}$.

$${\mathrm{V}}_{\mathrm{res}}=\pm {\mathrm{V}}_{\mathrm{mp}}\pm {\mathrm{V}}_{\mathrm{man}}$$

(1)

2.2. The Mobile Platform KUKA youBot

The KUKA youBot mobile platform gives us the opportunity to expand the working space of the manipulator. Thus, the proposed structure of mobile manipulator enhances its functionalities.

However, in the design, this combination also increases the complexity of the whole robot system structure while increasing the mobile robot’s own weight and energy consumption. In order to complete complex work, the manipulator and omnidirectional vehicle must be in perfect harmony, and they also need to reduce energy loss. Therefore, the study of omnidirectional mobile operating robot has important theoretical and practical value.

The next figure (Figure 2) shows the most significant dimensions of the KUKA youBot mobile platform and its structure.

Figure 2. Geometric dimensions (in [mm]) of the mobile platform KUKA youBot.

The mobile platform is controlled by an onboard computer with embedded Intel Atom dual-core CPU and 2 GB RAM, 32 GB SSD, WLAN and a USB interface. Available interfaces are USB and EtherCAT/Ethernet. The existing WiFi connection makes possible the establishment of ROS communication.

The conventional approach for description of the motion of the mobile platform in a 2D plane requires the definition of coordinate frames. To specify the position and the orientation of the mobile platform onto the plane, we have to establish a relationship between the reference frame of the plane and the local frame of the platform. The axes ${\mathrm{X}}_{\mathrm{I}}$ and ${\mathrm{Y}}_{\mathrm{I}}$ define an arbitrary inertial basis on the plane as the global reference frame from some origin $\mathrm{O}:\left\{{\mathrm{X}}_{\mathrm{I}},{\mathrm{Y}}_{\mathrm{I}}\right\}$. To indicate the position and orientation of the mobile platform, we choose a geometric center of the platform chassis-point P, and, thus, it is the origin of the platform’s local reference frame. The axes $\left\{{\mathrm{X}}_{\mathrm{R}},{\mathrm{Y}}_{\mathrm{R}}\right\}$ define the mobile platform’s local reference frame. The column vector (2) specifies the position and the orientation of the platform local reference frame in the global reference frame [25,26]:

$$\mathrm{P}=\left[\begin{array}{c}\mathrm{x}\\ \mathrm{y}\\ \mu \end{array}\right]$$

(2)

where x and y are the displacements along the ${\mathrm{X}}_{\mathrm{I}}$ axis and ${\mathrm{Y}}_{\mathrm{I}}$ axis, and the orientation is defined by $\mu$.

The KUKA youBot mobile robotic platform is controlled by varying the rotational speed of each of its omnidirectional wheels (Figure 3b).

Figure 3. A sketch with the necessary notations to calculate the forward kinematics of the mobile platform: (a) wheel parameters and (b) wheels and the corresponding speeds.

The kinematic model describes the relationship between the rotational speeds of the four wheels ${\dot{\phi }}_1$, ${\dot{\phi }}_2$, ${\dot{\phi }}_3$ and ${\dot{\phi }}_4$ and the speed of the mobile platform in the 2D plane. The types of wheels and robot chassis geometry define this relationship. Each wheel introduces a set of mathematical constraints on the movement of the platform.

According to [25], using the constraints, we can obtain the kinematic model of the mobile robot. For a Swedish (mecanum) wheel, the following equations must be satisfied [25,26]:

$$\begin{gathered} {\mathrm{V}}_1\mathrm{R}(\mathit{\mu })\dot{\mathrm{P}}-{\mathrm{r}\dot{\phi }}_{\mathrm{i}}\cos \left({\mathsf{\gamma }}_{\mathrm{i}}\right)=0 \\ {\mathrm{V}}_2\mathrm{R}(\mathit{\mu })\dot{\mathrm{P}}-{\mathrm{r}\dot{\phi }}_{\mathrm{i}}\cos \left({\mathsf{\gamma }}_{\mathrm{i}}\right)-{\mathrm{r}}_{\mathrm{sw}}{\dot{\phi }}_{\mathrm{sw}}=0, \end{gathered}$$

(3)

where V1 and V2 denote the following three-element vectors:

$${\mathrm{V}}_1=\left[\sin \left({\mathsf{\alpha }}_{\mathrm{i}}+{\mathsf{\beta }}_{\mathrm{i}}+{\mathsf{\gamma }}_{\mathrm{i}}\right)-\cos \left({\mathsf{\alpha }}_{\mathrm{i}}+{\mathsf{\beta }}_{\mathrm{i}}+{\mathsf{\gamma }}_{\mathrm{i}}\right)-\mathrm{lcos}\left({\mathsf{\beta }}_{\mathrm{i}}+{\mathsf{\gamma }}_{\mathrm{i}}\right)\right]$$

$${\mathrm{V}}_2=\left[\cos \left({\mathsf{\alpha }}_{\mathrm{i}}+{\mathsf{\beta }}_{\mathrm{i}}+{\mathsf{\gamma }}_{\mathrm{i}}\right)\sin \left({\mathsf{\alpha }}_{\mathrm{i}}+{\mathsf{\beta }}_{\mathrm{i}}+{\mathsf{\gamma }}_{\mathrm{i}}\right)-\mathrm{lsin}\left({\mathsf{\beta }}_{\mathrm{i}}+{\mathsf{\gamma }}_{\mathrm{i}}\right)\right]$$

the matrix R is the rotational matrix, and r denotes the radius of the wheels.

$$\mathrm{R}=\left[\begin{array}{ccc}\cos \mathit{\mu }& \sin \mathit{\mu }& 0\\ -\sin \mathit{\mu }& \cos \mathit{\mu }& 0\\ 0& 0& 1\end{array}\right]$$

The index $\mathrm{i}$ denotes the number of platform wheels.

Considering the construction parameters of the platform: the length ${\mathrm{l}}_{\mathrm{x}}=235.5 \mathrm{mm}$, ${\mathrm{l}}_{\mathrm{y}}=150 \mathrm{mm} \mathrm{and} \mathrm{the} \mathrm{angle} \mathsf{\gamma }=\pm 45°$, (the robot dimensions are shown in (Figure 2) and the notations in (Figure 3a)), the angles ${\mathsf{\alpha }}_{\mathrm{i}}$ and ${\mathsf{\beta }}_{\mathrm{i}}$ can be calculated according to the following equations [26]:

$$\begin{gathered} {\mathsf{\alpha }}_1=\mathrm{arctg}\left(\frac{{\mathrm{l}}_{\mathrm{y}}}{{\mathrm{l}}_{\mathrm{x}}}\right),{\mathsf{\alpha }}_2={180}^{°}-\mathrm{arctg}\left(\frac{{\mathrm{l}}_{\mathrm{y}}}{{\mathrm{l}}_{\mathrm{x}}}\right) \\ {\mathsf{\alpha }}_3={180}^{°}+\mathrm{arctg}\left(\frac{{\mathrm{l}}_{\mathrm{y}}}{{\mathrm{l}}_{\mathrm{x}}}\right),{\mathsf{\alpha }}_4=\mathrm{arctg}\left(\frac{-{\mathrm{l}}_{\mathrm{y}}}{{\mathrm{l}}_{\mathrm{x}}}\right) \end{gathered}$$

(4)

where, for the considered platform,${\mathrm{l}}_{\mathrm{x}}=235.5 \mathrm{mm}$ and ${\mathrm{l}}_{\mathrm{y}}=150 \mathrm{mm}$.

According to the theory of kinematics of the Swedish wheel [25], and considering the fact that the axes of the four wheels are perpendicular to PX_R, then using the notations in Figure 3a, it is possible to express the angle ${\beta }_i$ and the length $l$:

$${\beta }_i=90-{\mathsf{\alpha }}_{\mathrm{i}};$$

(5)

$$l=\sqrt{{\mathrm{l}}_{\mathrm{x}}^2+{\mathrm{l}}_{\mathrm{y}}^2}\approx 0.279 \mathrm{m}$$

Table 1 contains the kinematic parameters of the mobile platform.

Table 1. Kinematic parameters of the mobile platform.

	$\mathit{\alpha } \left[\mathbf{deg}\right]$	β [deg]	$\mathit{\gamma } \left[\mathbf{deg}\right]$	$\mathbf{l}\left[\mathbf{m}\right]$	$\mathbf{r}\left[\mathbf{m}\right]$
Wheel (1)	32.5	57.5	−45	0.279	0.05
Wheel (2)	147.5	−57.5	45	0.279	0.05
Wheel (3)	−147.5	−122.5	−45	0.279	0.05
Wheel (4)	−32.5	122.5	45	0.279	0.05

The forward kinematics of the mobile platform is described by the following matrix equation [26]:

$$\left[\begin{array}{c}\dot{\mathrm{x}}\\ \dot{\mathrm{y}}\\ \dot{\mu }\end{array}\right]=\frac{\mathrm{r}}{4}\left[\begin{array}{cccc}1& 1& 1& 1\\ -1& 1& -1& 1\\ \frac{-1}{\left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)}& \frac{-1}{\left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)}& \frac{1}{\left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)}& \frac{1}{\left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)}\end{array}\right]\left[\begin{array}{c}{\dot{\phi }}_1\\ {\dot{\phi }}_2\\ {\dot{\phi }}_3\\ {\dot{\phi }}_4\end{array}\right]$$

(6)

And the inverse kinematic model is

$$\left[\begin{array}{c}{\dot{\phi }}_1\\ {\dot{\phi }}_2\\ \begin{array}{c}{\dot{\phi }}_3\\ {\dot{\phi }}_4\end{array}\end{array}\right]=\frac{1}{\mathrm{r}}\left[\begin{array}{ccc}1& -1& -\left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)\\ 1& 1& -\left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)\\ \begin{array}{c}1\\ 1\end{array}& \begin{array}{c}-1\\ 1\end{array}& \begin{array}{c}\left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)\\ \left({\mathrm{l}}_{\mathrm{x}}+{\mathrm{l}}_{\mathrm{y}}\right)\end{array}\end{array}\right]\left[\begin{array}{c}\dot{\mathrm{x}}\\ \dot{\mathrm{y}}\\ \dot{\mu }\end{array}\right]$$

(7)

2.3. The Manipulator KINOVA Jaco Gen 2

An overview of the robot manipulator Jaco Gen2 and its dimensions are shown on (Figure 4) [27].

Figure 4. Notations (a) and dimensions (b) of Jaco Gen 2 manipulator.

The geometric dimensions are shown in Table 2; these values are essential components of forward kinematics and notations: ${\mathrm{D}}_1,{\mathrm{D}}_2,{\mathrm{D}}_3,{\mathrm{D}}_4,{\mathrm{D}}_5,{\mathrm{D}}_6{ \mathrm{and}\mathrm{e}}_2$ are structural elements of the homogeneous transformations.

Table 2. Structural elements of the homogeneous transformations [27].

Parameters	Descriptions	Length (m)
${\mathrm{D}}_1$	Base to shoulder	0.2755
${\mathrm{D}}_2$	Upper Length (shoulder to elbow)	0.41
${\mathrm{D}}_3$	Forearm length (elbow to wrist)	0.2073
${\mathrm{D}}_4$	First wrist length	0.1038
${\mathrm{D}}_5$	Second wrist length	0.1038
${\mathrm{D}}_6$	Wrist to center of the hand	0.16
${\mathrm{e}}_2$	Joint 3–4 lateral offset	0.0133

In Table 3, some physical characteristics of the manipulator Jaco Gen2 Spherical Wrist are given.

Table 3. Physical characteristics of the manipulator Jaco Gen2 [27].

Characteristics	Values
Total weight	4.4 kg
Reach	98.4 cm
Maximum payload	2.6 kg (mid-range)/2.2 kg (full-range)
Materials	carbon fiber/aluminum
Maximum linear arm speed	20 cm/sec
Power supply voltage	18–29 VDC
Average power	25 W (15 W standby)
Peak power	100 W
Communication protocol	RS485
Communication cables	20 pins flat flex cable
Water resistance	IPX2
Operating temperature	−10 to 40

Kinova Jaco Gen2 has specifically designed actuators mechanics and electronics. They use DC brushless motors with harmonic drive technology and are equipped with encoders, a torque sensor, a current sensor, a temperature sensor and accelerometers [23]. Kinova’s actuators and parameters are presented in Figure 5 and Table 4, respectively.

Figure 5. Kinova’s actuators: K-75+, K-75 and K-58.

Table 4. Kinova Jaco Gen 2’s actuator specifications.

Title	$\mathbf{K}-75+$	$\mathbf{K}-75$	$\mathbf{K}-58$
Nominal torque (Nm)	12	9.2	3.6
Peak torque (Nm)	30.5	18	6.8
No load speed (rpm)	12.2	9.8	20.3
Nominal speed (rpm)	9.4	7	15
Weight (g)	570	587	357
Reduction ratio	136	160	110
Angular ranges, software limited (turns)	±27.7
Communication protocol	RS485

Multiple functionalities are offered by the software provided by the company Kinova (Boisbriand, QC, Canada). Using the Development Center and the Torque Console [27], researchers can monitor the manipulator’s state, activate admittance, switch between Cartesian and angular control and define trajectories. There is a C++ programming function integrated that can be used to access the API, for example, the API function can deactivate auto-avoidance behavior. Kinova Jaco 2 is an integrable device in ROS environment. The kinova-ros stack (set of ROS packages) provides a ROS interface for Kinova Jaco2. The stack is developed above the Kinova C++ API functions, which communicate with the DSP inside the robot base [27]. Using our knowledge of the robot’s structure and measurements, we define its forward kinematic table, using classical Denavit–Hartenberg convention (Table 5).

Table 5. Table formed using Denavit–Hartenberg parameters, classical approach [27].

$\mathbf{i}$	${\alpha }_{\mathbf{i}}$	${\mathbf{a}}_{\mathbf{i}}$	${\mathbf{d}}_{\mathbf{i}}$	${\theta }_{\mathbf{i}}$
1	$\frac{\mathsf{\pi }}{2}$	0	${\mathrm{D}}_1$	${\theta }_1$
2	$\mathsf{\pi }$	${\mathrm{D}}_2$	0	${\theta }_2$
3	$\frac{\mathsf{\pi }}{2}$	0	$-{\mathrm{e}}_2$	${\theta }_3$
4	$\frac{\mathsf{\pi }}{2}$	0	$-\left({\mathrm{D}}_3+{\mathrm{D}}_4\right)$	${\theta }_4$
5	$\frac{\mathsf{\pi }}{2}$	0	0	${\theta }_5$
6	$\mathsf{\pi }$	0	$-\left({\mathrm{D}}_5+{\mathrm{D}}_6\right)$	${\theta }_6$

The classical Denavit–Hartenberg convention is defined by the following matrix [28]:

$${}_{\mathrm{i}}{}^{\mathrm{i}-1}\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_{\mathrm{i}}& -{\mathrm{s}\theta }_{\mathrm{i}}{\mathrm{c}\mathsf{\alpha }}_{\mathrm{i}}& {\mathrm{s}\theta }_{\mathrm{i}}{\mathrm{s}\mathsf{\alpha }}_{\mathrm{i}}& {\mathrm{a}}_{\mathrm{i}}{\mathrm{c}\theta }_{\mathrm{i}}\\ {\mathrm{s}\theta }_{\mathrm{i}}& {\mathrm{c}\theta }_{\mathrm{i}}{\mathrm{c}\mathsf{\alpha }}_{\mathrm{i}}& -{\mathrm{c}\theta }_{\mathrm{i}}{\mathrm{s}\mathsf{\alpha }}_{\mathrm{i}}& {\mathrm{a}}_{\mathrm{i}}{\mathrm{s}\theta }_{\mathrm{i}}\\ 0& {\mathrm{s}\mathsf{\alpha }}_{\mathrm{i}}& {\mathrm{c}\mathsf{\alpha }}_{\mathrm{i}}& {\mathrm{d}}_{\mathrm{i}}\\ 0& 0& 0& 1\end{array}\right]$$

(8)

The modified Denavit–Hartenberg convention is defined by the following matrix [29]:

$${}_{\mathrm{i}}{}^{\mathrm{i}-1}\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_{\mathrm{i}}& -{\mathrm{s}\theta }_{\mathrm{i}}& 0& {\mathrm{a}}_{\mathrm{i}-1}\\ {\mathrm{s}\theta }_{\mathrm{i}}{\mathrm{c}\mathsf{\alpha }}_{\mathrm{i}-1}& {\mathrm{c}\theta }_{\mathrm{i}}{\mathrm{c}\mathsf{\alpha }}_{\mathrm{i}-1}& -{\mathrm{s}\mathsf{\alpha }}_{\mathrm{i}-1}& -{\mathrm{s}\mathsf{\alpha }}_{\mathrm{i}-1}{\mathrm{d}}_{\mathrm{i}}\\ {\mathrm{s}\theta }_{\mathrm{i}}{\mathrm{s}\mathsf{\alpha }}_{\mathrm{i}-1}& {\mathrm{c}\theta }_{\mathrm{i}}{\mathrm{s}\mathsf{\alpha }}_{\mathrm{i}-1}& {\mathrm{c}\mathsf{\alpha }}_{\mathrm{i}-1}& {\mathrm{c}\mathsf{\alpha }}_{\mathrm{i}-1}{\mathrm{d}}_{\mathrm{i}}\\ 0& 0& 0& 1\end{array}\right]$$

(9)

We mark the trigonometric functions ${\mathrm{c}\theta }_{\mathrm{i}}$ and ${\mathrm{s}\theta }_{\mathrm{i}}$ with ${\cos \theta }_{\mathrm{i}}$ and ${\sin \theta }_{\mathrm{i}}$, respectively. Additionally, ${\mathrm{c}\theta }_{\mathrm{ij}}$ and ${\mathrm{s}\theta }_{\mathrm{ij}}$ are equal to $\cos \left({\theta }_{\mathrm{i}}+{\theta }_{\mathrm{j}}\right)$ and $\sin \left({\theta }_{\mathrm{i}}+{\theta }_{\mathrm{j}}\right)$, respectively.

The Manipulator Jaco Gen 2 Inverse Kinematics

All approaches to obtain the inverse kinematics of a robot manipulator proposed in the scientific literature can be divided into two classes: (i) approaches using numerical solutions and (ii) approaches based on closed-form solutions. Due to their iterative nature, approaches based on the numerical solutions are much slower than those using closed-form solution. So, in the most applications, especially for online implementations, the numerical approach for solving inverse kinematics is not suitable.

“Closed form” denotes a solution method based on analytic expressions or the solution of polynomials of degree 4 or less. Although, in the general case, the six-degrees-of-freedom manipulator does not have a closed–form solution, for some particular manipulator structures characterized by either several intersecting joint axes or several angles ${\mathsf{\alpha }}_{\mathrm{i}}$ equal to 0 or ±90 degrees, closed-form solutions exist.

Such an approach to obtain a closed-form solution is Pieper’s approach that studies manipulators with six degrees of freedom where three of their consecutive axes intersect at a point [29]. The inverse kinematic problem calculates the desired angular position ${\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4,{\theta }_5,{\mathrm{and}\theta }_6$ of each rotational joint, according the predefined position and orientation in the final homogeneous transformation ${}_6{}^0\mathrm{T}$. We have implemented Pieper’s method for all six revolute joints of Jaco Gen 2, with the last three axes intersecting. The idea in Pieper’s solution is to split the calculation into two separate problems—the first one, taking into account the first three joints, and the second one, considering the last three joints. A brief formulation of the algorithm can be presented as follows:

• Locate the intersection point of the last three joint axes;
• Calculate the position of this intersection point, given that we know the desired position and orientation of the end-effector;
• Solve inverse kinematics for first three joints;
• Compute ${}_3{}^0\mathrm{T}$ and determine ${}_6{}^3\mathrm{T}$;
• Solve the inverse kinematics for the last three joints.

Using the modified Denavit–Hartenberg convention, given by (9) we can derive the homogeneous transformation (Table 6):

Table 6. Table formed by Denavit–Hartenberg parameters, using a modified approach.

$\mathbf{i}$	${\alpha }_{\mathbf{i}-1}$	${\mathbf{a}}_{\mathbf{i}-1}$	${\mathbf{d}}_{\mathbf{i}}$	${\theta }_{\mathbf{i}}$
1	$0$	0	0	${\theta }_1$
2	$\frac{\mathsf{\pi }}{2}$	0	0	${\theta }_2$
3	$0$	$D_2$	${\mathrm{e}}_2$	${\theta }_3$
4	$-\frac{\mathsf{\pi }}{2}$	0	${\mathrm{D}}_3+{\mathrm{D}}_4$	${\theta }_4$
5	$\frac{\mathsf{\pi }}{2}$	0	0	${\theta }_5$
6	$-\frac{\mathsf{\pi }}{2}$	0	$0$	${\theta }_6$

Where ${\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4,{\theta }_5{ \mathrm{and}\theta }_6$ are the known angular displacements as follows:

$${}_1{}^0\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_1& -{\mathrm{s}\theta }_1& 0& 0\\ {\mathrm{s}\theta }_1& {\mathrm{c}\theta }_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(10)

$${}_2{}^1\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_2& -{\mathrm{s}\theta }_2& 0& 0\\ 0& 0& 1& 0\\ -{\mathrm{s}\theta }_2& -{\mathrm{c}\theta }_2& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(11)

$${}_3{}^2\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_3& -{\mathrm{s}\theta }_3& 0& 0.41\\ {\mathrm{s}\theta }_3& {\mathrm{c}\theta }_3& 0& 0\\ 0& 0& 1& 0.0133\\ 0& 0& 0& 1\end{array}\right]$$

(12)

$${}_4{}^3\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_4& -{\mathrm{s}\theta }_4& 0& 0\\ 0& 0& 1& 0.3111\\ -{\mathrm{s}\theta }_4& -{\mathrm{c}\theta }_4& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(13)

$${}_5{}^4\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_5& -{\mathrm{s}\theta }_5& 0& 0\\ 0& 0& -1& 0\\ {\mathrm{s}\theta }_5& {\mathrm{c}\theta }_5& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(14)

$${}_6{}^5\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_6& -{\mathrm{s}\theta }_6& 0& 0\\ 0& 0& 1& 0\\ -{\mathrm{s}\theta }_6& -{\mathrm{c}\theta }_6& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(15)

The complete model of forward kinematics, using a modified Denavit–Hartenberg convention [29], taking into account the actual angular displacement of each rotational joint, is

$${}_6{}^0\mathrm{T}={}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right){}_4{}^3\mathrm{T}\left({\theta }_4\right){}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)$$

(16)

In order to solve the inverse kinematics, we have to calculate the angular position of each rotational joint, knowing the desired orientation and position ${}_6{}^0\mathrm{T}$:

$${}_6{}^0\mathrm{T}=\left[\begin{array}{ccc}{\mathrm{r}}_{11}& {\mathrm{r}}_{12}& \begin{array}{cc}{\mathrm{r}}_{13}& {\mathrm{p}}_{\mathrm{x}}\end{array}\\ {\mathrm{r}}_{21}& {\mathrm{r}}_{22}& \begin{array}{cc}{\mathrm{r}}_{23}& {\mathrm{p}}_{\mathrm{y}}\end{array}\\ \begin{array}{c}{\mathrm{r}}_{31}\\ 0\end{array}& \begin{array}{c}{\mathrm{r}}_{32}\\ 0\end{array}& \begin{array}{c}\begin{array}{cc}{\mathrm{r}}_{33}& {\mathrm{p}}_{\mathrm{z}}\end{array}\\ \begin{array}{cc}0& 1\end{array}\end{array}\end{array}\right]={}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right){}_4{}^3\mathrm{T}\left({\theta }_4\right){}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)$$

(17)

Values ${\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4,{\theta }_5{ \mathrm{and}\theta }_6$ in (17) are the desired angular positions, and ${}_6{}^0\mathrm{T}$ contents are the predefined desired orientations and positions. Using (17), we put ${}_1{}^0\mathrm{T}\left({\theta }_1\right)$ on the left-hand side of the equation:

$${\left[{}_1{}^0\mathrm{T}\left({\theta }_1\right)\right]}^{-1}{}_6{}^0\mathrm{T}={}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right){}_4{}^3\mathrm{T}\left({\theta }_4\right){}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)$$

(18)

Inverting the matrix ${}_1{}^0\mathrm{T}$, we can write (18)

$$\left[\begin{array}{ccc}{\mathrm{c}\theta }_1& {\mathrm{s}\theta }_1& \begin{array}{cc}0& 0\end{array}\\ -{\mathrm{s}\theta }_1& {\mathrm{c}\theta }_1& \begin{array}{cc}0& 0\end{array}\\ \begin{array}{c}0\\ 0\end{array}& \begin{array}{c}0\\ 0\end{array}& \begin{array}{c}\begin{array}{cc}1& 0\end{array}\\ \begin{array}{cc}0& 1\end{array}\end{array}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{r}}_{11}& {\mathrm{r}}_{12}& \begin{array}{cc}{\mathrm{r}}_{13}& {\mathrm{p}}_{\mathrm{x}}\end{array}\\ {\mathrm{r}}_{21}& {\mathrm{r}}_{22}& \begin{array}{cc}{\mathrm{r}}_{23}& {\mathrm{p}}_{\mathrm{y}}\end{array}\\ \begin{array}{c}{\mathrm{r}}_{31}\\ 0\end{array}& \begin{array}{c}{\mathrm{r}}_{32}\\ 0\end{array}& \begin{array}{c}\begin{array}{cc}{\mathrm{r}}_{33}& {\mathrm{p}}_{\mathrm{z}}\end{array}\\ \begin{array}{cc}0& 1\end{array}\end{array}\end{array}\right]={}_6{}^1\mathrm{T}$$

(19)

According to Pieper’s approach, we equate the element placed on row 2, column 4 inside the matric ${}_6{}^1\mathrm{T}$ with the element placed on row 2, column 4 from the matrix ${\left[{}_1{}^0\mathrm{T}\left({\theta }_1\right)\right]}^{-1}{}_6{}^0\mathrm{T}$. The unknown angular displacement θ1 can now be expressed by (20).

$$-{\mathrm{s}\theta }_1{\mathrm{p}}_{\mathrm{x}}+{\mathrm{c}\theta }_1{\mathrm{p}}_{\mathrm{y}}=0.0133$$

(20)

To solve an equation of this form, we make the following trigonometric substitutions:

$$\begin{gathered} {\mathrm{p}}_{\mathrm{x}}=\mathsf{\rho }\mathrm{c}\varphi \\ {\mathrm{p}}_{\mathrm{y}}=\mathsf{\rho }\mathrm{s}\varphi \end{gathered}$$

(21)

where $\mathsf{\rho }$ is the distance from the origin of the base coordinate system to the point defined by the x and y coordinates of the end-effector:

$$\begin{gathered} \mathsf{\rho }=\sqrt{{\mathrm{p}}_{\mathrm{x}}^2+{\mathrm{p}}_{\mathrm{y}}^2} \\ \varphi =\mathrm{Atan}2\left({\mathrm{p}}_{\mathrm{y}},{\mathrm{p}}_{\mathrm{x}}\right) \end{gathered}$$

(22)

Substituting (21) into (20) [29],

$${\mathrm{c}\theta }_1\mathrm{s}\varphi -{\mathrm{s}\theta }_1\mathrm{c}\varphi =\frac{0.0133}{\mathsf{\rho }}$$

(23)

From the difference-of-angles formula,

$$\mathrm{s}\left(\varphi -{\theta }_1\right)=\frac{0.0133}{\mathsf{\rho }}$$

(24)

Hence,

$$\mathrm{c}\left(\varphi -{\theta }_1\right)=\pm \sqrt{1-\frac{0.00017689}{{\mathsf{\rho }}^2}}$$

(25)

and so

$$\varphi -{\theta }_1=\mathrm{Atan}2\left(\frac{0.0133}{\mathsf{\rho }},\pm \sqrt{1-\frac{0.00017689}{{\mathsf{\rho }}^2}}\right)$$

(26)

The solution for ${\theta }_1$ can be written as

$${\theta }_1=\mathrm{Atan}2\left({\mathrm{p}}_{\mathrm{y}},{\mathrm{p}}_{\mathrm{x}}\right)-\mathrm{Atan}2\left(0.0133,\pm \sqrt{{\mathrm{p}}_{\mathrm{x}}^2+{\mathrm{p}}_{\mathrm{y}}^2-0.00017689}\right)$$

(27)

We have found two possible solutions for the desired value of ${\theta }_1$, corresponding to the plus-or-minus sign in (27). Now that ${\theta }_1$ is known, the left-hand side of (19) is known. By equating the elements on row 1, column 4 and row 3, column 4 from both sides of (19), we can write Equation (28) as follows [29]:

$$\begin{gathered} {\mathrm{c}\theta }_1{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{p}}_{\mathrm{y}}=0.41{\mathrm{c}\theta }_2-0.3111{\mathrm{s}\theta }_{23} \\ -{\mathrm{p}}_{\mathrm{z}}=0.41{\mathrm{s}\theta }_2+0.3111{\mathrm{c}\theta }_{23} \end{gathered}$$

(28)

After taking the squares of Equations (28) and (20) and adding them, the resulting equations are obtained:

$$-\left({\mathrm{D}}_3+{\mathrm{D}}_4\right){\mathrm{s}\theta }_3=\frac{{\mathrm{p}}_{\mathrm{x}}^2+{\mathrm{p}}_{\mathrm{y}}^2+{\mathrm{p}}_{\mathrm{z}}^2-{\mathrm{D}}_2^2-{\mathrm{e}}_2^2-{\left({\mathrm{D}}_3+{\mathrm{D}}_4\right)}^2}{2{\mathrm{D}}_2}$$

(29)

$${\mathrm{s}\theta }_3=\frac{0.265-{\mathrm{p}}_{\mathrm{x}}^2-{\mathrm{p}}_{\mathrm{y}}^2-{\mathrm{p}}_{\mathrm{z}}^2}{0.2551}=\mathrm{K}$$

(30)

$${\mathrm{c}\theta }_3=\pm \sqrt{1-{\mathrm{K}}^2}$$

(31)

$${\theta }_3=\mathrm{Atan}2\left(\mathrm{K},\pm \sqrt{1-{\mathrm{K}}^2}\right)$$

(32)

The plus or minus sign in (32) leads to two different solutions for the angle ${\theta }_3$. We can rewrite Equation (17) so that the entire left-hand side becomes a function of three parameters ${\theta }_1,{\theta }_2 {\mathrm{and}\theta }_3$. The only unknown parameter is ${\theta }_2$:

$${\left[{}_3{}^0\mathrm{T}\left({\theta }_2\right)\right]}^{-1}\left[\begin{array}{ccc}{\mathrm{r}}_{11}& {\mathrm{r}}_{12}& \begin{array}{cc}{\mathrm{r}}_{13}& {\mathrm{p}}_{\mathrm{x}}\end{array}\\ {\mathrm{r}}_{21}& {\mathrm{r}}_{22}& \begin{array}{cc}{\mathrm{r}}_{23}& {\mathrm{p}}_{\mathrm{y}}\end{array}\\ \begin{array}{c}{\mathrm{r}}_{31}\\ 0\end{array}& \begin{array}{c}{\mathrm{r}}_{32}\\ 0\end{array}& \begin{array}{c}\begin{array}{cc}{\mathrm{r}}_{33}& {\mathrm{p}}_{\mathrm{z}}\end{array}\\ \begin{array}{cc}0& 1\end{array}\end{array}\end{array}\right]={}_4{}^3\mathrm{T}\left({\theta }_4\right){}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)$$

(33)

or

$$\begin{gathered} \left[\begin{array}{cccc}{\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}& {\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}& -{\mathrm{s}\theta }_{23}& -0.41{\mathrm{c}\theta }_3\\ -{\mathrm{c}\theta }_1{\mathrm{s}\theta }_{23}& -{\mathrm{s}\theta }_1{\mathrm{s}\theta }_{23}& -{\mathrm{c}\theta }_{23}& 0.41{\mathrm{s}\theta }_3\\ -{\mathrm{s}\theta }_1& {\mathrm{c}\theta }_1& 0& 0.0133\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{r}}_{11}& {\mathrm{r}}_{12}& {\mathrm{r}}_{13}& {\mathrm{p}}_{\mathrm{x}}\\ {\mathrm{r}}_{21}& {\mathrm{r}}_{22}& {\mathrm{r}}_{23}& {\mathrm{p}}_{\mathrm{y}}\\ {\mathrm{r}}_{31}& {\mathrm{r}}_{32}& {\mathrm{r}}_{33}& {\mathrm{p}}_{\mathrm{z}}\\ 0& 0& 0& 1\end{array}\right]= \\ =\left[\begin{array}{cccc}{\mathrm{c}\theta }_4{\mathrm{c}\theta }_5{\mathrm{c}\theta }_6-{\mathrm{s}\theta }_4{\mathrm{s}\theta }_6& -{\mathrm{c}\theta }_6{\mathrm{s}\theta }_4-{\mathrm{c}\theta }_4{\mathrm{c}\theta }_5{\mathrm{s}\theta }_6& -{\mathrm{c}\theta }_4{\mathrm{s}\theta }_5& 0\\ {\mathrm{c}\theta }_6{\mathrm{s}\theta }_5& {\mathrm{s}\theta }_5{\mathrm{s}\theta }_6& {\mathrm{c}\theta }_5& 0.3111\\ -{\mathrm{c}\theta }_4{\mathrm{s}\theta }_6-{\mathrm{c}\theta }_5{\mathrm{c}\theta }_6{\mathrm{s}\theta }_4& {\mathrm{c}\theta }_5{\mathrm{s}\theta }_4{\mathrm{s}\theta }_6-{\mathrm{c}\theta }_4{\mathrm{c}\theta }_6& {\mathrm{s}\theta }_4{\mathrm{s}\theta }_5& 0\\ 0& 0& & 1\end{array}\right] \end{gathered}$$

(34)

If we equate the elements on row 1, column 4 and row 2, column 4 from both sides of (34), we can write [29]

$$\begin{gathered} {\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{p}}_{\mathrm{y}}-{\mathrm{s}\theta }_{23}{\mathrm{p}}_{\mathrm{z}}-0.41{\mathrm{c}\theta }_3=0 \\ -{\mathrm{c}\theta }_1{\mathrm{s}\theta }_{23}{\mathrm{p}}_{\mathrm{x}}-{\mathrm{s}\theta }_1{\mathrm{s}\theta }_{23}{\mathrm{p}}_{\mathrm{y}}-{\mathrm{c}\theta }_{23}{\mathrm{p}}_{\mathrm{z}}+0.41{\mathrm{s}\theta }_3=0.3111 \end{gathered}$$

(35)

These equations can be solved simultaneously for ${\mathrm{s}\theta }_{23}$ and ${\mathrm{c}\theta }_{23}$, resulting in

$$\begin{gathered} {\mathrm{s}\theta }_{23}=\frac{0.41\left({\mathrm{c}\theta }_1{\mathrm{s}\theta }_3{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{s}\theta }_3{\mathrm{p}}_{\mathrm{y}}-{\mathrm{c}\theta }_3{\mathrm{p}}_{\mathrm{z}}\right)}{{\mathrm{p}}_{\mathrm{z}}^2+{\left({\mathrm{c}\theta }_1{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{p}}_{\mathrm{y}}\right)}^2} \\ {\mathrm{c}\theta }_{23}=\frac{0.41\left({\mathrm{c}\theta }_1{\mathrm{c}\theta }_3{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{c}\theta }_3{\mathrm{p}}_{\mathrm{y}}+{\mathrm{s}\theta }_3{\mathrm{p}}_{\mathrm{z}}\right)-0.3111}{{\mathrm{p}}_{\mathrm{z}}^2+{\left({\mathrm{c}\theta }_1{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{p}}_{\mathrm{y}}\right)}^2} \end{gathered}$$

(36)

The denominators are equal and positive, so we solve for the sum of ${\theta }_2$ and ${\theta }_3$ as follows [29]:

$${\theta }_{23}=\mathrm{Atan}2\left(arg1,arg2\right)$$

(37)

where $arg1=\left[{\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{p}}_{\mathrm{y}}-0.41{\mathrm{c}\theta }_3\right]$ and $arg2=\left[0.41\left({\mathrm{c}\theta }_1{\mathrm{c}\theta }_3{\mathrm{p}}_{\mathrm{x}}+{\mathrm{s}\theta }_1{\mathrm{c}\theta }_3{\mathrm{p}}_{\mathrm{y}}+{\mathrm{s}\theta }_3{\mathrm{p}}_{\mathrm{z}}\right)-0.3111\right]$.

Equation (37) computes four values of ${\theta }_2{\theta }_3$, according to the four possible combinations of solutions for ${\theta }_1$ and ${\theta }_3$; then, four possible solutions for the desired angular position ${\theta }_2$ are computed as follows:

$${\theta }_2={\theta }_{23}-{\theta }_3$$

(38)

The entire left side of (34) is known. If we equate the elements on row 1, column 3 and row 3, column 3 from both sides of (34), we can write:

$$\begin{gathered} {\mathrm{r}}_{13}{\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}+{\mathrm{r}}_{23}{\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}-{\mathrm{r}}_{33}{\mathrm{s}\theta }_{23}=-{\mathrm{c}\theta }_4{\mathrm{s}\theta }_5 \\ -{\mathrm{r}}_{13}{\mathrm{s}\theta }_1+{\mathrm{r}}_{23}{\mathrm{c}\theta }_1={\mathrm{s}\theta }_4{\mathrm{s}\theta }_5 \end{gathered}$$

(39)

As long as ${\mathrm{s}\theta }_5\ne 0$, the solution for ${\theta }_4$ is

$${\theta }_4=\mathrm{Atan}2\left(-{\mathrm{r}}_{13}{\mathrm{s}\theta }_1+{\mathrm{r}}_{23}{\mathrm{c}\theta }_1,{\mathrm{r}}_{13}{\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}+{\mathrm{r}}_{23}{\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}-{\mathrm{r}}_{33}{\mathrm{s}\theta }_{23}\right)$$

(40)

When ${\theta }_5=0$, the manipulator is in a configuration in which axes 6 and 4 line up and cause the same motion of the last link of the robot. In this case, all that can be solved for is the sum or difference of ${\theta }_4$ and ${\theta }_6$. This situation is detected by checking whether both arguments of the Equation (40) are near to zero. If so, ${\theta }_4$ is chosen arbitrarily, and when ${\theta }_6$ is computed later, it will be computed accordingly.

If we consider (17), we can rewrite it so that the entire left-hand side is a function of only knowns and ${\theta }_4$, rewriting it as follows [29]:

$${\left[{}_4{}^0\mathrm{T}\left({\theta }_4\right)\right]}^{-1}\left[\begin{array}{ccc}{\mathrm{r}}_{11}& {\mathrm{r}}_{12}& \begin{array}{cc}{\mathrm{r}}_{13}& {\mathrm{p}}_{\mathrm{x}}\end{array}\\ {\mathrm{r}}_{21}& {\mathrm{r}}_{22}& \begin{array}{cc}{\mathrm{r}}_{23}& {\mathrm{p}}_{\mathrm{y}}\end{array}\\ \begin{array}{c}{\mathrm{r}}_{31}\\ 0\end{array}& \begin{array}{c}{\mathrm{r}}_{32}\\ 0\end{array}& \begin{array}{c}\begin{array}{cc}{\mathrm{r}}_{33}& {\mathrm{p}}_{\mathrm{z}}\end{array}\\ \begin{array}{cc}0& 1\end{array}\end{array}\end{array}\right]={}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)$$

(41)

$$\begin{gathered} {\left[{}_4{}^0\mathrm{T}\left({\theta }_4\right)\right]}^{-1}= \\ =\left[\begin{array}{cccc}{\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{c}\theta }_4+{\mathrm{s}\theta }_1{\mathrm{s}\theta }_4& {\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{c}\theta }_4-{\mathrm{c}\theta }_1{\mathrm{s}\theta }_4& -{\mathrm{s}\theta }_{23}{\mathrm{c}\theta }_4& 0.0133{\mathrm{s}\theta }_4-0.41{\mathrm{c}\theta }_3{\mathrm{c}\theta }_4\\ -{\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{s}\theta }_4+{\mathrm{s}\theta }_1{\mathrm{c}\theta }_4& -{\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{s}\theta }_4-{\mathrm{c}\theta }_1{\mathrm{c}\theta }_4& {\mathrm{s}\theta }_{23}{\mathrm{s}\theta }_4& 0.0133{\mathrm{c}\theta }_4+0.41{\mathrm{c}\theta }_3{\mathrm{s}\theta }_4\\ -{\mathrm{c}\theta }_1{\mathrm{s}\theta }_{23}& -{\mathrm{s}\theta }_1{\mathrm{s}\theta }_{23}& -{\mathrm{c}\theta }_{23}& 0.41{\mathrm{s}\theta }_3-{\mathrm{D}}_3-{\mathrm{D}}_4\\ 0& 0& 0& 1\end{array}\right] \end{gathered}$$

(42)

$${}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)=\left[\begin{array}{cccc}{\mathrm{c}\theta }_5{\mathrm{c}\theta }_6& -{\mathrm{c}\theta }_5{\mathrm{s}\theta }_6& -{\mathrm{s}\theta }_5& 0\\ {\mathrm{s}\theta }_6& {\mathrm{c}\theta }_6& 0& 0\\ {\mathrm{c}\theta }_6{\mathrm{s}\theta }_5& -{\mathrm{s}\theta }_5{\mathrm{s}\theta }_6& {\mathrm{c}\theta }_5& 0\\ 0& 0& 0& 1\end{array}\right]$$

(43)

If we equate the elements on row 1, column 3 and row 3, column 3 from the both sides of (42), we obtain

$$\begin{gathered} {\mathrm{r}}_{13}({\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{c}\theta }_4+{\mathrm{r}}_{23}\left({\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}-{\mathrm{c}\theta }_1{\mathrm{s}\theta }_4\right)-{\mathrm{r}}_{33}{\mathrm{s}\theta }_{23}{\mathrm{c}\theta }_4=-{\mathrm{s}\theta }_5 \\ -{\mathrm{r}}_{13}{\mathrm{c}\theta }_1{\mathrm{s}\theta }_{23}-{\mathrm{r}}_{23}{\mathrm{s}\theta }_1{\mathrm{s}\theta }_{23}-{\mathrm{r}}_{33}{\mathrm{c}\theta }_{23}={\mathrm{c}\theta }_5 \end{gathered}$$

(44)

We solve for the desired value of ${\theta }_5$ as follows:

$${\theta }_5=\mathrm{Atan}2\left({\mathrm{s}\theta }_5,{\mathrm{c}\theta }_5\right)$$

(45)

Applying the same method one more time, we compute ${}_5{}^0\mathrm{T}^{-1}$ and write (17) in the following form:

$${\left[{}_5{}^0\mathrm{T}\left({\theta }_5\right)\right]}^{-1}\left[\begin{array}{ccc}{\mathrm{r}}_{11}& {\mathrm{r}}_{12}& \begin{array}{cc}{\mathrm{r}}_{13}& {\mathrm{p}}_{\mathrm{x}}\end{array}\\ {\mathrm{r}}_{21}& {\mathrm{r}}_{22}& \begin{array}{cc}{\mathrm{r}}_{23}& {\mathrm{p}}_{\mathrm{y}}\end{array}\\ \begin{array}{c}{\mathrm{r}}_{31}\\ 0\end{array}& \begin{array}{c}{\mathrm{r}}_{32}\\ 0\end{array}& \begin{array}{c}\begin{array}{cc}{\mathrm{r}}_{33}& {\mathrm{p}}_{\mathrm{z}}\end{array}\\ \begin{array}{cc}0& 1\end{array}\end{array}\end{array}\right]={}_6{}^5\mathrm{T}\left({\theta }_6\right)$$

(46)

If we equate the elements on row 3, column 1 and row 1, column 1 from the both sides of (42), we can write

$$\begin{gathered} -{\mathrm{r}}_{11}\left({\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{s}\theta }_4-{\mathrm{s}\theta }_1{\mathrm{c}\theta }_4\right)-{\mathrm{r}}_{21}\left({\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{s}\theta }_4+{\mathrm{c}\theta }_1{\mathrm{c}\theta }_4\right)+{\mathrm{r}}_{31}{\mathrm{s}\theta }_{23}{\mathrm{s}\theta }_4={\mathrm{s}\theta }_6 \\ {\mathrm{r}}_{11}\left[\left({\mathrm{c}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{c}\theta }_4+{\mathrm{s}\theta }_1{\mathrm{s}\theta }_4\right){\mathrm{c}\theta }_5-{\mathrm{c}\theta }_1{\mathrm{s}\theta }_{23}{\mathrm{s}\theta }_5\right]+ \\ +{\mathrm{r}}_{21}\left[\left({\mathrm{s}\theta }_1{\mathrm{c}\theta }_{23}{\mathrm{c}\theta }_4-{\mathrm{c}\theta }_1{\mathrm{s}\theta }_4\right){\mathrm{c}\theta }_5-{\mathrm{s}\theta }_1{\mathrm{s}\theta }_{23}{\mathrm{s}\theta }_5\right]- \\ -{\mathrm{r}}_{31}\left({\mathrm{s}\theta }_{23}{\mathrm{c}\theta }_4{\mathrm{c}\theta }_5+{\mathrm{c}\theta }_{23}{\mathrm{s}\theta }_5\right)={\mathrm{c}\theta }_6 \end{gathered}$$

(47)

$${\theta }_6=\mathrm{Atan}2\left({\mathrm{s}\theta }_6,{\mathrm{c}\theta }_6\right)$$

(48)

Because of the plus or minus signs appearing in (27) and (32), these equations compute four solutions. Additionally, there are four more solutions obtained by the wrist of the manipulator. For each of the four solutions computed above, we obtain the flipped solution by [29]

$$\begin{gathered} {\theta }_4^{\prime }={\theta }_4+{180}^{\mathrm{o}} \\ {\theta }_5^{\prime }=-{\theta }_5 \\ {\theta }_6^{\prime }={\theta }_6+{180}^{\mathrm{o}} \end{gathered}$$

(49)

2.4. The Designed Mobile Manipulator’s Common Chain of Frames

To achieve a successful combination between the KUKA youBot mobile platform and the Kinova Jaco Gen 2 robot manipulator, we have to describe the position of the end-effector according the local frame, attached to the center of the mobile chassis (platform). In order to deploy the mathematical equations, we have to describe the connection between the Kinova manipulator’s base frame and the KUKA mobile platform frame using the classical Denavit–Hartenberg convention (18). The aim is to derive a homogeneous transformation ${}_{\mathrm{manip}}{}^{\mathrm{mobP}}\mathrm{T}$ between these two frames; the transformation will be sized as a $4\times 4$ matrix, and the matrix will include the rotations between frame ${\mathrm{R}}_{\mathrm{manip}}^{\mathrm{mobP}}\left(\mathrm{t}\right)$ and vector ${\mathrm{o}}_{\mathrm{manip}}^{\mathrm{mobP}}\left(\mathrm{t}\right)$ that describes the displacement between the origins of these two frames. The origin of the Kinova manipulator base frame is in negative direction according the $x$ axis of the mobile platform, a fact that causes a negative sign to be attached to the Denavit–Hartenberg parameter $\mathrm{a}$. The distance between the origins is denoted as ${\mathrm{h}}_2$.

$${}_{\mathrm{manip}}{}^{\mathrm{mobP}}\mathrm{T}=\left[\begin{array}{cc}{\mathrm{R}}_{\mathrm{manip}}^{\mathrm{mobP}}\left(\mathrm{t}\right)& {\mathrm{o}}_{\mathrm{manip}}^{\mathrm{mobP}}\left(\mathrm{t}\right)\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& 0& -1& -{\mathrm{l}}_2\\ -1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(50)

To calculate the position and the orientation of the end-effector, we have to provide the product of seven homogeneous transformations:

$${}_6{}^{\mathrm{mobP}}\mathrm{T}={}_{\mathrm{manip}}{}^{\mathrm{mobP}}\mathrm{T}{}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right){}_4{}^3\mathrm{T}\left({\theta }_4\right){}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)$$

(51)

The final result is shown in (Figure 6).

Figure 6. Position of the end-effector according to the mobile platform frame. The manipulator is placed in home position.

2.5. Jacobian

Since the Kinova Jaco Gen 2 manipulator is mounted upon the mobile platform KUKA youBot, in order to be able to maintain the stable position of the manipulator’s end-effector, while the platform is moving within its 2D workspace, we have to calculate the Jacobian matrix to determine the instantaneous velocity of the gripper along the Cartesian $y$ axis ($y$ axis of the base coordinate system). The manipulator’s $y$ axis coincides with the mobile platform’s $x$ axis. The goal that we imposed for our experiments is even simplified: to keep the position of the gripper stable, according to the inertial coordinate system of the environment, while the mobile platform is moving along a straight line, its $x$ axis, for example. To achieve the purpose, we set the velocity of the youBot mobile platform according to the velocity of the gripper, using the opposite direction. The joint velocities are accessible through the messages, provided by the ROS stack, a concatenated set of ROS packages. The calculation of the Jacobi matrix is an essential mathematical element for the successful conduct of the experiments.

2.5.1. Linear Velocity

Consider frame $\left\{\mathrm{B}\right\}$ to be attached to a rigid body. We must describe the motion of point ${}^{\mathrm{B}}\mathrm{Q}$ relative to the frame $\left\{\mathrm{A}\right\}$, where we consider $\left\{\mathrm{A}\right\}$ to be fixed. Frame $\left\{\mathrm{B}\right\}$ is located relative to frame $\left\{\mathrm{A}\right\}$, the position vector is ${}^{\mathrm{A}}\mathrm{P}_{\mathrm{BORG}}$, the rotation matrix is ${}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}$, and we will assume that the orientation of ${}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}$ is constant with time, that is, the motion of point $\mathrm{Q}$ relative to $\left\{\mathrm{A}\right\}$ is due to ${}^{\mathrm{A}}\mathrm{P}_{\mathrm{BORG}}$ [29]. Equation (52) solves the linear velocity of point $\mathrm{Q}$ in terms of $\left\{\mathrm{A}\right\}$. We have to express both components of the velocity in terms of $\left\{\mathrm{A}\right\}$ and sum them, keeping the relative orientation between these two frames constant as follows:

$${}^{\mathrm{A}}\mathrm{V}_{\mathrm{Q}}={}^{\mathrm{A}}\mathrm{V}_{\mathrm{BORG}}+{}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}{}^{\mathrm{B}}\mathrm{V}_{\mathrm{Q}}$$

(52)

2.5.2. Rotational Velocity

If the orientation between two frames is changing in time, the point that is fixed in frame $\left\{\mathrm{B}\right\}$ is indicated by vector ${}^{\mathrm{B}}\mathrm{Q}$. The rotational velocity of the frame $\left\{\mathrm{B}\right\}$ relative to frame $\left\{\mathrm{A}\right\}$ is expressed by a vector ${}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}$. We will consider that the linear velocity vector ${}^{\mathrm{B}}\mathrm{V}_{\mathrm{Q}}$ is constant equal to zero. Point $\mathrm{Q}$ will have only rotational velocity ${}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}$ according frame $\left\{\mathrm{A}\right\}$. We can observe both the magnitude and the direction of the change in vector ${}^{\mathrm{B}}\mathrm{Q}$, as viewed from $\left\{\mathrm{A}\right\}$. The differential change in ${}^{\mathrm{A}}\mathrm{Q}$ is perpendicular to ${}^{\mathrm{A}}\mathrm{Q}$ and ${}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}$. Additionally, it is observable that the magnitude of the differential change is

$$\left|\Delta \mathrm{Q}\right|=(\left|{}^{\mathrm{A}}\mathrm{Q}\right|\sin \theta \left(\left|{}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}\right|\Delta \mathrm{t}\right)$$

(53)

The vector cross-product is suggested by a defined condition of magnitude and direction. These conditions are satisfied by the following equation:

$${}^{\mathrm{A}}\mathrm{V}_{\mathrm{Q}}={}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}\times {}^{\mathrm{A}}\mathrm{Q}$$

(54)

Generally, the vector $\mathrm{Q}$ can also be changed according the frame $\left\{\mathrm{B}\right\}$. This concept is described by

$$\begin{gathered} {}^{\mathrm{A}}\mathrm{V}_{\mathrm{Q}}{=}^{\mathrm{A}}\left({}^{\mathrm{B}}\mathrm{V}_{\mathrm{Q}}\right)+{}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}\times {}^{\mathrm{A}}\mathrm{Q}={}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}{}^{\mathrm{B}}\mathrm{V}_{\mathrm{Q}}+{}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}\times {}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}{}^{\mathrm{B}}\mathrm{Q}= \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}={}^{\mathrm{A}}\mathrm{V}_{\mathrm{BORG}}+{}_{\mathrm{A}}{}^{\mathrm{B}}\mathrm{R}{}^{\mathrm{B}}\mathrm{V}_{\mathrm{Q}}+{}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}\times {}_{\mathrm{A}}{}^{\mathrm{B}}\mathrm{R}{}^{\mathrm{B}}\mathrm{Q} \end{gathered}$$

(55)

When the geometric approach is described, however, to investigate more precisely, we must describe also the mathematical approach.

To derive the relationship between the derivative of an orthonormal matrix and a certain skew-symmetric matrix, we will use $\mathrm{n}\times \mathrm{n}$ orthonormal matrix $\mathrm{R}$:

$${\mathrm{RR}}^{\mathrm{T}}={\mathrm{I}}_{\mathrm{n}}$$

(56)

the matrix ${\mathrm{I}}_{\mathrm{n}}$ is an $\mathrm{n}\times \mathrm{n}$ identity matrix. Differenting (56), we derive

$${\dot{\mathrm{R}}\mathrm{R}}^{\mathrm{T}}+{\mathrm{R}\dot{\mathrm{R}}}^{\mathrm{T}}=0_{\mathrm{n}}$$

(57)

The matrix $0_{\mathrm{n}}$ is an $\mathrm{n}\times \mathrm{n}$ zero matrix; thus, (57) can be written as

$${\dot{\mathrm{R}}\mathrm{R}}^{\mathrm{T}}+{\left({\dot{\mathrm{R}}\mathrm{R}}^{\mathrm{T}}\right)}^{\mathrm{T}}=\mathrm{S}+{\mathrm{S}}^{\mathrm{T}}=0_{\mathrm{n}},$$

(58)

where $\mathrm{S}$ is a skew-symmetric matrix. The relationship between the derivative of orthonormal matrices and skew-symmetric matrices is

$$\mathrm{S}={\dot{\mathrm{R}}\mathrm{R}}^{-1}$$

(59)

Let us consider a vector ${}^{\mathrm{B}}\mathrm{P}$ that is fixed in frame $\left\{\mathrm{B}\right\}$. Its description in another frame $\left\{\mathrm{A}\right\}$ is

$${}^{\mathrm{A}}\mathrm{P}={}_{\mathrm{A}}{}^{\mathrm{B}}\mathrm{R}{}^{\mathrm{B}}\mathrm{P}$$

(60)

If frame $\left\{\mathrm{B}\right\}$ is rotating, it means that the derivative ${}_{\mathrm{A}}{}^{\mathrm{B}}\dot{\mathrm{R}}$ is not a zero, so the resulting vector ${}^{\mathrm{A}}\mathrm{P}$ will also be changing:

$${}^{\mathrm{A}}\dot{\mathrm{P}}={}_{\mathrm{A}}{}^{\mathrm{B}}\dot{\mathrm{R}}{}^{\mathrm{B}}\mathrm{P}$$

(61)

Using the notation for velocity that it will give and (59), we have:

$${}^{\mathrm{A}}\mathrm{V}_{\mathrm{P}}={}_{\mathrm{A}}{}^{\mathrm{B}}\dot{\mathrm{R}}{}^{\mathrm{B}}\mathrm{P}={}_{\mathrm{B}}{}^{\mathrm{A}}\dot{\mathrm{R}}{}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}_{-1}{}^{\mathrm{A}}\mathrm{P}={}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{S}{}^{\mathrm{A}}\mathrm{P}$$

(62)

The skew-symmetric matrix in (59) is called the angular-velocity matrix. We assign the elements in a skew-symmetric matrix $\mathrm{S}$ as follows:

$$\mathrm{S}=\left[\begin{array}{ccc}0& -{\mathsf{\Omega }}_{\mathrm{x}}& {\mathsf{\Omega }}_{\mathrm{y}}\\ {\mathsf{\Omega }}_{\mathrm{x}}& 0& -{\mathsf{\Omega }}_{\mathrm{x}}\\ -{\mathsf{\Omega }}_{\mathrm{y}}& {\mathsf{\Omega }}_{\mathrm{x}}& 0\end{array}\right]$$

(63)

The column vector $\mathsf{\Omega }$, which corresponds to the $3\times 3$ angular-velocity matrix, is an angular-velocity vector:

$$\mathsf{\Omega }=\left[\begin{array}{c}{\mathsf{\Omega }}_{\mathrm{x}}\\ {\mathsf{\Omega }}_{\mathrm{y}}\\ {\mathsf{\Omega }}_{\mathrm{z}}\end{array}\right]$$

(64)

Applying vector cross-product gives

$$\mathrm{SP}=\mathsf{\Omega }\times \mathrm{P}$$

(65)

where $\mathrm{P}$ is any vector.

Hence, (59) can be written as follows:

$${}^{\mathrm{A}}\mathrm{V}_{\mathrm{P}}={}^{\mathrm{A}}\mathsf{\Omega }_{\mathrm{B}}\times {}^{\mathrm{A}}\mathrm{P}$$

(66)

where the notation for $\mathsf{\Omega }$ indicates that it is the angular-velocity vector specifying the motion of the frame $\left\{\mathrm{B}\right\}$ with respect to frame $\left\{\mathrm{A}\right\}$ [29].

2.5.3. Motion of the Robot’s Links

Notations ${\mathrm{v}}_{\mathrm{i}}$ and ${\mathsf{\omega }}_{\mathrm{i}}$ are the linear and angular velocities of the origin of link frame $\left\{\mathrm{i}\right\}$. The robot’s structure is a chain of connected links, where each one is capable of motion relative to its neighbors. To calculate the velocities of each link, the starting point is the robot’s base. The velocities of link $\mathrm{i}+1$ will be calculated according to the velocity of link $\mathrm{i}$ plus new velocity components added by link $\mathrm{i}+1$. We have to divide the velocity into two subparts: the linear velocity of the origin of the link frame and the rotational velocity of the link.

Rotational velocities can be added when both $\mathsf{\omega }$ vectors are written with respect to the same frame. Therefore, the angular velocity of link $\mathrm{i}+1$ is the same as that of link $\mathrm{i}$ plus a new component caused by rotational velocity at joint $\mathrm{i}+1$. This statement can be written according to frame $\left\{\mathrm{i}\right\}$ as follows:

$${}^{\mathrm{i}}\mathsf{\omega }_{\mathrm{i}+1}={}^{\mathrm{i}}\mathsf{\omega }_{\mathrm{i}}+{}_{\mathrm{i}+1}{}^{\mathrm{i}}\mathrm{R}{\dot{\theta }}_{\mathrm{i}+1}{}^{\mathrm{i}+1}\hat{\mathrm{Z}}_{\mathrm{i}+1}$$

(67)

Also

$${\dot{\theta }}_{\mathrm{i}+1}{}^{\mathrm{i}+1}\hat{\mathrm{Z}}_{\mathrm{i}+1}={}^{\mathrm{i}+1}\left[\begin{array}{c}0\\ 0\\ {\dot{\theta }}_{\mathrm{i}+1}\end{array}\right]$$

(68)

In order to represent the added rotational component due to motion at the joint in the frame $\left\{\mathrm{i}+1\right\}$, we have made use of the rotational matrix related to frames $\left\{\mathrm{i}\right\}$ and $\left\{\mathrm{i}+1\right\}$. The rotational matrix describes the rotation of joint $\mathrm{i}+1$ using its description in frame $\left\{\mathrm{i}\right\}$, and we can add the two components of angular velocities [29]. We can find the description of the angular velocity of link $\mathrm{i}+1$ according to frame $\left\{\mathrm{i}+1\right\}$ using:

$${}^{\mathrm{i}+1}\mathsf{\omega }_{\mathrm{i}+1}={}_{\mathrm{i}}{}^{\mathrm{i}+1}\mathrm{R}{}^{\mathrm{i}}\mathsf{\omega }_{\mathrm{i}}+{\dot{\theta }}_{\mathrm{i}+1}{}^{\mathrm{i}+1}\hat{\mathrm{Z}}_{\mathrm{i}+1}$$

(69)

The linear velocity of the origin of the frame $\left\{\mathrm{i}+1\right\}$ is equal to the velocity of frame $\left\{\mathrm{i}\right\}$ plus a new component caused by the rotation of link $\mathrm{i}$. It is described by the last part of (55), in frame $\left\{\mathrm{i}\right\}$. One term vanishes because ${}^{\mathrm{i}}\mathrm{P}_{\mathrm{i}+1}$ is constant. We have

$${}^{\mathrm{i}}\mathrm{v}_{\mathrm{i}+1}={}^{\mathrm{i}}\mathrm{v}_{\mathrm{i}}+{}^{\mathrm{i}}\mathsf{\omega }_{\mathrm{i}}\times {}^{\mathrm{i}}\mathrm{P}_{\mathrm{i}+1}$$

(70)

Premultiplying both sides by ${}_{\mathrm{i}}{}^{\mathrm{i}+1}\mathrm{R}$, we compute

$${}^{\mathrm{i}+1}\mathrm{v}_{\mathrm{i}+1}={}_{\mathrm{i}}{}^{\mathrm{i}+1}\mathrm{R}\left({}^{\mathrm{i}}\mathrm{v}_{\mathrm{i}}+{}^{\mathrm{i}}\mathsf{\omega }_{\mathrm{i}}\times {}^{\mathrm{i}}\mathrm{P}_{\mathrm{i}+1}\right)$$

(71)

Equations (69) and (71) are important results related to revolute joints. The corresponding relationships for the case when joint $\mathrm{i}+1$ is prismatic are

$$\begin{gathered} {}^{\mathrm{i}+1}\mathrm{v}_{\mathrm{i}+1}={}_{\mathrm{i}}{}^{\mathrm{i}+1}\mathrm{R}\left({}^{\mathrm{i}}\mathrm{v}_{\mathrm{i}}+{}^{\mathrm{i}}\mathsf{\omega }_{\mathrm{i}}\times {}^{\mathrm{i}}\mathrm{P}_{\mathrm{i}+1}\right)+{\dot{\mathrm{d}}}_{\mathrm{i}+1}{}^{\mathrm{i}+1}\hat{\mathrm{Z}}_{\mathrm{i}+1} \\ {}^{\mathrm{i}+1}\mathsf{\omega }_{\mathrm{i}+1}={}_{\mathrm{i}}{}^{\mathrm{i}+1}\mathrm{R}{}^{\mathrm{i}}\mathsf{\omega }_{\mathrm{i}} \end{gathered}$$

(72)

Applying Equation (72) or (69) and (71) successively from link to link, we can compute ${}^{\mathrm{N}}\mathsf{\omega }_{\mathrm{N}}$ and ${}^{\mathrm{N}}\mathrm{v}_{\mathrm{N}}$.

The multidimensional form of the derivatives is known as Jacobian. If we have six functions and the number of the independent parameters is also six, we have [29]

$$\begin{gathered} {\mathrm{y}}_1={\mathrm{f}}_1\left({\mathrm{x}}_1,{\mathrm{x}}_2,{\mathrm{x}}_3,{\mathrm{x}}_4,{\mathrm{x}}_5,{\mathrm{x}}_6\right) \\ {\mathrm{y}}_2={\mathrm{f}}_2\left({\mathrm{x}}_1,{\mathrm{x}}_2,{\mathrm{x}}_3,{\mathrm{x}}_4,{\mathrm{x}}_5,{\mathrm{x}}_6\right) \\ ⋮ \\ {\mathrm{y}}_6={\mathrm{f}}_6\left({\mathrm{x}}_1,{\mathrm{x}}_2,{\mathrm{x}}_3,{\mathrm{x}}_4,{\mathrm{x}}_5,{\mathrm{x}}_6\right) \end{gathered}$$

(73)

Using a vector notation (72) can be written as

$$\mathrm{Y}=\mathrm{F}\left(\mathrm{X}\right)$$

(74)

We can use the chain rule to calculate ${\mathsf{\delta }\mathrm{y}}_{\mathrm{i}}$ as follows [29]:

$$\begin{gathered} {\mathsf{\delta }\mathrm{y}}_1=\frac{\partial {\mathrm{f}}_1}{\partial {\mathrm{x}}_1}{\mathsf{\delta }\mathrm{x}}_1+\frac{\partial {\mathrm{f}}_1}{\partial {\mathrm{x}}_2}{\mathsf{\delta }\mathrm{x}}_2+\cdots +\frac{\partial {\mathrm{f}}_1}{\partial {\mathrm{x}}_6}{\mathsf{\delta }\mathrm{x}}_6 \\ {\mathsf{\delta }\mathrm{y}}_2=\frac{\partial {\mathrm{f}}_2}{\partial {\mathrm{x}}_1}{\mathsf{\delta }\mathrm{x}}_1+\frac{\partial {\mathrm{f}}_2}{\partial {\mathrm{x}}_2}{\mathsf{\delta }\mathrm{x}}_2+\cdots +\frac{\partial {\mathrm{f}}_2}{\partial {\mathrm{x}}_6}{\mathsf{\delta }\mathrm{x}}_6 \\ ⋮ \\ {\mathsf{\delta }\mathrm{y}}_6=\frac{\partial {\mathrm{f}}_6}{\partial {\mathrm{x}}_1}{\mathsf{\delta }\mathrm{x}}_1+\frac{\partial {\mathrm{f}}_6}{\partial {\mathrm{x}}_2}{\mathsf{\delta }\mathrm{x}}_2+\cdots +\frac{\partial {\mathrm{f}}_6}{\partial {\mathrm{x}}_6}{\mathsf{\delta }\mathrm{x}}_6 \end{gathered}$$

(75)

In vector notation, (76) is

$$\mathsf{\delta }\mathrm{Y}=\frac{\partial \mathrm{F}}{\partial \mathrm{X}}\mathsf{\delta }\mathrm{X}$$

(76)

The Jacobian is a $6\times 6$ matrix. It contains partial derivatives, and the notion of the matrix is $\mathrm{J}$. If the functions ${\mathrm{f}}_1\left(\mathrm{X}\right)$ through ${\mathrm{f}}_6\left(\mathrm{X}\right)$ are nonlinear, then the partial derivatives are functions of ${\mathrm{x}}_{\mathrm{i}}$, so we can use the following notation:

$$\mathsf{\delta }\mathrm{Y}=\mathrm{J}\left(\mathrm{X}\right)\mathsf{\delta }\mathrm{X}$$

(77)

The Jacobian can be presented as mapping velocities in $\mathrm{X}$ to $\mathrm{Y}$

$$\dot{\mathrm{Y}}=\mathrm{J}\left(\mathrm{X}\right)\dot{\mathrm{X}}$$

(78)

At each new instance, $\mathrm{X}$ has changed, which means that the linear transformation $\mathrm{J}\left(\mathrm{X}\right)$ has also changing. In robotics, the general purpose of Jacobian is that it relates joint velocities to the Cartesian velocities of the tip of the arm [29]

$${}^0\mathcal{v}={}^0\mathrm{J}(\Theta )\dot{\Theta }$$

(79)

where the vector of joint displacements is $\Theta$, and the Cartesian velocities are represented by the vector $\mathcal{v}$. The superscript in front of the vector notations indicates in which frame the resulting Cartesian velocities are expressed. The shape of the vector $\dot{\Theta }$ is $6\times 1$ and the vector ${}^0\mathcal{v}$ has the shape $6\times 1$. The vector ${}^0\mathcal{v}$ is split into two parts:

$${}^0\mathcal{v}=\left[\begin{array}{c}{}^0\mathrm{v}\\ {}^0\mathsf{\omega }\end{array}\right]$$

(80)

The first three elements of ${}^0\mathcal{v}$ represent linear velocity, and the second three elements represent angular velocity. The Jacobian has a number of rows equal to the degrees of freedom in the Cartesian space, with columns representing the number of joints of the manipulator. The Jacobian can be found by directly differentiating the kinematic equations of the manipulator. This approach is easily implemented for linear velocities, but there is no $3\times 1$ orientation vector whose derivative is $\mathsf{\omega }$. That is the reason why we introduced Equations (69) and (71).

When the Jacobian is nonsingular, we can calculate joint velocities from the given Cartesian velocities:

$$\dot{\Theta }={\mathrm{J}}^{-1}(\Theta ){}^0\mathcal{v}$$

(81)

2.6. Maintaining the Constant Position of the Manipulator’s Gripper While the Mobile Platform Is Moving

The position and orientation of the working tool of Kinova Jaco Gen2 are calculated by multiplying homogeneous transformations from (82) to (87). These transformations are derived using a classical Denavit–Hartenberg convention, as is presented in (18):

$${}_1{}^0\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_1& 0& {\mathrm{s}\theta }_1& 0\\ -{\mathrm{s}\theta }_1& 0& {\mathrm{c}\theta }_1& 0\\ 0& -1& 0& {\mathrm{D}}_1\\ 0& 0& 0& 1\end{array}\right]$$

(82)

$${}_2{}^1\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_2& {\mathrm{s}\theta }_2& 0& {\mathrm{D}}_2{\mathrm{c}\theta }_2\\ {\mathrm{s}\theta }_2& {\mathrm{c}\theta }_2& 0& {\mathrm{D}}_2{\mathrm{c}\theta }_2\\ 0& 0& -1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(83)

$${}_3{}^2\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_3& 0& {\mathrm{s}\theta }_3& 0\\ {\mathrm{s}\theta }_3& 0& -{\mathrm{c}\theta }_3& 0\\ 0& 1& 0& -{\mathrm{e}}_2\\ 0& 0& 0& 1\end{array}\right]$$

(84)

$${}_4{}^3\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_4& 0& {\mathrm{s}\theta }_4& 0\\ {\mathrm{s}\theta }_4& 0& -{\mathrm{c}\theta }_4& 0\\ 0& 1& 0& -\left({\mathrm{D}}_3+{\mathrm{D}}_4\right)\\ 0& 0& 0& 1\end{array}\right]$$

(85)

$${}_5{}^4\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_5& 0& {\mathrm{s}\theta }_5& 0\\ {\mathrm{s}\theta }_5& 0& -{\mathrm{c}\theta }_5& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(86)

$${}_6{}^5\mathrm{T}=\left[\begin{array}{cccc}{\mathrm{c}\theta }_6& {\mathrm{s}\theta }_6& 0& 0\\ {\mathrm{s}\theta }_6& {\mathrm{s}\theta }_6& 0& 0\\ 0& 1& -1& -\left({\mathrm{D}}_5+{\mathrm{D}}_6\right)\\ 0& 0& 0& 1\end{array}\right]$$

(87)

The vector $\Theta$ contains all the angular positions of the joints:

$$\Theta ={\left[{\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4,{\theta }_5,{\theta }_6\right]}^{\mathrm{T}}$$

(88)

The values of $\Theta$ are instantaneous. Since the investigated mobile manipulator has only rotational joints, we can use (69) and (71) to calculate the Jacobian. The output will be a vector with velocities in the Cartesian space, while the input values are the velocities of the manipulator’s joints $\dot{\Theta }$. As the values of $\Theta$ and $\dot{\Theta }$ were previously saved, we will use them to calculate ${}^0\mathcal{v}$ for each instance. The calculation of the Jacobian is performed in steps; in each step, the algorithm calculates a column of the matrix $\mathrm{J}$.

$$\begin{gathered} {}_2{}^0\mathrm{R}={}_2{}^0\mathrm{T}\left(0:2,0:2\right)=\left[{}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right)\right]\left(0:2,0:2\right) \\ {}_3{}^0\mathrm{R}={}_3{}^0\mathrm{T}\left(0:2,0:2\right)=\left[{}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right)\right]\left(0:2,0:2\right) \\ {}_4{}^0\mathrm{R}={}_4{}^0\mathrm{T}\left(0:2,0:2\right)=\left[{}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right){}_4{}^3\mathrm{T}\left({\theta }_4\right)\right]\left(0:2,0:2\right) \\ {}_5{}^0\mathrm{R}={}_5{}^0\mathrm{T}\left(0:2,0:2\right)=\left[{}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right){}_4{}^3\mathrm{T}\left({\theta }_4\right){}_5{}^4\mathrm{T}\left({\theta }_5\right)\right]\left(0:2,0:2\right) \\ {}_6{}^0\mathrm{R}={}_6{}^0\mathrm{T}\left(0:2,0:2\right)=\left[{}_1{}^0\mathrm{T}\left({\theta }_1\right){}_2{}^1\mathrm{T}\left({\theta }_2\right){}_3{}^2\mathrm{T}\left({\theta }_3\right){}_4{}^3\mathrm{T}\left({\theta }_4\right){}_5{}^4\mathrm{T}\left({\theta }_5\right){}_6{}^5\mathrm{T}\left({\theta }_6\right)\right]\left(0:2,0:2\right) \end{gathered}$$

(89)

Using (86), we can calculate

$$\mathrm{J}\left(0:5,0\right)=\left[\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\times \left({}_6{}^0\mathrm{T}\left(0:3,3\right)-\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]\right)\\ \left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}\right]$$

(90)

$$\mathrm{J}\left(0:5,1\right)=\left[\begin{array}{c}{}_1{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\times \left({}_6{}^0\mathrm{T}\left(0:3,3\right)-{}_1{}^0\mathrm{T}\left(0:3,3\right)\right)\\ {}_1{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}\right]$$

(91)

$$\mathrm{J}\left(0:5,2\right)=\left[\begin{array}{c}{}_2{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\times \left({}_6{}^0\mathrm{T}\left(0:3,3\right)-{}_2{}^0\mathrm{T}\left(0:3,3\right)\right)\\ {}_2{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}\right]$$

(92)

$$\mathrm{J}\left(0:5,3\right)=\left[\begin{array}{c}{}_3{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\times \left({}_6{}^0\mathrm{T}\left(0:3,3\right)-{}_3{}^0\mathrm{T}\left(0:3,3\right)\right)\\ {}_3{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}\right]$$

(93)

$$\mathrm{J}\left(0:5,4\right)=\left[\begin{array}{c}{}_4{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\times \left({}_6{}^0\mathrm{T}\left(0:3,3\right)-{}_4{}^0\mathrm{T}\left(0:3,3\right)\right)\\ {}_4{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}\right]$$

(94)

$$\mathrm{J}\left(0:5,5\right)=\left[\begin{array}{c}{}_5{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\times \left({}_6{}^0\mathrm{T}\left(0:3,3\right)-{}_5{}^0\mathrm{T}\left(0:3,3\right)\right)\\ {}_5{}^0\mathrm{R}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}\right]$$

(95)

The final Jacobian is derived by combining Equations (90) to (95).

$$\mathrm{J}=\left[\mathrm{J}\left(0:5,0\right),\mathrm{J}\left(0:5,1\right),\mathrm{J}\left(0:5,2\right),\mathrm{J}\left(0:5,3\right),\mathrm{J}\left(0:5,4\right),\mathrm{J}\left(0:5,5\right)\right]$$

(96)

The velocities of the joints are represented by

$$\dot{\Theta }={\left[{\dot{\theta }}_1,{\dot{\theta }}_2,{\dot{\theta }}_3,{\dot{\theta }}_4,{\dot{\theta }}_5,{\dot{\theta }}_6\right]}^{\mathrm{T}}$$

(97)

The final Cartesian velocities are calculated using (96) and (97) by

$$\left[\begin{array}{c}{}^0\mathrm{v}\\ {}^0\mathsf{\omega }\end{array}\right]=\mathrm{J}(\Theta )\dot{\Theta }$$

(98)

If we have to calculate $\dot{\Theta }$, we use the inverse Jacobian

$${\mathrm{J}}^{-1}=\frac{1}{\det \left(\mathrm{J}\right)}\mathrm{adj}\left(\mathrm{J}\right)$$

(99)

$$\dot{\Theta }={\mathrm{J}}^{-1}(\Theta )\left[\begin{array}{c}{}^0\mathrm{v}\\ {}^0\mathsf{\omega }\end{array}\right]$$

(100)

3. Experimental Results

The proposed approach is validated by conducting a real-world experiment with the laboratory prototype of the proposed mobile manipulator. Both components of the introduced manipulator are ROS-supported. This gives a universal point of view regarding the implemented solutions, despite the differences in the architecture of the mobile manipulator subsystems.

Using the message provided by the Kinova ROS stack:/’${kinova_robotType}_driver’/out/joint_state, we are able to observe the velocities and angular positions for each robot joint. The calculated movement speeds along the Kinova Jaco Gen 2′s Cartesian coordinate system’s x, y and z axes and angular velocities of ${\mathsf{\omega }}_{\mathrm{x}},{\mathsf{\omega }}_{\mathrm{y}},{\mathrm{and}\mathsf{\omega }}_{\mathrm{z}}$ are fed to KUKA youBot. The data recorded during the experiments are used to display the results. The velocities are measured in radians per second. Based on this, we structure a matrix that contains the joint velocities during the experiment. Using these measurements, we are able to determine the end-effector’s velocities along the Cartesian coordinate system’s axis and its angular velocities.

$$\left[\begin{array}{c}\dot{\mathrm{x}}\\ \dot{\mathrm{y}}\\ \dot{\mathrm{z}}\\ {\mathsf{\omega }}_{\mathrm{x}}\\ {\mathsf{\omega }}_{\mathrm{y}}\\ {\mathsf{\omega }}_{\mathrm{z}}\end{array}\right]={\mathrm{J}}_{6\times 6}{\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\\ {\dot{\theta }}_3\\ {\dot{\theta }}_4\\ {\dot{\theta }}_5\\ {\dot{\theta }}_6\end{array}\right]}_{6\times 1}$$

(101)

where ${\dot{\theta }}_1$, ${\dot{\theta }}_2$, ${\dot{\theta }}_3$, ${\dot{\theta }}_4$, ${\dot{\theta }}_5$ and ${\dot{\theta }}_6$ are joint velocities. Figure 7 shows the velocity ramp for each joint during the validation.

Figure 7. Joint velocities: (a) velocity of the first joint; (b) velocity of the second joint; (c) velocity of the third joint; (d) velocity of the fourth joint; (e) velocity of the fifth joint; (f) velocity of the sixth joint.

The positions along the axes and the orientations around them for the end-effector in the Cartesian coordinate system are shown in Figure 8.

Figure 8. Measured positions and orientations of the end-effector. There are 3 positions and 3 orientations according to the manipulator’s base (Cartesian) coordinate system: (a) position along the $x$ axis; (b) position along the $y$ axis; (c) position along the $z$ axis; (d) orientation around the $x$ axis—$\mathsf{\alpha }$; (e) orientation around the $y$ axis—$\mathsf{\beta }$; (f) orientation around the $z$ axis—$\mathsf{\gamma }$.

The values of the vector elements that represent the linear and angular velocities during the experiment are shown in Figure 9. It visualizes the results of applying formula (98). The velocities of the joints are applied to the earlier proposed algorithm. It calculates the vector that is given in $\left(98\right)$. The values inside the vector provide a stable position for the end-effector in the 3D space, despite the movable base.

Figure 9. Calculated velocities according to the manipulator’s base coordinate system, along each axis and around each axis. These velocities applied to the base provide constant position of the end-effector during its movement according to the adjusted joint velocities vector $\dot{\Theta }$: (a) velocity along the $x$ axis; (b) velocity along the $y$ axis; (c) velocity along the $z$ axis; (d) velocity around the $x$ axis; (e) velocity around the $y$ axis; (f) velocity around the $z$ axis.

The displacement between the positions of the end-effector of the Kinova Jaco Gen 2 manipulator and the KUKA youBot mobile platform during the experiment is presented in Figure 10.

Figure 10. Displacement between the position of the movable base coordinate system and the position of the end-effector.

According to the proposed approach, we calculated the manipulator’s velocity along the axes of its base frame to keep its end-effector ’s position constant during the movement of the base. The base is attached to the mobile platform KUKA youBot. To validate the proposed approach, we visualized manipulator’s position along its y axis during the movement of the mobile robot. The manipulator’s home position along y axis is around −0.25 m. The horizontality of the line shows that the velocities of the manipulator and mobile robot are approximately the same, slightly observable slope may be caused by not perfect environment conditions like the slippage of the mobile platform’s wheels.

Figure 11 shows the positions of the mobile platform and the manipulator gripper during the experiment. Additionally, the starting and the final positions of their movements are visible.

Figure 11. The positions of the mobile platform and manipulator gripper during the experiment.

4. Conclusions

We have proposed an affordable open-source mobile manipulator composed of the KUKA youBot mobile platform and a Kinova Jaco manipulator, and we have developed corresponding kinematic problems. The later have been verified by running experiments with the designed laboratory prototype. The proposed open-source character of the mobile manipulator and the initial software provided by the ROS community can help robot researchers to concentrate directly on their research topics. The latter can more easily complete challenging research and development tasks, which will accelerate the technology’s transfer and application.