Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment
Pajak, Iwona;Pajak, Grzegorz
2021-07-05 00:00:00
applied sciences Article Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment † ,† Iwona Pajak and Grzegorz Pajak * Institute of Mechanical Engineering, University of Zielona Gora, 65-516 Zielona Gora, Poland; i.pajak@iim.uz.zgora.pl * Correspondence: g.pajak@iim.uz.zgora.pl † These authors contributed equally to this work. Abstract: This paper presents the usage of holonomic mobile humanoid manipulators to carry out autonomous tasks in industrial environments, according to the smart factory concept and the Industry 4.0 philosophy. The problem of transporting lengthy objects, taking into account mechanical limitations, the conditions for avoiding collisions, as well as the dexterity of the manipulator arms was considered. The primary problem was divided into three phases, leading to three different types of robotic tasks. In the proposed approach, the pseudoinverse Jacobian method at the acceleration level to solve each of the tasks was used. The redundant degrees of freedom were used to satisfy secondary objectives such as robot kinetic energy, the maximization of the manipulability measure, and the fulfillment mechanical and collision-avoidance limitations. A computer example involving a mobile humanoid manipulator, operating in an industrial environment, illustrated the effectiveness of the proposed method. Keywords: humanoid manipulator; mobile robot; motion planning; transportation task Citation: Pajak, I.; Pajak, G. Motion Planning for a Mobile Humanoid Manipulator Working in an Industrial Environment. Appl. Sci. 2021, 11, 1. Introduction 6209. https://doi.org/10.3390/ The history of industrial robotics dates back to the 1950s. At that time, the first designs app11136209 of numerically controlled machine tools and programmable manipulators for industrial applications were created. At the beginning of the 1960s, the first mass-produced robots Academic Editors: Antoni Swic ´ and were used in production processes, and a few years later, the first automated production Justyna Patalas-Maliszewska line was created. In subsequent years, the use of automation systems increased, leading to the third industrial revolution in the 1970s. This stage, in the development of robotics, Received: 25 May 2021 was characterized by the use of relatively simple robots that performed repetitive tasks Accepted: 1 July 2021 that had been planned in advance. The fourth industrial revolution, known as Industry 4.0, Published: 5 July 2021 has now already begun, leading to smart factories and autonomous systems that complete production processes without human intervention [1,2]. Such developments require the Publisher’s Note: MDPI stays neutral use of sensors and new ideas to control algorithms, thus allowing operations to be carried with regard to jurisdictional claims in out autonomously. According to the philosophy of Industry 4.0, manufacturing is required published maps and institutional affil- to be ever faster and more efficient, in order that companies might strive to increase the iations. degree of automation [3]. Due to the necessity of meeting the needs of individual customers, interest in the application of knowledge transfer support [4,5] is growing. At present, stationary manipulators predominate in many industrial applications. Their main advantages are their agility and manual dexterity and the ability to perform Copyright: © 2021 by the authors. tasks with high precision. On the other hand, the working space for this type of robotic Licensee MDPI, Basel, Switzerland. mechanism is limited by the length of the arm, which reduces the possibilities of their use. This article is an open access article This results in an increased interest in mobile platforms [6], whose working space is only distributed under the terms and limited by the elements of the equipment present in their surroundings. Unfortunately, conditions of the Creative Commons these type of robots are much less precise than stationary manipulators and usually do not Attribution (CC BY) license (https:// have their manipulation abilities, so they are mainly used to solve simple logistic tasks, creativecommons.org/licenses/by/ such as the transportation of pallets [7]. The combination of the advantages of both systems 4.0/). Appl. Sci. 2021, 11, 6209. https://doi.org/10.3390/app11136209 https://www.mdpi.com/journal/applsci Appl. Sci. 2021, 11, 6209 2 of 23 is a mobile manipulator, which consists of a mobile platform and a manipulator mounted on top. The use of this type of robotic system allows the working space to be enlarged and high precision, in the completion of tasks, to be maintained. Moreover, it allows several stationary manipulators to be replaced with one mobile manipulator moving between different workstations. A review of the main industrial domains suitable for such systems was presented in the works [8,9]. There are, however, some types of tasks that cannot be executed by a single robot. Such tasks are associated with the movement of heavy objects that a single robot cannot lift, due to the physical limitations of its actuators or, again, due to the movement of the large objects, which must be supported at several points. The necessity of solving such problems leads to the deployment of cooperating robots performing a common task. The method of the motion planning of multiple mobile manipulators moving a common rigid object was presented in [10]. As shown in practical applications, the use of cooperating robots results in additional problems related to the necessity for the coordination of the movement of mobile platforms. On the other hand, there are tasks, such as the handling of lengthy objects and peg-in-hole assemblies, etc., where the use of several, individual robots is unnecessary. It seems simply providing a grip at several points is quit sufficient, and in such cases, multi- arm systems, such as humanoid robots, can be used successfully. Moreover, mounting such a system onto a mobile platform increases the operating range significantly and simultaneously maintains the precision of manipulation. For this reason, both stationary and mobile humanoid manipulators have become the subject of growing interest in recent years. Typical solutions for the motion planning problems cannot be used directly for robots of this type, because in this case, it is necessary to take into account additional conditions such as closed-loop constraints, the manipulability of multiarm system, etc. There are several approaches to motion planning of humanoid robots. According to the review presented in [11], the most motion generation methods, due to the requirement of simplicity, are addressed using only kinematics. The method based on master–slave coordination such as the stationary, dual-arm, Baxter robot, working on an assembly line and completing peg-in-hole tasks, was presented in the work [12]. The use of highly redundant humanoid mobile manipulators was proposed in [13]. The authors designed the motion planning algorithms to carry out “human-like” autonomous navigation and manipulation tasks. The redundancy resolution scheme for humanoid manipulators, equipped with differential driven platforms, was presented in [14]. The validity of the algorithm was checked by simulating the robot, in tracing the global optimal path generated by the A (A-star) graph search algorithm. It is worth noting that research on humanoid robotic systems is not just limited to industrial applications. Such systems are also utilized as support for disabled and elderly people [15], medical assistants [16], and service robots for intelligent or smart homes [17]. The solutions obtained in these areas can also be used in industrial applications. The mobility of robotic systems can be realized in different ways. In the case of humanoid manipulators, the use of human-like bipedal platforms seems to be natural [18]. Unfortunately, such a solution causes many technical problems that have not been solved satisfactorily. Additionally, walking robots can be useful in difficult, natural conditions; however, in an industrial environment, the use of wheeled robots is more efficient because this allows higher velocity to be achieved in the robotic system. The choice of the drive type used is also important. Wheeled platforms can be divided into the holonomic type, which can move in any direction, since they have three degrees of freedom, and the nonholonomic type, which are systems whose movement is subject to the constraint of rolling without slipping. The motion planning methods for mobile robots presented above have concerned systems equipped with nonholonomic platforms, but it seems that in industrial conditions, the use of holonomic platforms is justified. Such systems, equipped with Mecanum-type wheels, for example, perform well on the even surface of a production or warehouse hall, and the lack of movement limitations makes it possible to move in a cluttered environment and significantly increases the usefulness of the robot. Appl. Sci. 2021, 11, 6209 3 of 23 In this paper, the trajectory planning method for a humanoid dual-arm manipulator mounted on a holonomic wheeled mobile platform, inspired by the Rollin’ Justin [19] robot, was considered. The main goal of this work was to present the general framework for the motion planning of such systems. In contrast to other solutions, presented in the literature, the comprehensive approach to the execution of the robot task in an industrial environment was proposed. Solving the problem of the handling of lengthy objects, while simultaneously gripping such objects at several points, is something of a priority and was taken into account. In this case, the task of the robot, in the general form, can be divided into three phases, viz., move-to-grip, where the robot moves towards the object, moving the object to a specified location, and precise manipulation along a prescribed path. The proposed approach allows the motion planning problem to be resolved in all three phases. Moreover, in the presented method, the series of constraints, resulting from the mechanical properties of the mechanism and the boundary constraints resulting from the task, was considered. The constraints imposed on the mechanical limits of joint angles, as well as the constraints connected with potential collisions among the robot, its environment, the object being handled, and the inner collisions between the robot arms were taken into account, in particular. Additionally, motions were planned in order to maximize the instantaneous manipulability measure, in order to avoid manipulator singularities during movement and to ensure the high dexterity of the robot at the end of each phase of a particular task. Such an approach allows the next phase to be performed without the necessity of reconfiguration. In order to solve the robot task, formulated in the above manner, the Jacobian pseudoinverse method at the acceleration level with secondary objective functions was used. The application of the Jacobian pseudoinverse matrix is a common approach [20–22] in resolving redundancy, due to its effectiveness and the possibility of being able to use computations in real time. The efficiency of the proposed algorithm was confirmed by computer simulation, involving a humanoid mobile robot, in its completion of three phases of the transportation of a lengthy object. The paper is organized as follows. Section 2 presents the mathematical models, both of the robot and of the object (Section 2.1). The formulation of the robot task and constraints are taken into account (Section 2.2), as well as the solution for the three phases of the robot task (Sections 2.3 and 2.4). The simulations performed are presented in Section 3, and their results are discussed in Section 4. A summary of the research undertaken and the future direction thereof is presented in Section 5. 2. Materials and Methods 2.1. Model of the System In the paper, a holonomic mobile humanoid manipulator, working in an industrial environment, was considered. The robot was inspired by the Rollin’ Justin robot [19], developed by the German Aerospace Center (DLR), which is made up of a humanoid dual-arm upper body, mounted on a mobile platform. In contrast to the original design, the robot proposed in this work was equipped with a holonomic (3, 0)-type platform with two pairs of Mecanum wheels and the arms with four DOF. The model of the humanoid manipulator on its holonomic platform is shown in Figure 1. As can be seen, the robot consists of four main components: • The 3DOF holonomic platform, which allows any position and orientation to be achieved in the XY-plane of the workspace; • The 3DOF torso with three revolute joints and an additional, passive revolute joint, placed at the end of the kinematic chain, ensuring that the chest is always kept orthogonally to the ground; • Two arms, each with 4DOF. Appl. Sci. 2021, 11, 6209 4 of 23 Figure 1. Model of the humanoid manipulator. In the work here presented, and for the sake of simplicity and in order to increase the readability of the presentation, the orientation of the end-effectors was not taken into account. Such an assumption does not affect the generality of the presented approach because the proposed method did not limit the number of arm joints, so it is possible to complement the arms with additional 3DOF grippers. The problem with planning a grasping action is beyond the scope of this work; however, it is the subject of ongoing research, and the results presented in such as [23] can be utilized when considering the humanoid manipulator in this paper. It was assumed that the humanoid robot consisted of a series of rigid bodies connected by rigid joints. In this case, the location of each link with respect to a previous link can be easily described by the location, i.e., the position and orientation of the local coordinate frame, attached rigidly to the chosen link, relative to the local coordinate frame attached to the previous link. In order to describe the position of the two end-effectors, i.e., the left and right arms of the robot, in the workspace, modified Denavit–Hartenberg (DH) notation [24] was used, and the parameters of the robot joints and links were determined and are shown in Table 1. Appl. Sci. 2021, 11, 6209 5 of 23 Table 1. DH parameters for the humanoid manipulator. Torso Left Arm Right Arm i a a d q i a a d q i a a d q i 1 i 1 i i i 1 i 1 i i i 1 i 1 i i 1 p/2 0 l q 1 p 0 l q 1 0 0 l q T T L L R R 1 1 1 1 1 1 2 p/2 0 0 q 2 p/2 0 0 q 2 p/2 0 0 q T L R 2 2 2 3 0 l 0 q 3 p/2 0 l q 3 p/2 0 l q T T L L R R 2 3 2 3 2 3 4 0 l 0 q 4 p/2 0 0 q 4 p/2 0 0 q T L R 3 T 4 4 l , l , l , l , l , l , l are the lengths of the humanoid links for the torso and the T T T L L R R 1 2 3 1 2 1 2 left and right arms, respectively. q , q , q are the joints angles of the torso. q = T T T 1 2 3 T pi/2 q q is the joint angle of the last joint of the torso and is used to compensate T T 2 3 the rotations of the two previous joints. q . . . q , q . . . q are the joint angles of the left L L R R 1 4 1 4 and right arms, respectively. The rotation axes of the independent joints of the torso (i.e., the axes for the first, second, and third joints) and all joint axes of left arm are shown in Figure 1; the rotation axes of the right arm are similar. As shown, the arms are located at the end of the torso, and their bases are rotated by an angle of b. The grippers are located at distances l and l from the fourth joints of the left and right arm, respectively. L R 3 3 The position and orientation of the holonomic platform is uniquely defined by three parameters: x and y determine the platform center location in the XY plane, and q describes the platform orientation. Finally, the configuration of the four main parts of the humanoid can be described by four vectors: q = [x, y, q] , q = q , q , q , P T T T T 1 2 3 T T q = q , q , q , q , q = q , q , q , q L L L L L R R R R R 1 2 3 4 1 2 3 4 while the configuration of the whole robot is described by the n-element vector of the generalized coordinates (in this case, n = 14). h i T T T T q = q , q , q , q . (1) P T L R Using the coordinate vector (1) and the parameters of the robot collected in Table 1, it is possible to determine the kinematic model of the system f(q), describing the positions of the end-effectors in the workspace, which can be written in compact form, as follows: f (q) p = f(q) = , (2) f (q) 2m where p 2 R is the vector with the positions of the humanoid end-effectors expressed in the workspace global frame, m is the dimension of the robot task, and f (q), f (q) are the L R functions describing the positions of the left and right end-effectors. The linear velocities of the end-effectors are given by differential kinematics as: p ˙ = J(q)q ˙ , (3) where p ˙ is the vector containing the linear velocities of the end-effectors, q ˙ is the vector of the generalized velocities of the robot joints, and J = ¶f(q)/¶q is the (2m n)-dimensional Jacobian matrix of the humanoid manipulator, which can be written as: J J 0 L,torso L,larm J(q) = , (4) J 0 J R,torso R,rarm where J = ¶f (q)/¶q , J = ¶f (q)/¶q , J = ¶f (q)/¶q , L,torso L T L,larm L L R,torso R T J = ¶f (q)/¶q . R,rarm R R Considering the transportation task, it is necessary to take into account the model of the object transported by the robot. In this work, it was assumed that the object was rigid Appl. Sci. 2021, 11, 6209 6 of 23 and connected to the end-effectors of the robot. In this case, the location of the object in the workspace can be described by the 6-element vector o: h i T T o = c , f , (5) where c and f are the 3-element vectors describing the position and orientation of the local frame in the workspace global frame. The method does not limit the choice of the origin of the object coordinate frame, so it can be attached to the object at any given point. 2.2. Formulation of the Problem From the perspective of Industry 4.0, the use of information from the integral sensors of the robot, as well as from the sensors located in its surroundings allows tasks that, formerly, had to be planned in detail to now be completed autonomously. The basic idea of a task-motion-planning system for the task of autonomous robot considered in this work is presented in Figure 2. Such a system consists of the Task Planner, which specifies the main goals for the robot based on information from the sensors, and the Motion Planner, which plans the robot motion, according to the goals from the Task Planner. Trajectories generated by the Motion Planner are transmitted to the Robotic System. In the presented work, it was assumed that the information from the Task Planner was known and only the subsequent phases of the transportation problem of the lengthy object, as represented by blue boxes in the diagram, remained to be considered. Mo on Planner grip points trajectory Move to grip object posi on trajectory Transport path trajectory Precision manipula on Figure 2. Basic idea of the task-motion-planning system. According to the idea of the Motion Planner, presented in Figure 2, the considered task of a humanoid mobile manipulator transporting an object in an industrial environment is divided into three phases (subtasks), solved separately: • Moving the end-effectors to the given location p starting from the current configura- tion q in order to approach the object to be moved; • Moving the object to the specified location o in the workspace; • Moving the object along a prescribed path j(s) in order to complete a precise opera- tion, such as an assembly task. The first task was to move the end-effectors of the humanoid manipulator to the grip points specified by the Task Planner. This is a common, point-to-point task, in this case, to be undertaken by two end-effectors simultaneously. It can be expressed as: f (q(T)) L L, f f(q(T)) = = = p , (6) f (q(T)) p R R, f where T is the final moment of the motion and p is the 2m-element vector containing the final position of the left p and the right p end-effectors of the robot. L, f R, f Task Planner Robo c System Appl. Sci. 2021, 11, 6209 7 of 23 In the second and third task, it is necessary to take into account the object being transported by the robot. In the presented work, such tasks were formulated as the movement of the object in the workspace, where the motion of the robot was forced by the set of constraints resulting from the closed kinematic chain established by the robot arms and the object being held by them. In both cases, the robot began the movement from the initial configuration q holding the object; however, in the second task, achieving the desired final location was the only goal of any importance, while in the third task, the movement of the object was determined by the prescribed path j(s). In the second task, starting from the configuration achieved in the first one, the robot moved the object being transported to the position specified by the Task Planner. According to the above considerations, this task can be described as: o(T) = o . (7) Finally, the humanoid manipulator started from the final configuration of the previous task and carried out the precise manipulation of the object, according to the path determined by the Task Planner. Such a task can be written as follows: o(t) j(s) = 0, (8) where s is the scaling parameter, which depends on time t, i.e., s = s(t), s 2 [0, 1]. In both of the above cases, additional constraints resulting from grasping should be considered. The humanoid manipulator held the object at two contact points, and the hard finger model [25] of contact was assumed in this paper. This assumption means that the linear components of the velocities were transmitted through the contact point, while, on the other hand, the angular components were not. Introducing the function p (o), describing the locations of the contact points, the grasp limitations can be expressed as geometric and kinematic constraints as follows: f(q(t)) = p (o(t)) (9) J q ˙ (t) = p ˙ (o(t)), where p ˙ = dp /dt describes the linear velocities of the contact points. C C The practical processes that are accomplished by the industrial robots impose some conditions at the beginning and the end of the trajectory. It is natural to assume that at the initial and final moments of the performance of the task, the velocities of the manipulator equal zero: q ˙ (0) = 0, q ˙ (T) = 0. (10) During the completion of the task, the robot should maintain the limitations resulting from its construction and the range of movements permitted in the individual joints. Introducing the scalar function C , defining the algebraic distance of a given configuration from the range permitted, such mechanical constraints can be expressed in the form of a set of L inequalities in the form: 8t 2 [0, T] C (q(t)) 0, i = 1, . . . , L . (11) The space in which the robot operates may contain some equipment that should be treated as obstacles and must be taken into account during the motion planning. Intro- ducing the scalar functions C and C , which describe the algebraic distances from the Or Oo obstacles to the robot and object, respectively, the collision avoidance conditions can be written as sets of L for the robot and L for the object inequalities in the form: Or Oo 8t 2 [0, T] C (q(t)) 0, i = 1, . . . , L (12) Or Or 8t 2 [0, T] C (o(t)) 0, i = 1, . . . , L (13) Oo Oo Appl. Sci. 2021, 11, 6209 8 of 23 In practice, the configuration of the manipulator joints should be far away from singular configurations. In order to achieve this goal, the Yoshikawa manipulability measure [26], which describes the distance to singular configurations, may be maximized. The Yoshikawa manipulability index m(q) is proportional to the volume of the velocity ellipsoid. The semi-axes of this ellipsoid coincide with the eigenvectors of the matrix J J , the lengths of these semi-axes being equal to the square roots of the eigenvalues of J J . Since the product of the lengths of the semi-axes is proportional to the volume of the ellipsoid and the product of the eigenvalues of the matrix is equal to the determinant of the matrix, the manipulability measure may be calculated as: m(q) = det(J J ). It seems that in the case of a dual-arm humanoid manipulator, the avoidance of singular configurations of the robot arms [27] is important, so the following manipulability measure was taken into account: m(q) = det(J J ), (14) arms arms J 0 L,larm where J = consists of the 8 last columns of the matrix (4). arms 0 J R,rarm 2.3. Solution: Primary Tasks According to the analysis from the previous section, the transportation task, con- sidered in this work, consisted of three steps: moving the robot to the object, moving the object to a specific location, and moving the object along a prescribed path. The first step is a point-to-point task described by the dependency (6). In the presented approach, in order to eliminate, known from the literature, the drift effect resulting in a trajectory error increasing in time, a feedback loop algorithm, correcting the robot movement on the basis of the information from the sensors, was proposed. For this purpose, the following end-effector error function e(t) between the current and fixed final end-effectors positions was introduced: e(t) = f(q) p . (15) Using this function, the task of the robot can be presented in a more general form as: lim e(t) = 0, lim e ˙(t) = 0. (16) t!¥ t!¥ Such a formulation of the task allows reaching the final positions of the end-effectors, in a finite time, with any accuracy defined by the needs of the realized process. Moreover, in the proposed solution, the convergence rate and resulting final time were dependent on the gain coefficients and these can be tuned by their appropriate selection. In order to solve the trajectory planning problem of the humanoid manipulator, the following system of differential equations was introduced: e ¨(q, q ˙ , q ¨ ) + K e ˙(q, q ˙ ) + K e(q) = 0, (17) 1 2m 1 2m where K = diag(K , . . . , K ) and K = diag(K , . . . , K ) are (2m 2m) diagonal gain d p p d d matrices with positive coefficients, which determine the convergence rate of the solution of Equation (17). The system (17) is a homogeneous differential equation with constant coefficients. In order to find the solution, 4m consistent dependencies should be given. They can be determined from the error function (15) for t = 0, taking into account the initial condition (10). Moreover, using the Lyapunov stability theory and an approach similar to that presented in [28] for nonholonomic mobile manipulators, it is possible to show that the system is asymptotically stable, so the end-effector error e(t) and its derivative e ˙(t) converge to zero, guaranteeing fulfillment of the conditions (16). The convergence rate depends on the gain coefficients of matrices K and K . d p Appl. Sci. 2021, 11, 6209 9 of 23 Determining the components e ¨(q, q ˙ , q ¨ ), e ˙(q, q ˙ ) as: e ¨(q, q ˙ , q ¨ ) = J(q)q ˙ + J(q)q ¨ , (18) e ˙(q, q ˙ ) = J(q)q ˙ and substituting them into System (17), the dependency that allows determining the trajectory of the humanoid can be written as: J(q)q ¨ + uu(q, q ˙ ) = 0, (19) where uu(q, q ˙ ) = J(q)q ˙ + K J(q)q ˙ + K (f(q) p ). d f The dependency (19) formulates the primary task of the humanoid manipulator, which grasps the object in the locations specified by vector p . Due to the high redundancy of the humanoid (n > 2m), the Jacobian matrix J(q) is rectangular (the number of its columns is greater than the number of its rows), and there is an infinite number of trajectories satisfying the condition (19). In order to find solution q ¨ , the task can be formulated as a quadratic optimization problem with linear constraints, which leads to the determination of a trajectory minimizing a certain cost. Using the Moore–Penrose pseudoinverse of † T T 1 matrix J(q), J (q) = J (J J ) , the optimal solution minimizing the acceleration norm of a mobile robot kqk and satisfying the linear constraints (19) can be determined as: q ¨ (t) = J (q) uu(q, q ˙ ). (20) The dependency (20) describes the trajectory realizing the primary task of a humanoid robot. The additional constraints imposed on the robot motion, as presented in the previous section, will be introduced as secondary objectives in the next section. In the second step of the transportation task, the robot moves the object to the desired location o . In order to solve this task, in the presented work, the method of trajectory planning for the object, guaranteeing fulfilling the dependency (7), while simultaneously determining suitable robot movement, was proposed. In this case, the object was considered as the single 6DOF joint, which can achieve any position and orientation in a 3-dimensional workspace, and its movement was planned using an idea similar to that used in a previous task. For this purpose, the following function e(t) describing the error between the current and desired fixed location of the object was introduced: e(t) = o(t) o (21) and this task can be formulated as: lim e(t) = 0, lim e ˙(t) = 0. (22) t!¥ t!¥ Similarly, as in the case of the trajectory planning in the move-to-grip phase (22), such a formulation of the object task would allow achieving the desired location of the transported object with any accuracy in finite time. Introducing the (6 6) diagonal matrices with positive coefficientsKKK andKKK , the d p trajectory of the object can be determined from the following equation: e ¨(o, o ˙ , o ¨) +KKK e ˙(o, o ˙ ) +KKK e(o) = 0 (23) with respect to the initial conditions determined from (21) for t = 0 and zero initial velocity of the object resulting from the lower condition (9) and (10). The stability of Equation (23) is assured by the assumed form of the gain matrices, guaranteeing the fulfillment conditions (22). Finally, similar to the previous task, after determining e ¨(o, o ˙ , o ¨), e ˙(o, o ˙ ) and using the simple algebra, the trajectory of the object can be written as: o ¨(t) = KKK o ˙ KKK (o o ). (24) d p f Appl. Sci. 2021, 11, 6209 10 of 23 Taking into account that the transported object is rigid and that the location of the grip points in the object frame is known (this was determined by the Task Planner), the location of the grip points in each time instant can be calculated using function p (o). According to the proposed approach, the trajectory of the contact points, determined in this way, defines the trajectory tracking task to be performed by the robot in this phase. In order to ensure suitable robotic movement, enabling the desired location of the object determined from (24) to be reached, the grasp limitations (9) must be satisfied, so the primary task of the robot, after introducing end-effector error e(t): e(t) = f(q) p (o(t)), (25) may be formulated as: lim e(t) = 0, lim e ˙(t) = 0. (26) t!¥ t!¥ The trajectory of the robot, as in the case of the point-to-point task, can be deter- mined from differential Equation (17) for a new form of error function introduced by the dependency (25) as follows: q ¨ = J (q) uuu(q, q ˙ ). (27) where uuu(q, q ˙ ) = J(q)q ˙ + K (J(q)q ˙ p ˙ ) + K (f(q) p ) p ¨ . d C C C Due to the asymptotic stability of the differential Equation (17) and assuming that the initial condition (10) and the grasp limitations (9) at the initial moment of the motion are fulfilled (the robot is motionless and holds the object at contact points p , so e(0) = 0 and e ˙(0) = 0), the error function e(t) = 0 and e ˙(t) = 0 at each instant, which guarantees the fulfillment of the grasp limitations while the robot is moving. The task realized in the third step is similar to the second one, but the position and orientation of the object during the whole movement is given by the path f(s), preplanned earlier by the Task Planner, as shown in Figure 2. It was assumed that the path was given in parametric form as a function of scaling parameter s, dependent on time. In this case, the error function describing the distance between the current location of the object and the location determined by the path, extended by an additional element responsible for tracing the entire path, was introduced as follows: e(t) o(t) f(s) y(t) = = . (28) x(t) s(t) 1 Using this function, the third task (8) can be described in a more general form as: lim y(t) = 0, lim y(t) = 0. (29) t!¥ t!¥ Similarly, as in the first and second phase of the transportation task, such a formulation of the path tracking problem will allow achieving the final location of the object path with any accuracy. Moreover, if the object is not positioned precisely at the beginning of the path, the tracking error y(t) will converge to zero during the object movement. Finally, the object trajectory and the path parameterization can be determined using the following system of differential equations: K K e ¨(o, o ˙ , o ¨ , s, s ˙, s ¨) +KK e ˙(o, o ˙ , s, s ˙) +KK e(o, s) = 0 (30) ¨ ˙ x(s ¨) + k x(s ˙) + k x(s) = 0. d p where the gain matricesKKK andKKK were accepted in the same way as in the second task and ensure the asymptotic stability of the first equation, and k and k are the positive d p scalars, which provide the asymptotic stability of the second equation. The property of the stability of Equation (30) implies the fulfillment of the conditions (29); moreover, if in the Appl. Sci. 2021, 11, 6209 11 of 23 initial moment of motion, y(0) = 0 and y(0) = 0 (the object is located at the beginning of the path, and s(0) = 0), the object will be moved along the path for the whole process. Finally, after simple calculations, the trajectory of the object o(t) and the path parame- terization s(t) can be determined from (30), in a manner similar to the previous tasks, using the initial conditions obtained from (28) for t = 0 as follows: K K o ¨(t) = KK o ˙ KK (o f(s)) (31) s ¨(t) = k s ˙