Posture Optimization of the TIAGo Highly-Redundant Robot for Grasping Operation

Abstract

This study explores the optimization of the TIAGo robot’s configuration for grasping operation, with a focus on the context of aging. In fact, featuring a mobile base and a robotic arm, the TIAGo robot can conveniently aid individuals with disabilities, including those with motor and cognitive impairments in both domestic and clinical settings. Its capabilities include recognizing visual targets such as faces or gestures using stereo cameras, as well as interpreting vocal commands through acoustic sensors to execute tasks. For example, the robot can grasp and lift objects such as a glass of water and navigate autonomously in order to fulfill a request. The paper presents the position and differential kinematics that form the basis for using the robot in numerous application contexts. In the present case, they are used to evaluate the kinematic performance of the robot relative to an assigned pose in the search for the optimal configuration with respect to the higher-order infinite possible configurations. Ultimately, the article provides insight into how to effectively use the robot in gripping operations, as well as presenting kinematic models of the TIAGo robot.

1. Introduction

The aging population presents significant challenges in eldercare. As the world ages, there is a growing need for technology to support healthy aging, as Tian and Li [1] point out. Technologies like personal mobility aids, vehicle modifications, healthcare access technologies, electronic assistive devices, and smart home solutions are becoming vital for helping the elderly maintain their independence and quality of life [2]. Peek et al. [3] highlight that technology can help older adults stay independent and active, stressing the importance of considering psychological and environmental factors. In the field of robotics for eldercare, several studies have been performed. Krishnan and Pugazhenthi [4] reviewed various mobility and self-transfer systems as assistive devices for the elderly, focusing on improving their quality of life and independence through intelligent robotic systems. Broekens et al. [5] provided a comprehensive review of assistive social robots, which are designed for social interaction and can play a significant role in eldercare, especially in providing companionship and functioning as an interface with digital technology. Ienca et al. [6] discussed the integration of robotics in formal and informal dementia care, outlining the ethical, legal, and social implications that should be considered early in the development of assistive and social robots for dementia care. Coşar et al. [7] introduced the ENRICHME (Enabling Robot and assisted living environment for Independent Care and Health Monitoring of the Elderly) project’s robotic platform, which aims to enrich the day-to-day experience of elderly people with mild cognitive impairments by offering health monitoring, complementary care, and social support. Similar objectives are sought in the FOCAAL project, an acronym for FOg Computing in Ambient Assisted Living, funded by the Italian MIMIT, Ministry of Business and Made in Italy. The project is aimed at integrating artificial intelligence, assistive robotics, home automation, and sensors through the fog computing network architecture, which allows data to be processed locally, in the context of the patient’s life, and to implement personalized actions for different contexts and assistance scenarios, such as home care, rehabilitation clinics, and geriatric medical clinics. Although robots have the potential to assist in eldercare, their integration into this field is not without challenges. According to Remmers and Fischer’s research [8], care workers often perceive robots as mere tools, which can impede their widespread use in caregiving. Therefore, there is a challenge in convincing people to accept and appreciate the benefits of robots in this field. In order for robots to function effectively in eldercare, they must be designed to move and interact safely and efficiently in these settings. This requires a thorough examination of robotic movement, or kinematics, to ensure that robots can meet the diverse needs of elderly individuals without causing disruptions. Overcoming these challenges necessitates a collaborative effort from the fields of robotics, healthcare, and social science to ensure that robots can provide effective support for the elderly.

1.1. The Problem of an Aging Population in Europe

The aging of the population is due to two factors: increasing life expectancy and falling birth rates. On the one hand, life expectancy in the EU (European Union) fell slightly in 2021 due to the pandemic, but has generally increased, leading to an aging population. Huang et al. [9] analyzed the impact of COVID-19 on life expectancy and found a temporary decline in 2020 with a partial recovery in some countries by 2021. This aging trend is clear when comparing the population structure of Europe in 2007 and 2022, which shows a shift towards an older population [10]. On the other hand, the decline in birth rates lead to demographic aging. Figure 1b elucidates this trend by contrasting the population composition of Europe in 2007 with that of 2022, illustrating a demographic shift towards an older population. The hollow rectangles refer to data from 2007 and the filled rectangles to 2022.

1.2. TIAGo in the Literature

TIAGo, a versatile assistive robot by PAL Robotics, comes in various versions (see Figure 2) and is ideal for research and applications in healthcare, assistance, and light industry. It features advanced perception, autonomous navigation, object manipulation, and human–robot interaction (HRI), with a modular design for customization. Telembici et al. in [11] demonstrate how TIAGo can be integrated into everyday life based on its audio capabilities, achieving a high classification rate of audio events. Grama and Rusu enhanced TIAGo’s audio signal task handling and interaction, introducing three modules for better home environment interaction [12,13]. Lach et al. [14] present tactile-equipped end-effectors for TIAGo aimed at improving the reliability and success of mobile manipulation, contributing to its physical interaction capabilities. Muscar et al. [15] show how TIAGo can issue real-time warnings using its audio skills, identifying dangerous and helpful sounds and issuing alerts via email or SMS depending on the sound category identified. Miguel et al. [16] propose a fault-tolerant approach for service robots that uses a Robust Unknown-Input Observer (RUIO) and state feedback control strategy to compensate for fault effects and handle non-linearities.

Using VOSviewer 1.6.19 [17], a bibliometric tool, we analyzed TIAGo robot research through a keyword search of tiago and robot*, identifying 71 articles. These were visualized in three main clusters in VOSviewer, indicating research themes (see Figure 3):

• Red: Social Robotics and Human Interaction
• Blue: Mobile Robotics and Programming
• Green: Machine Learning and Robotic Services

The links between the various words demonstrate the co-occurrence between the keywords. The most-used words are robotics, robot programming, mobile robots, and services robot. There is no reference about kinematics. This visualization not only outlines the research landscape of TIAGo but also emphasizes the importance of each theme, with the size of the item representing its prominence in the field. Continuing, when the term kinema* was added to refine the search, we obtained only seven results on Scopus. Among these results, we found an absence of articles that explore the kinematics of TIAGo. Some works, such as that of Sulaiman et al. [18], where they integrate different robotic arms like the UR3 into the mobile platform, or the Emiko Franka Panda robotic manipulator by Baumgartner et al. [19], show a trend to study applications through performance analysis.

1.3. Summary

In the course of this literature review and subsequent analysis, the authors of this paper have found a scarcity of articles in the literature on the kinematics of the TIAGo robotic arm. The formal definition of its kinematics is considered useful to make the most of the features offered by such a robot, providing developers working on the ROS (robot operating system) platform and TIAGo robot drivers with detailed equations to improve its motion planning logic, for example in the case of obstacle avoidance problems [20], while also providing useful information for determining optimal postures.

The concept of pose optimization is extensively discussed in scientific circles. For instance, Cheah and Caverly’s research [21] focuses on the pose regulation of a six-degree-of-freedom cable-driven parallel robot (CDPR), analyzing it from a passivity perspective. Similarly, Gao et al.’s work [22] delves into dynamic Jacobian identification for robots operating in unstructured environments. Their studies illustrate how solving Jacobian identification as state estimation problems allows for the optimal estimation of the desired Jacobian, ensuring precise robotic alignment with the intended pose. Jens et al. [23] propose a kinematic redundancy in order to improve the achievable pose accuracy of a parallel robot’s traveling platform. Guojian et al. [24] propose a tightly coupled laser-inertial pose estimation and map building method that uses B-spline curves to represent a continuous-time trajectory and achieve high robustness of the registration steps. These are only some scientific papers that address this topic in different ways.

In this paper, the pose optimization is more related to investigating the manipulability ellipsoids in understanding a robot’s performance capabilities. This is pivotal not only for the TIAGo arm’s efficiency but also for enhancing its functionality across various tasks requiring precision and finesse. The examination of how the Jacobian matrix responds during grasping tasks is essential for determining the most effective methods for robots to interact with objects. This is particularly helpful in healthcare and assistance settings, where high dexterity and agility are demanded.

This article is structured as follows: Section 2 sets the context for this work, outlining the robot’s deployment in complex tasks, such as navigating through environments with obstacles and retrieving objects for assistance purposes. It illustrates the robot’s capability to interpret commands and plan its motion, highlighting the practical applications of autonomous robots in supporting daily activities. Section 3 focuses on the position kinematics of the TIAGo robot, exploring its optimal configurations for interacting with objects. Section 4 investigates the differential kinematics, detailing the relationship between the robot’s joint velocities and its end-effectors’ velocities. In Section 5, the routines used to optimize the robot posture are presented. Some results of their application are gathered in Section 6, where the optimal configuration that maximizes a kinematic index is discussed. This paper aims to contribute to the robotics field by providing the kinematics of a highly redundant mobile robot that shows a great potential in enhancing the quality of life for people with mobility challenges.

2. Advanced Tasks in an Unstructured Environment

Featuring a mobile base, namely, an Autonomous Mobile Robot (AMR), and a 7 dof robotic arm, the TIAGo robot has a redundant number of degrees of freedom that can be exploited in complex applications. Advanced tasks in supporting elderly people or people with motor impairments require movement of objects and/or equipment within a large environment, typically characterized by fixed but also mobile obstacles, namely, those whose position is not necessarily known a priori. A complex task taken as reference in this study can be outlined as follows:

• The person in need of support asks the robot, either vocally or with a hand gesture, to go and retrieve an object, suppose a glass of water, located on a support surface that cannot be reached by the person;
• The robot either directly interprets the voice command or points its stereoscopic camera system towards the visual target, triggered by the voice command, interpreting its relative meaning;
• The robot scans the surrounding environment through its sensors, namely, sonars and laser range-finders, to map the space in which it operates, necessary for planning the motion of the mobile base. Furthermore, through the vision system positioned on its head, it searches for the visual target to reach and take. The target search itself is a complex task that may require moving the robot randomly if there is visual coverage of the target to be reached;
• A trajectory is automatically planned for the robot to follow to avoid colliding with fixed obstacles; along the path, the robot continues to acquire information from its surroundings through its sensors by stopping or changing its trajectory based on obstacle avoidance algorithms [20];
• Once the proximity of the target object is reached, the optimal configuration of the robot with which to grasp the target is evaluated, taking into account all the available degrees of freedom as a whole;
• Finally, the robot’s motion is planned to bring the object to and interact with the person who requested it.

The present work aims to answer step 5 of the above outline, focusing on the practical task of grasping a glass from a table while enhancing the TIAGo robot dexterity and precision. In order to identify the optimal configuration of the robot on the basis of an appropriate performance index, it is necessary to investigate its position kinematics in both the direct and inverse problems, as well as determine and analyze its Jacobian matrix.

3. Position Kinematics of the TIAGo Robot

The TIAGo robot is available in different versions, as already mentioned in the introductory section. The setup taken as reference in this work is the one shown in the middle of Figure 2, where a mobile base at the bottom is driven by two differential wheels, with four caster wheels used to ensure stable support on the ground. The robot trunk can be moved vertically through the presence of an actuated prismatic pair, which allows the robot to operate at different heights. The head is connected to the upper part of the robot, where a stereoscopic vision system resides, which is equipped with two revolute joints that allow the vision system to be pointed towards the visual target. Under the head, the stereo microphone system is rigidly mounted to the top part of the trunk. The robotic arm equipped with seven revolute joints is connected to the front part of the robot trunk, under the microphones. The redundancy of the arm compared to a 6 dof Cartesian task makes it suitable for highly dexterous operations, being able to have an additional degree of freedom, without taking into account the other degrees of freedom made available by the mobile base and the trunk itself. A grasping tool is then installed at the end of the arm. In the present study, a collaborative two-finger electric gripper is considered.

In order to study the TIAGo position kinematics, the positioning of several reference frames is required, attached rigidly to the movable rigid bodies of the robot. The main reference systems are shown in Figure 4, whereas the others related to the robotic arm and the TIAGo head are shown in Figure 5 and Figure 6, respectively.

In more detail, $O_b-x_b{y}_b{z}_b$ is the reference system placed at the center of the mobile platform, between the centers of the two actuated wheels, $O_t-x_t{y}_t{z}_t$ refers to the trunk and is placed on its top surface, $O_h-x_h{y}_h{z}_h$ is the frame at the root of the head, where the latter is physically connected, and finally, $O_f-x_f{y}_f{z}_f$ is located at the end of the robotic arm, on the flange where several tools can be assembled according to the user’s needs. In Figure 4, on the bottom right, a simple electric gripper is considered, with a further reference frame $O_g-x_g{y}_g{z}_g$ in the middle of the two actuated fingers.

The mentioned frames are placed in appropriate positions along the mechanical structure of the robot, respecting the conventions adopted in the Robotics Systems Toolbox 4.0 made available by Mathworks Matlab. Referring to the robotic arm, the frames are placed so that the joint rotations occur around the local unit vector z. The Denavit–Hartenberg (DH) convention is not convenient for studying the kinematics of this robot because of the numerous offsets between the robotic arm joints, resulting in an unintelligible home configuration when the arm joint axes have zero rotation. It is clear from Figure 5 on the right that with the chosen convention all the frames assume a simple relative orientation in the home configuration. The simulation environment offered by Matlab is used, on the one hand, as a verification tool and, on the other hand, so that the kinematic equations provided in this study are compatible with the toolbox in view of further developments on the robot.

Focusing on the head of the robot, two further reference systems allow movement from the base of the head toward the stereo vision system: $O_p-x_p{y}_p{z}_p$ is the intermediate system responsible of the head pointing direction, whereas $O_e-x_e{y}_e{z}_e$ is placed conveniently between the sensors of the two RGB-D cameras. Each camera has a wide field of view, ${58}^{\circ }$ horizontally, ${45}^{\circ }$ vertically, and ${70}^{\circ }$ diagonally, with a depth range from $0.4$ m to 8 m. Actually, the vision system can be handled by means of the Python scripts and drivers that Pal Robotics makes available in the programming environment, allowing for the evaluation of distance f and other physical distances required to place $O_e-x_e{y}_e{z}_e$ accurately.

3.1. Direct Position Kinematics

The direct position kinematics of the TIAGo robot is handled with conventional homogeneous transformations, according to the following modular operation:

$${}_B{}^A\mathbf{T}=\left[\begin{array}{cc}{}_B{}^A\mathbf{R}{\mathbf{R}}_z\left(\vartheta \right)& {}^A\mathbf{p}_B\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right],$$

(1)

where ${}^A\mathbf{p}_B={\left[{}^{A}x_B{,}^A{y}_B{,}^A{z}_B\right]}^T$ represents the position vector between the two origins A and B of two generic reference systems and ${}_B{}^A\mathbf{R}$ is the rotation matrix between the two frames, always given by the same sequence of elementary rotations around local x, y, and z axes, namely,

$${}_B{}^A\mathbf{R}={\mathbf{R}}_x\left(\alpha \right){\mathbf{R}}_y\left(\beta \right){\mathbf{R}}_z\left(\gamma \right).$$

(2)

In (2) the conventional elementary rotations have the form given by

$$\begin{array}{ccc}{\mathbf{R}}_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]& {\mathbf{R}}_y=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]& {\mathbf{R}}_z=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]\end{array}.$$

(3)

The last row in (1) consists of three zeros and one one, according to the convention for homogeneous transformations. Moreover, it can be observed that the rotation $\vartheta$ associated with the degree of freedom provided locally by a revolute joint, when focusing on the robotic arm, is explicitly involved in a rotation matrix around a z-axis. In fact, matrix ${}_B{}^A\mathbf{R}$ is used first to orient the local z-axis along the revolute joint, then an elementary ${\mathbf{R}}_z\left(\vartheta \right)$ rotation can be driven by the actuated joint. This choice has allowed for a more compact notation.

All the geometric data of the TIAGo robot required in the computation of the Homogeneous transformations in (1) can be retrieved from

Table 1. Kinematic and geometric parameters of the TIAGo robot.

A	B	${}^{\mathit{A}}{\mathit{x}}_{\mathit{B}}\left[\mathbf{mm}\right]$	${}^{\mathit{A}}{\mathit{y}}_{\mathit{B}}\left[\mathbf{mm}\right]$	${}^{\mathit{A}}{\mathit{z}}_{\mathit{B}}\left[\mathbf{mm}\right]$	$\mathit{\alpha }\left[\mathbf{rad}\right]$	$\mathit{\beta }\left[\mathbf{rad}\right]$	$\mathit{\gamma }\left[\mathbf{rad}\right]$	$\mathit{\vartheta }\left[\mathbf{rad}\right]$
O	$O_b$	$x_{O_b}$	$y_{O_b}$	98.5	0	0	0	${\vartheta }_b$
$O_b$	$O_t$	0	−62.0	d	0	0	0	0
$O_t$	$O_1$	155.1	14.0	−151.0	0	0	$-\pi /2$	${\vartheta }_1$
$O_1$	$O_2$	125.0	16.5	−31.0	$\pi /2$	0	0	${\vartheta }_2$
$O_2$	$O_3$	89.5	0	1.5	$-\pi /2$	$-\pi /2$	0	${\vartheta }_3$
$O_3$	$O_4$	−20.0	−27.0	−222.0	$-\pi /2$	0	$-\pi /2$	${\vartheta }_4$
$O_4$	$O_5$	−162.0	20.0	27.0	0	$-\pi /2$	0	${\vartheta }_5$
$O_5$	$O_6$	0	0	150.0	$-\pi /2$	0	$-\pi /2$	${\vartheta }_6$
$O_6$	$O_7=O_f$	66.0	0	0	$\pi /2$	$\pi /2$	0	${\vartheta }_7$
$O_f$	$O_g$	0	0	0.2	0	0	0	0
$O_t$	$O_h$	182.0	0	0	0	0	0	${\vartheta }_{h_1}$
$O_h$	$O_p$	5.0	0	98.0	$\pi /2$	0	0	${\vartheta }_{h_2}$
$O_p$	$O_e$	120	106	0	0	0	0	0

by selecting the required reference frames. Each line in the table is related to a specific couple of frames, with origins labeled A and B. In Table 1, the distances ${}^A{x}_B$ and ${}^A{y}_B$ in the last row are hypothesized by comparison with other known dimensions.

The joints of the robot, representing its degrees of freedom, can change in the ranges collected in

Table 2. Motion range of the actuated kinematic joints.

Joint	Type	Lower Limit	Upper Limit
d	P	0 mm	350 mm
${\theta }_1$	R	$0^{\circ }$	${157.5}^{\circ }$
${\theta }_2$	R	$-{90}^{\circ }$	${62.5}^{\circ }$
${\theta }_3$	R	$-{202.5}^{\circ }$	${90}^{\circ }$
${\theta }_4$	R	$-{22.5}^{\circ }$	${135}^{\circ }$
${\theta }_5$	R	$-{120}^{\circ }$	${120}^{\circ }$
${\theta }_6$	R	$-{90}^{\circ }$	${90}^{\circ }$
${\theta }_7$	R	$-{120}^{\circ }$	${120}^{\circ }$
${\theta }_{h_1}$	R	$-{75}^{\circ }$	${75}^{\circ }$
${\theta }_{h_2}$	R	$-{60}^{\circ }$	${45}^{\circ }$

due to mechanical limitations. Given the configuration vector of the robot in terms of the coordinates $\mathbf{q}={\left[{\mathbf{q}}_b^T,d,{\mathbf{q}}_r^T\right]}^T$ with ${\mathbf{q}}_b={\left[x_{O_b},y_{O_b},{\vartheta }_b\right]}^T$ related to the AMR and ${\mathbf{q}}_r={\left[{\vartheta }_1,{\vartheta }_2,{\vartheta }_3,{\vartheta }_4,{\vartheta }_5,{\vartheta }_6,{\vartheta }_7\right]}^T$ to the robotic arm, in order to obtain the pose ${}_{O_g}^O\mathbf{T}$ of the end-effector of the robotic arm with respect to the absolute reference frame $O-xyz$ on the left upper corner of Figure 4, a matrix post-multiplication can be carried out:

$${}_{O_g}^O\mathbf{T}=\left[\begin{array}{cc}{}_{O_g}^O\mathbf{R}& {}^O{\mathbf{p}}_g\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]={}_{O_b}^O\mathbf{T}{}_{O_t}^{O_b}\mathbf{T}{}_{O_7}^{O_t}\mathbf{T}{}_{O_g}^{O_f}\mathbf{T},$$

(4)

where ${}^O{\mathbf{p}}_g$ and ${}_{O_g}^O\mathbf{R}$ represent, respectively, the absolute position of point $O_g$ at the end-effector and the rotation matrix that provides its orientation with respect to the absolute reference system in O. The last row in (4) consists of three zeros and one one, as usual. Moreover, looking at the member on the right of (4), the two reference frames in $O_7$ and $O_f$ coincide and matrix ${\mathbf{T}}_{O_b}^O$ refers to the planar motion of the TIAGo mobile base, whose degrees of freedom can be assumed to vary in the ranges $x_{O_b}\in \left[-\infty ,\infty \right]$, $y_{O_b}\in \left[-\infty ,\infty \right]$ and ${\vartheta }_b\in \left[-{180}^{\circ },{180}^{\circ }\right]$. Obviously, the extremes with infinite value mean that the translation of the base is free within the available space, namely, the limits of the room in which the robot moves for indoor applications. If an orientation in terms of a minimum set of angles was required instead of via the rotation matrix ${}_{O_g}^O\mathbf{R}$ in (4), one of the most common choices in robotics applications would be the ZYZ convention of Euler angles. In this case, three angles $\left(\varphi ,\theta ,\psi \right)$, conveniently gathered in a vector called $\mathbf{e}$, can be used to underscore the three degrees of freedom related to the spatial rotation of the robot end-effector following the sequence ZYZ, namely, ${}_{O_g}^O\mathbf{R}={\mathbf{R}}_z\left(\varphi \right){\mathbf{R}}_y\left(\theta \right){\mathbf{R}}_z\left(\psi \right)$, by using the elementary rotations in (3). The conventional inverse solution to make them explicit can be used:

$$\begin{array}{c}\hfill \mathbf{e}=\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right]=\left[\begin{array}{c}arctan2\left({\mathbf{R}}_{2,3},{\mathbf{R}}_{1,3}\right)\\ arctan2\left(\sqrt{{\mathbf{R}}_{3,1}^2+{\mathbf{R}}_{3,2}^2},{\mathbf{R}}_{3,3}\right)\\ arctan2\left({\mathbf{R}}_{3,2},-{\mathbf{R}}_{3,1}\right)\end{array}\right]\end{array},$$

(5)

where the subscripts $\left(i,j\right)$ in ${\mathbf{R}}_{i,j}$ point at the $i^{th}$ row and $j^{th}$ column, respectively, of matrix ${}_{O_g}^O\mathbf{R}$. Particular attention must be paid to $\theta =\pm \pi /2$, namely, when singularities of representation occur.

The planar motion of the TIAGo base, namely, a two-wheeled nonholonomic AMR, is regulated by the following well-known differential problem:

$${\dot{\mathbf{q}}}_b=\left[\begin{array}{c}{\dot{x}}_{O_b}\\ {\dot{y}}_{O_b}\\ {\dot{\theta }}_b\end{array}\right]=\left[\begin{array}{cc}{\displaystyle \frac{r}{2}}cos{\theta }_b& {\displaystyle \frac{r}{2}}cos{\theta }_b\\ {\displaystyle \frac{r}{2}}sin{\theta }_b& {\displaystyle \frac{r}{2}}sin{\theta }_b\\ -{\displaystyle \frac{r}{2t}}& {\displaystyle \frac{r}{2t}}\end{array}\right]\left[\begin{array}{c}{\omega }_L\\ {\omega }_R\end{array}\right],$$

(6)

where $r=98.5$ mm, which is also the coordinate z of the local frame in $O_b$ with respect to the absolute fixed frame $O-xyz$, $t=202.2$ mm is the distance of the wheel center from the origin $O_b$ shown in Figure 4, and ${\omega }_L$ and ${\omega }_R$ are, respectively, the left and right angular velocities of the AMR actuated wheels. Therefore, the absolute position coordinates of the TIAGo base result from a time integration of (6).

3.2. Differential Kinematics

The relationship between the velocity vector $\dot{\mathbf{q}}={\left[{\dot{\mathbf{q}}}_b^T,\dot{d},{\dot{\mathbf{q}}}_r^T\right]}^T$ related to the actuated joints of the robot, involving directly the planar velocities of the moving base ${\dot{\mathbf{q}}}_b^T$ in place of the wheel angular velocities, and the velocity vector of the robot end-effector $\dot{\mathbf{p}}={\left[{}^O{\dot{\mathbf{p}}}_g^T{,}^O{\mathsf{\omega }}_g^T\right]}^T={\left[{}^O{\dot{x}}_g{,}^O{\dot{y}}_g{,}^O{\dot{z}}_g{,}^O{\omega }_{g,x}{,}^O{\omega }_{g,y}{,}^O{\omega }_{g,z}\right]}^T$ can be obtained by parting the linear and angular velocities according to the following procedure:

• A time derivative of vector ${}^O{\mathbf{p}}_g$ gives directly the expression of the first three rows of the Jacobian matrix. Isolating the coordinates gathered in vector $\dot{\mathbf{q}}$, a $3\times 11$ Jacobian matrix related to the linear velocities is obtained:

  $${}^O{\dot{\mathbf{p}}}_g={\mathbf{J}}_l\dot{\mathbf{q}}$$

  (7)
• The angular velocity ${}^O{\mathsf{\omega }}_g$ results from the vector sum of all the angular velocities gained along the kinematic structure of the robot, by driving the planar rotation of the mobile base and actuating the robot joints. ${\widehat{\mathbf{z}}}_i$ is the unit vector along the local $i^{th}$ z-axis around which rotations occur; thus, it follows that

  $${}^O{\mathsf{\omega }}_g={\dot{\vartheta }}_b^O{\widehat{\mathbf{z}}}_O+{\dot{{\vartheta }_1}}^O{\widehat{\mathbf{z}}}_1+{\dot{\vartheta }}_2^O{\widehat{\mathbf{z}}}_2+{\dot{\vartheta }}_3^O{\widehat{\mathbf{z}}}_3+{\dot{{\vartheta }_4}}^O{\widehat{\mathbf{z}}}_4+{\dot{\vartheta }}_5^O{\widehat{\mathbf{z}}}_5+{\dot{\vartheta }}_6^O{\widehat{\mathbf{z}}}_6+{\dot{\vartheta }}_7^O{\widehat{\mathbf{z}}}_7$$

  (8)

  which, in compact form, provides the expression of a $3\times 11$ Jacobian matrix, this time related to rotations:

  $${}^O{\mathsf{\omega }}_g=\left[{\mathbf{0}}_{3\times 1},{\mathbf{0}}_{3\times 1},{\widehat{\mathbf{z}}}_O,{\mathbf{0}}_{3\times 1},{\widehat{\mathbf{z}}}_1,{\widehat{\mathbf{z}}}_2,{\widehat{\mathbf{z}}}_3,{\widehat{\mathbf{z}}}_4,{\widehat{\mathbf{z}}}_5,{\widehat{\mathbf{z}}}_6,{\widehat{\mathbf{z}}}_7\right]\dot{\mathbf{q}}={\mathbf{J}}_a\dot{\mathbf{q}}$$

  (9)

From Equations (7) and (9), the complete $6\times 11$ Jacobian ${\mathbf{J}}_G$ that allows one to solve the direct velocity kinematics of the TIAGo robot can be found:

$$\dot{\mathbf{p}}=\left[\begin{array}{c}{}^O{\dot{\mathbf{p}}}_g\\ {}^O{\mathsf{\omega }}_g\end{array}\right]={\mathbf{J}}_G\dot{\mathbf{q}}=\left[\begin{array}{c}{\mathbf{J}}_l\\ {\mathbf{J}}_a\end{array}\right]\dot{\mathbf{q}}$$

(10)

Matrix ${\mathbf{J}}_G$ is the geometric Jacobian, but sometimes the analytical version is required. ${\mathbf{J}}_A$ is the analytical Jacobian, which can be derived by knowing the relationship between the angular velocity vector ${}^O{\mathsf{\omega }}_g$ and the time derivative of the Euler angles in (5):

$${}^O{\mathsf{\omega }}_g=\mathsf{\Theta }\dot{\mathbf{e}}=\left[\begin{array}{ccc}0& -sin\varphi & cos\varphi sin\theta \\ 0& cos\varphi & sin\varphi sin\theta \\ 1& 0& cos\theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

(11)

Since ${\mathbf{p}}_e={\left[{}^O{\mathbf{p}}_g^T,{\mathbf{e}}^T\right]}^T$ and ${\dot{\mathbf{p}}}_e$ are, respectively, the pose and the velocity vector of the robot’s end-effector as a function of Euler angles, it follows that

$${\dot{\mathbf{p}}}_e=\left[\begin{array}{c}{}^O{\dot{\mathbf{p}}}_g\\ \dot{\mathbf{e}}\end{array}\right]={\mathbf{J}}_A\dot{\mathbf{q}}=\left[\begin{array}{c}{\mathbf{J}}_l\\ {\mathsf{\Theta }}^{-1}{\mathbf{J}}_a\end{array}\right]\dot{\mathbf{q}}$$

(12)

Both ${\mathbf{J}}_G$ and ${\mathbf{J}}_A$ are rectangular matrices and share the same size; therefore, the inverse velocity kinematics presents the complexity of matrix inversion, typical of redundant manipulators. A well-known solution is to solve a constrained optimal problem by using the pseudoinverse ${\mathbf{J}}^{†}$ of the Jacobian matrix, also called the Moore–Penrose inverse of the Jacobian matrix, which gives the best possible solution in terms of least squares [25]. Matrix ${\mathbf{J}}^{†}$ is defined as ${\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{J}}^T\right)}^{-1}=\mathbf{V}{\mathsf{\Sigma }}^{†}{\mathbf{U}}^T$, also taking advantage of singular value decomposition, namely, $\mathbf{J}=\mathbf{U}\mathsf{\Sigma }{\mathbf{V}}^T$. It is recalled from linear algebra that both $\mathbf{U}$ and $\mathbf{V}$ are orthogonal matrixes, that $\mathsf{\Sigma }$ gathers diagonally the singular values of matrix $\mathbf{J}$, and, finally, that the pseudoinverse ${\mathsf{\Sigma }}^{†}$ of $\mathsf{\Sigma }$ requires the substitution of the non-zero diagonal terms with their reciprocal, as well as a transposition of the matrix obtained. A damped version of the pseudoinverse ${\mathbf{J}}^{†}$ can be formulated in order to avoid singular configurations, namely, ${\mathbf{J}}^{†}={\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{J}}^T+k^2{\mathbf{I}}_6\right)}^{-1}$ with ${\mathbf{I}}_6$, the $6\times 6$ identity matrix, and k a damping coefficient, whose value can be chosen according to optimization techniques [26]. This choice is important in the context of motion planning, proposing a control-managed variable damping coefficient that operates in the proximity of singular configurations and reverts to zero outside these conditions, avoiding a loss of accuracy in trajectory tracking [27]. Furthermore, a weighting matrix $\mathbf{W}$ can also be used in order to favor some motors with respect to others [28], for instance, the motion of the moving base with respect to the motion of the robotic arm, according to the expression ${\mathbf{J}}^{†}={\mathbf{W}}^{-1}{\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{W}}^{-1}{\mathbf{J}}^T+k^2{\mathbf{I}}_6\right)}^{-1}$.

3.3. Inverse Position Kinematics

The inverse position kinematics of the TIAGo robot, meaning the search for a configuration $\mathbf{q}$ corresponding to an assigned Cartesian pose of the robot end-effector, can be solved only numerically by choosing an optimization strategy, as already pointed out in the previous section for velocities, since TIAGo is a highly redundant robot. Therefore, with reference to the expressions of ${\mathbf{J}}^{†}$ presented above, the iterative Gauss–Newton algorithm is proposed to solve the inverse kinematics problem:

$${\mathbf{q}}_{k+1}={\mathbf{q}}_k+{\mathbf{J}}^{†}\left({\mathbf{p}}_{e,f}-{\mathbf{p}}_k\right)$$

(13)

where ${\mathbf{p}}_{e,f}={\left[{}^O{x}_g{,}^O{y}_g{,}^O{z}_g,\varphi ,\theta ,\psi \right]}^T$ is the assigned pose of the TIAGo end-effector, ${\mathbf{q}}_k$ and ${\mathbf{p}}_k$ are, respectively, the robot configuration vector with all eleven variables and the corresponding end-effector pose vector related to the $k^{th}$ iteration, and ${\mathbf{J}}^{†}$ is the pseudoinverse already mentioned. It is shown in the following section that the algorithm in (13) is employed with some modifications to find the optimal posture of the robot for a given Cartesian pose, making use of the analytical Jacobian matrix ${\mathbf{J}}_A$ presented in (12) obtained through the use of Euler angles. In order to solve Equation (13), a first known configuration of the robot joint variables and their corresponding Cartesian pose must be determined, possibly not too far from the configuration searched by means of the inverse kinematics algorithm. Some attempts are sometimes required to find a good starting configuration to allow the algorithm to converge quickly to a solution, which must also meet the allowable ranges shown in Table 2. The singular configurations possibly encountered by the iterative algorithm in (13) is easily overcome by verifying the rank of ${\mathbf{J}}_A$ for each step, updating the pseudoinverse ${\mathbf{J}}^{†}$ with its damped version to avoid numerical indeterminacy.

4. Posture Optimization of the Grasping Task

In this section, a study is proposed to identify the optimal robot configuration for the grasping task from the infinite possibilities theoretically available (actually ${\infty }^5$, due to the excess of degrees of freedom). As already mentioned, the grasping operation can be carried out by adjusting all the independent variables for both the mobile base and the robotic arm to enhance the robot’s performance in accomplishing the task. Since the robot configuration is managed with eleven variables and the task is defined by assigning a Cartesian pose to the end-effector corresponding to six degrees of freedom, there is a surplus of five degrees of freedom that can be freely selected.

In the field of robotics, an index commonly used in the search for optimal kinematic configurations of a robot is the condition number $\kappa$ of its Jacobian matrix [29,30]. The condition number offers insights into the sensitivity of the robot’s motion to variations in control inputs, with lower values indicating more uniform control sensitivity across different directions. The 2-norm condition number for inversion has been chosen as the reference for the kinematic optimization proposed in this work, using the singular value decomposition already mentioned to find the ratio of the largest to smallest singular values. This method highlights configurations where the robot demonstrates similar performance in every direction, either in terms of generalized velocity or force, depending on the specific task requirements. Actually, an inversion is conveniently applied to the condition number to obtain a finite index. In this way, the highest possible theoretical value is one, corresponding to the isotropic configuration, where the robot gains analogous performance in every direction. Ultimately, in the most comprehensive case, the index expression is

$$I={\displaystyle \frac{1}{\kappa \left(\mathbf{J}{\mathbf{J}}^T\right)}}={\displaystyle \frac{{\lambda }_{min}\left(\mathbf{J}{\mathbf{J}}^T\right)}{{\lambda }_{max}\left(\mathbf{J}{\mathbf{J}}^T\right)}}$$

(14)

where $\mathbf{J}$ represents the geometric Jacobian ${\mathbf{J}}_G$ in (10), ${\lambda }_{min}$ and ${\lambda }_{max}$ are the minimum and maximum eigenvalues, respectively, of matrix $\mathbf{J}{\mathbf{J}}^T$, and $\kappa$ is the condition number of $\mathbf{J}{\mathbf{J}}^T$. To specifically focus on the linear velocities of the TIAGo end-effector, expression (14) can be updated by replacing $\mathbf{J}$ with ${\mathbf{J}}_l$, which is a $3\times 11$ Jacobian matrix. Similarly, for angular velocities, the $3\times 11$ Jacobian matrix ${\mathbf{J}}_a$ should be considered. If required, more complex indexes that allow us to overcome the non-homogeneous feature of the full Jacobian matrix can be taken into account [31].

The degrees of freedom that have been spanned in their possible range of motion are the three planar ones of the mobile base, gathered in vector ${\mathbf{q}}_b$, the vertical displacement d of the robot trunk, and finally, the spin ${\vartheta }_7$ around the last revolute joint of the robotic arm, related to the axial rotation of the gripper. The choice of considering these five variables as redundant is justified by the fact that the moving base and the vertical prismatic axis are the most influential in achieving a generic Cartesian configuration for the robot end-effector, the former being able to move freely in the horizontal plane unless encountering obstacles, and the latter being able to adjust the robot’s height relative to the gripping target. The rotation around the robot’s last joint was chosen with a pitching rotation of the grasped object in mind, allowing for a rotation of an object, such as a glass or bottle, under the assumption of spilling its contents. Other choices can be made, for instance, the rotation around the third axis of the robotic arm, considered typically the redundant axis of a 7 dof robot. These five specific degrees of freedom were selected for exploration to systematically evaluate how variations in each could impact the robot’s performance in the grasping task. In particular, their ranges vary as shown below:

$$\begin{array}{c}-200\mathrm{mm}\le x_b\le 200\mathrm{mm}\\ -200\mathrm{mm}\le y_b\le 200\mathrm{mm}\\ -{\displaystyle \frac{\pi }{4}}\le {\vartheta }_b\le {\displaystyle \frac{\pi }{4}}\\ 0\mathrm{mm}\le d\le 350\mathrm{mm}\\ -{\displaystyle \frac{2\pi }{3}}\le {\vartheta }_7\le {\displaystyle \frac{2\pi }{3}}\end{array}.$$

(15)

By assigning values to each of the variables listed above, a finite number of solutions for the inverse kinematics problem of the remaining 6 dof robotic arm can be determined, excluding singular configurations. Therefore, given a reference initial posture ${\mathbf{q}}_{r_6,0}={\left[{\vartheta }_1,{\vartheta }_2,{\vartheta }_3,{\vartheta }_4,{\vartheta }_5,{\vartheta }_6\right]}^T$ related to a pose ${\mathbf{p}}_e$ not too far from the required pose ${\mathbf{p}}_{e,f}$ for grasping the target object, the numerical algorithm presented in (13) efficiently identifies the robot configuration that matches the assigned final pose ${\mathbf{p}}_{e,f}$. The algorithm consists of two routines: the first one revolves the inverse kinematics for an assigned value of the five variables in $\left(x_b,y_b,{\vartheta }_b,d,{\vartheta }_7\right)$; the second one ranges all possible values for the variables in $\left(x_b,y_b,{\vartheta }_b,d,{\vartheta }_7\right)$, providing information about the index I in (14).

4.1. Routine 1—Inverse Kinematics Routine

• A Cartesian pose ${\mathbf{p}}_{e,f}$ is assigned, namely, the gripper is located on the target in the gripping configuration. In this case, as shown in Figure 7, the pose is

  $${\mathbf{p}}_{e,f}=\left[\begin{array}{cccc}0& 0& 1& 0.9\mathrm{m}\\ 0& 1& 0& 0\mathrm{m}\\ -1& 0& 0& 0.7\mathrm{m}\\ 0& 0& 0& 1\end{array}\right];$$

  (16)
• A vector ${\mathbf{q}}_{ind}={\left[x_b,y_b,{\vartheta }_b,d,{\vartheta }_7\right]}^T$ is chosen so that its elements fall into the intervals mentioned above. For instance, ${\mathbf{q}}_{ind}={\left[0,0,0,0,\pi /2\right]}^T$;
• A reference initial posture ${\mathbf{q}}_{r_6,0}$ is defined. Without loss of generality, the following non-singular configuration has been chosen for the robotic arm joints, also verified visually through the Matlab Robotics Systems Toolbox, as shown in Figure 8:

  $${\mathbf{q}}_{r_6,0}={\left[{20}^{\circ },{10}^{\circ },-{70}^{\circ },{70}^{\circ },{70}^{\circ },{30}^{\circ }\right]}^T;$$

  (17)
• Equation (4), together with the relations in (5), is used to find the initial pose ${\mathbf{p}}_{e,0}$ associated with the overall vector $\mathbf{q}$ that gathers ${\mathbf{q}}_{ind}$ and ${\mathbf{q}}_{r_6,0}$;
• The $6\times 6$ portion ${\mathbf{J}}_{A,r}$ of the analytical Jacobian matrix in (12) related only to the rotations in ${\mathbf{q}}_{r_6,0}$, more specifically, taking from ${\mathbf{J}}_A$ the third column and the columns from the fifth to the tenth, is evaluated for the $\mathbf{q}$ of step 4;
• The algorithm in (13) is updated with the $6\times 6$ Jacobian of the previous step to solve the inverse kinematics problem:

  $${\mathbf{q}}_{r_6,k+1}={\mathbf{q}}_{r_6,k}+{\mathbf{J}}_{A,r}^{†}\left({\mathbf{p}}_{e,f}-{\mathbf{p}}_{e,k}\right),$$

  (18)

  where the integer k starts from 0 and gradually increases in the iterative process toward convergence, achieved only when a tolerance for both position and orientation is observed: $\parallel {\mathbf{p}}_{g,f}-{\mathbf{p}}_{g,k}\parallel \le {ϵ}_l$ and $\parallel {\mathbf{e}}_f-{\mathbf{e}}_k\parallel \le {ϵ}_a$, with sufficiently small values for the tolerances ${ϵ}_l$ and ${ϵ}_a$;
• The new position ${\mathbf{p}}_{e,k+1}$ is computed by means of the direct kinematics in (4) applied to ${\mathbf{q}}_{k+1}$, namely, the vector that gathers ${\mathbf{q}}_{r_6,k+1}$ and ${\mathbf{q}}_{ind}$;
• The procedure continues iteratively until convergence, when reached. An example is presented in Figure 7, where the shown ${\mathbf{q}}_{r_6}$ results from a given ${\mathbf{q}}_{ind}$. On the contrary, the choice for ${\mathbf{q}}_{ind}$ is discarded from the routine and another vector is evaluated, starting again from step 2. The routine outputs the value of ${\mathbf{q}}_{r_6,k}$ that verifies the tolerances of step 6.

4.2. Routine 2—Posture Optimization

• Five nested loops sweep the values of the variables in ${\mathbf{q}}_{ind}$;
• For each ${\mathbf{q}}_{ind}$ determined in the previous step, Routine 1 is executed in order to find the final ${\mathbf{q}}_{r_6,k}$ associated with ${\mathbf{q}}_{ind}$;
• The eigenvalues and eigenvectors of ${\mathbf{J}}_l{\mathbf{J}}_l{}^T$ are determined, recording this information so as to know the associated manipulability ellipsoid. It is known that the eigenvectors represent the principal axes of the ellipsoid and the eigenvalues the respective dimensions;
• The index I in (14) is evaluated and recorded for that particular posture of the robot;
• The highest value obtained for I allows us to intercept the best posture in terms of velocity manipulability, to which the optimal ellipsoid corresponds;
• Similarly, for rotations, steps 3 to 5 can be repeated using matrix ${\mathbf{J}}_a$ instead of matrix ${\mathbf{J}}_l$.

5. Results

The routines proposed in the previous section have been conveniently implemented in Matlab to find the best posture of the TIAGo robot, according to index I values, when the Cartesian pose in (16) is assigned. On the left of Figure 9, the entire set of ellipsoids is shown, numbering 961 from the scanning of vector ${\mathbf{q}}_{ind}$. It can be noted that most of the ellipsoids must have a low index I because of their flattened shape, with semi-axes in the $yz$-plane much larger than that along the x-axis. On the right of Figure 9, the best ellipsoid in terms of linear velocities of the robot end-effector corresponding to the highest value of $I=0.41$ is shown, meaning that all the Cartesian directions have a similar linear velocity performance. The volume of such an ellipsoid is clearly smaller than many others that are visible in the left image, but there was a desire to favor a closer proximity to the isotropic condition over having a strongly anisotropic behavior. When an anisotropic behavior is accepted, a different posture of the robot can be chosen in order to have higher velocities in some directions.

Figure 10 shows the optimized posture that maximized index I. In this configuration, the mobile base of the robot is closer to the table and the arm is less extended, as well as positioned to lie on a horizontal plane $xy$. Such a condition is allowed by an extension of the vertical prismatic joint of the trunk. A comparison with the generic configuration already presented in Figure 7 points out these differences in posture, as Figure 11 clearly demonstrates.

This study has been also extended to angular velocities by using matrix ${\mathbf{J}}_a$ instead of ${\mathbf{J}}_l$. It results in a set of angular velocity ellipsoids similar to the one presented in Figure 9, but more extended along the x-axis, almost representing a sphere. In this case, the best ellipsoid is related to a higher value of the index I defined in (14), namely, $I=0.58$. It means that the robot end-effector can rotate with a similar angular velocity in every direction. Even in this case, the best ellipsoid has a shape similar to the one shown in Figure 9 on the right, but with a higher volume.

As is well known, ellipsoids in force have orthogonal behavior compared to those in velocity presented in this paper. These become useful for evaluating the robot’s performance in terms of static lifting capacity or thrust/resistance.

6. Conclusions

This study has systematically explored the impact of varying degrees of freedom on the performance of the TIAGo robot’s end-effector for grasping tasks, with a particular focus on the manipulability of linear and angular velocities. By employing a comprehensive methodology that includes the calculation of condition numbers through the geometric Jacobian and the execution of two distinct routines (inverse kinematics routine and posture optimization), optimal postures that maximize the robot’s efficiency in simulated grasping tasks have been identified.

Optimal Postures Enhance Performance: Adjusting specific degrees of freedom can significantly improve the robot’s efficiency in grasping tasks, underscoring the importance of precise control in robotic design.

Implications and Future Directions: The presented methodology provides a foundation for improving motion planning and control of the TIAGo robot, with potential applications across various platforms and tasks characterized by redundant degrees of freedom. Future research should explore these techniques in different contexts and integrate advanced predictive algorithms to further optimize performance.

Limitations: While insightful, the findings proposed in this paper are based on simulations with the TIAGo robot and may require validation in real-world scenarios.

In summary, this research highlights the critical role of posture optimization in robotic grasping tasks, offering a pathway toward more responsive and capable robotic systems.