A novel QP-based kinematic redundancy resolution method with joint constraints satisfaction

Kinematically redundant manipulators are advantageous for their increased dexterity and ability to fulfill some secondary requirements along with their primary task to follow the prescribed trajectory. The redundancy results in a non-trivial inverse kinematics problem (IK). Standard methods of redundancy resolution are based on the pseudoinverse of the Jacobian matrix of the manipulator. The excessive degrees of freedom are utilized to perform secondary tasks that are projected onto the null space of the Jacobian matrix. However, the joint constraints satisfaction cannot be easily ensured in this way—even though methods that account for the joint position limits are known, the constraints for joint velocities and especially accelerations are not straightforward to include. In this paper, a novel redundancy resolution method based on a less common quadratic programming (QP) approach is described. The proposed velocity-level IK method allows fulfilment of the joint constraints at the position, velocity, and acceleration levels. In the derived formulas, accelerations instead of the usual velocities are used—the discretized joint state equations allow the use of joint accelerations as decision variables in the QP problem. The developed algorithm is investigated in a series of numerical tests in which the kinematics of the KUKA LWR4+ redundant 7-DOF manipulator is exploited. The newly elaborated QP-based IK method is firstly compared with the classic pseudoinverse-based approach and then tested for its ability to keep the joint accelerations within the prescribed bounds. The prospects of the proposed approach are discussed in the concluding section.


I. INTRODUCTION
A TYPICAL industrial robot has as many degrees of freedom as necessary for a task it was chosen for. However, to work in human-centered unstructured environments, a robotic manipulator shall have a versatile design. As can be observed in a human arm, a redundant structure increases dexterity. Similarly, redundant manipulators can easily handle singularities, avoid obstacles or keep their joints away from the limits [1], [2].
Usually, the desired end effector trajectory is planned in the Cartesian space, whereas the robot's controller requires the joint space trajectory. Therefore, the inverse kinematics problem has to be solved. Since redundant robots have more degrees of freedom than the end effector trajectory, the solution of inverse kinematics (IK) is not trivial. Standard methods are based on the pseudoinverse of the Jacobian matrix of the manipulator [1], [2]. The redundant degrees of freedom can be utilized to perform tasks additional to the end effector trajectory tracking. These secondary tasks are projected onto the null space of the Jacobian matrix [3]- [7]. The classic methods of redundancy resolution are discussed in more detail in Sec. II of this article, in order to establish a ground for their comparison with the newly proposed approach.
However, the joint constraints satisfaction cannot be ensured in the pseudoinverse-based IK-attaining this goal needs additional effort. In [8], [9], fulfilment of the joint position limits is treated as the main task; moreover, an enhancement consisting of modification of the Jacobian matrix singular values so that it never loses rank is proposed. To guarantee the continuity of the solution, the pseudoinverse operator from [5], [10] is utilized. Another approach for avoiding joint limits, based on a weighted least norm solution and minimizing unnecessary self-motion, is presented in [11]. In [12], an iterative algorithm, exploiting constraints prioritization, is used to clamp the joint space motion in the vicinity of the joint limits. Another iterative method is proposed in [13] to construct a null space projection operator, which-after ensuring its continuity by introducing activation factors-forms a basis for obtaining joint velocities.
The aforementioned methods are more complex than the basic pseudoinverse-based IK. Even though they account for the joint position limits, the higher order constraints are not straightforward to include. To remedy this problem, a method based on a quadratic programming (QP) formulation is proposed in this paper. Although the QP formulation of IK is known-it can be used to derive the pseudoinverse-based IK [2], [14]-this paper presents an important enhancement. The scientific novelty of this work is the proposition of a velocity-level IK method that allows the fulfilment of the joint acceleration constraints together with the velocity-and position-level constraints. The elements of the goal function, the Hessian matrix and other necessary quantities, are formulated in the form that uses accelerations instead of the usual velocities, as in [2], [14]. The discretization of the joint state equations allows to use the joint accelerations as the decision variables in the QP-based IK.
The remainder of this paper is organized as follows. In Section II, classic methods of kinematic redundancy resolution are briefly recapitulated. Section III presents a novel method of enforcing joint limits on acceleration, velocity and position levels within the framework of the QP approach to the IK solution. Then, in Section IV, the performance of the proposed method is verified and compared with the classic approach. Finally, the conclusions and outlook of further works are given in Section V.

A. PRELIMINARIES
Let n be the dimension of the manipulator's joint space, and m be the dimension of the task space. The task space variables in the vector x ∈ R m are related to the joint space variables q ∈ R n by the equation: where f (q) → R m is a function describing the forward kinematics of the manipulator. For a full 6 degree-of-freedom task, the vector x ∈ R 6 can consist of the end effector's position r ∈ R 3 and orientation φ ∈ R 3 (given for example by Euler angles): whereas the forward kinematics function f (q) can be written as: where f r (q) describes the position and f φ (q) describes the orientation of the end effector. In a general case, the end effector task might have less degrees of freedom (m < 6), and not all elements of r and φ might be necessary in x. Accordingly, not all elements of f (q) given by (3) would be necessary in such a case.
Differentiating (1) with respect to time leads to the follow- where: • J a = J a (q) ∈ R m×n is the analytical task Jacobian of the manipulator, • m < n for the manipulator to be redundant for a given task of the size m, •q ∈ R n is the vector of joint velocities, •ẋ ∈ R m is the vector of time derivatives of the task variables x. The analytical task Jacobian matrix J a is computed as [1]: In practice, it is often more convenient to use the geometric Jacobian J = J(q) ∈ R m×n , since it describes the relationship between the joint velocitiesq and task velocites instead of relationship betweenq andẋ. Again, for a full 6 degree-of-freedom task, the vector v ∈ R 6 can consist of the end effector's translational velocityṙ ∈ R 3 and angular velocity ω ∈ and the angular velocity ω can be expressed as: where T(φ) is a transformation matrix that corresponds to the used method of parameterization of orientation. However, in a case of m < 6 not all elements ofṙ and ω might be necessary in v. For example, in a pure translational end effector task, the task variables vector x and the task velocity vector v are given by x = r and v =ṙ, respectively. It is worth noting that in such a case the analytical and geometric Jacobians are identical: J a = J. On the other hand, when m = 6, there exists a following relationship between J and J a : The well-known recursive algorithms for computing the geometric Jacobian J can be found in [1], [2]. Moreover: • R(J) ⊆ R m is the range space of J(q), and it contains the end effector task velocities v that can be generated by the joint velocitiesq for a given configuration q, Woliński, Wojtyra: A novel QP-based kinematic redundancy resolution method with joint constraints satisfaction • N (J) ⊆ R n is the null space of J(q), and it contains the joint velocitiesq that do not produce any task velocity, for a given configuration q, • ρ ≤ m is the rank of J, • dim (R(J)) = ρ, • dim (N (J)) = n − ρ, and, because of the redundancy, dim (N (J)) > 0, • dim (R(J)) + dim (N (J)) = n . For redundant manipulators, the Jacobian J(q) is not square (because m < n), and the straightforward solution to the inverse kinematics problem: is not available. Instead, to solve Eq. (6) with respect toq, a Moore-Penrose pseudoinverse J # of the Jacobian matrix J might be used [1], [2]:q Assuming that all of the manipulator joints are of the same type (e.g., rotational or translational), the solution (11) minimizes the Euclidean norm of the joint velocity vector q . However, other solutions are possible. In particular, the ability of the redundant manipulator to perform self motionsenabled by the existence of N (J)-might be utilized to achieve additional objectives along the main task, given by v. Therefore, the minimum norm solution (11) can be expanded to the following form [1], [2], [15], [16]: whereq N S ∈ N (J) is the null space velocity. In other words, q N S does not affect the end effector task velocity v, since: Usually, the null space velocity is expressed as [1], [2]: whereq JS is an arbitrarily chosen vector that represents the constraint of the additional task specified directly in the joint space, and P is the matrix which projects the vectorq JS onto the null space of the Jacobian N (J) [1], [2], [17]: and JP = 0. Substituting (15) and (14) into (12) results in: which is the solution to the inverse kinematics problem, known as the redundancy resolution at the velocity level.

B. SECONDARY TASKS
One of the methods to select the vectorq JS for the solution (16) is to employ the local optimization [1], [2], [16], [18]: where H(q) is a scalar configuration-dependent objective function, ∇ q H(q) is its gradient and k H is a scalar gain coefficient. Since (17) is projected onto the null space of the Jacobian, this approach is called the projected gradient method. It can be used for singularity avoidance [1], [2], increasing the dynamic manipulability [19], obstacle avoidance [2], [20], or keeping the joints close to the middle of their range [1], [2]. The ability of the redundant manipulator to avoid the collisions with obstacles in the workspace can be also utilized by adding a task defined by its Jacobian matrix and velocity vector [21], [22]. However, specifying multiple additional tasks alongside the main one might result in conflicting task situations. Therefore, a task priority strategy is needed to ensure that each lower priority task is satisfied only in the null space of the higher priority tasks [3], [4], [6], [22], [23].
The multiple tasks are usually defined as: where v i ∈ R mi is the i-th task velocity, J i ∈ R mi×n is the i-th task Jacobian, l is the number of tasks, m i is the i-th task dimension, and l i=1 m i ≤ n. The order of task priority is decreasing, i.e., the task i + 1 has a lower priority than the task i.
A recursive solution for (18) is given in [3] (called in [6] a standard method): for i = 1, . . . , l, and whereq l is the solution accounting for all the tasks, whileq 0 = 0 and P 0 A = I. The matrix P i A [3], [6]: is the projector onto the null space of the augmented Jacobian of the first i tasks:

C. JOINT SPACE SOLUTION
To obtain the joint coordinates in any given moment t, the solution (16) has to be integrated over time (starting from t 0 ): However, for implementation in the control system, usually equations in discrete form are required. The integral (22) VOLUME 4, 2016 can be approximated by using the forward rectangular rule, resulting in the explicit Euler method: which can be expressed recursively as: where q 0 = q(t 0 ) is the initial joint configuration, q k = q(t k ) is the sample at time instant t k , ∆t is the time step, and k = 1, 2, . . . , N . The joint velocityq k is just a discrete form of (16):q , and P(q k ) (recall (15)): It should be noted that the numerical integration introduces some error in each step which accumulates over time. The closed-loop inverse kinematics (CLIK) approach overcomes this problem by treating the inverse kinematics as a feedback control problem and including the end effector error in the solution [1], [2], [9], [24]- [30].
Up to this point, the end effector velocity v k was treated as the desired velocity: However, in the CLIK solution, the end effector velocity v k takes the following form: where K ∈ R m×m is a positive definite gain matrix, and e k is the end effector trajectory error in the k-th step: where r d,k is the desired end effector position in the k-th step, and f r (q k ) is the position part of the forward kinematics function f (q k ) (given by (3)) in the k-th step. The orientation error e φ,k is much harder to define and depends on the chosen orientation representation. A couple of ways of expressing the orientation error e φ,k are presented in [2]. It should also be noted that in many works (28) is simplified to a form [9], [10], [24]- [27], [29]: where K > 0 is a scalar gain coefficient. Stability of the CLIK algorithms based on (30) and using the explicit integration methods was investigated in [27], leading to the following condition for K:

III. PROPOSED METHOD A. JOINT STATE EQUATIONS
As can be seen in Sec. II, the pseudoinverse-based IK does not account for joint constraints-in particular for velocity and acceleration bounds. To directly bound the acceleration q k at each time t k , the joint state equations have to be formulated at the acceleration level: Equation (32) can be written as: where the state z k ∈ R 2n is: and the joint acceleration is the control vector: u k =q k . The state matrices A ∈ R 2n×2n and B ∈ R 2n×n are defined as: and are constant. Equation (33) can be divided into two parts: where the matrices A 0 , B 0 , A 1 and B 1 are: and I ∈ R n×n and 0 ∈ R n×n . The relationship between the joint and end effector velocity vectors-given by (6)-can be written for step k + 1 as: Combining (39) with (37), the following result is obtained: where: Equation (40) is nonlinear in terms of u k -because of J k+1 given by (41). It can be used to formulate and solve a nonlinear optimization problem; some examples of nonlinear optimization utilization in solving IK include [20], [31], [32]. On the other hand, an approximation of J k+1 based on the solution from the previous step can be used to formulate a linear kinematic constraint. In that approach, an whereû k = u * k−1 is the solution from the previous step. Then, by using (42), J k+1 can be approximated asĴ k+1 : Using (43) in (40), a linear equation in terms of u k is obtained:Ĵ In [33], a similar approach-utilizing the previous step solution to simplify the kinematic equations-was successfully applied to the joint space trajectory generation.

B. JOINT CONSTRAINTS
The equality constraint (44) describes the end effector trajectory tracking task. The novel IK formulation requires also the inequality constraints to represent the joint position (q min and q max ), velocity (q min andq max ) and acceleration limits (q min andq max ). Although the constant acceleration bounds represent purely kinematic limits, they are often used in literature as approximation of torque constraints [14], [34]- [38]. No bounds on acceleration might cause the discontinuities in joint velocities, as observed in [14]. However, in [14], the joint acceleration constraints are included indirectly in additional velocity bounds.
The joint position constraints can be written as: where the operator ≤ used with vectors means an elementwise operation. The joint velocity bounds are: and the acceleration limits can be included as: Another fact to bear in mind is that the robot joints cannot stop in an instant. Applying the maximum deceleration q min,j < 0 to the j-th joint for some time t ≥ t k results in the following position and velocity: .
When the j-th joint reaches its limit q max,j , its velocity should be less or equal to zero. Suppose this event happens at some t = t * > t k : From (49), it follows that the j-th joint velocity at t k is upper bounded by [14], [37], [38]: Similarly, from the case of applying the maximum accelerationq max,j > 0 to the j-th joint to avoid crossing the q min,j limit, it follows that the lower velocity bound at t k is [14], [37], [38]:q The joint constraints (45)-(51) can also be transformed into a form dependent on u k . In that regard, the joint position limits (45) become: while the joint velocity bounds (46) are now: and the acceleration limits (47) can be included as: Finally, the constraints (50) and (51) forq k+1 can be transformed, by using (32) and (42), into: where: for j = 1, . . . , n.

C. SECONDARY TASKS IN THE GOAL FUNCTION
As described in section II-B, a redundant manipulator can handle multiple tasks thanks to the utilization of the null space projection. In this section, a different approach is presented, based on a multiobjective optimization formalism [39]. The secondary tasks can be thought of as parts of a multiobjective goal function to be minimized: where l is the number of null space tasks, W i ∈ R mi×mi are positive-definite weight matrices, and m i is the size of the i-th task. By setting the different values of weights, the task hierarchy can be defined, with the most important task having the highest weight [39]. In general, deciding on the hierarchy of tasks of various kinds might not be trivial. The task Jacobian J i ∈ R mi×n and task velocity v i ∈ R mi can represent any secondary task defined as (18). It is worth pointing out that these tasks usually do not directly depend on time but rather on the joint configuration q. An example of such a task is the obstacle avoidance [21], [22]. The joint space tasks can be implemented by setting J i = I and v i =q JS,i , whereq JS,i is given by (17).
As described above, the secondary tasks can be defined in different ways, each having different units-for example, VOLUME 4, 2016 joint space task velocity will have units of rad s , whereas the Cartesian space: m s . Since in (57) the terms are all summed, the units shall be consistent. Therefore, the role of W i matrices is not only to set the task priorities with weights but also to ensure the consistency of the units. To this end, the units of elements of W i can be chosen in such a way that each term in the sum in (57) becomes unitless.
Since the equality constraint (44) and inequality constraints (52)- (55) are formulated in terms of u k , the goal function in (57) also has to be redefined. A new minimization problem-with the joint acceleration vector replacing the joint velocity as the optimization variable-can be written as: where the joint velocity seen in (57) was replaced byq k+1 from (37). The termsĴ i k+1 andv i k+1 are the approximations of the i-th task Jacobian and velocity, respectively, computed for step k + 1 with the use ofq k+1 , given by (42). If the task velocity v i does not depend on the joint configuration q but rather on time, . The cost function in (58) includes the additional goal of minimizing the control action u k , introduced with the weight γ l+1 > 0. It should be pointed out that the joint velocity minimization task is still possible by defining the i-th task as: J i k+1 = I,v i k+1 = 0 and W i = γ i I, where γ i > 0. Equation (58) can be further regrouped into: The constant terms of (58) were omitted in (59), because they do not affect the optimization process.

D. QUADRATIC PROGRAMMING FORMULATION OF THE INVERSE KINEMATICS
Gathering the equations derived in sections III-A, III-B, and III-C, the QP formulation of IK can be expressed as: where the vector of optimization variables ξ ∈ R n is: whereas the components of the Hessian matrix H ∈ R n×n and gradient vector h ∈ R n of the goal function are, respectively (compare with (59)): and: It should be pointed out that due to the joint acceleration being the decision variable (61), the matrices H and h, given by (62) and (63), respectively, take a different form than in [2], [14]. Based on (44), the matrix of the equality constraints set Φ eq ∈ R m×n is: whereas the equality constraints vector b eq ∈ R m is: Based on (52)- (55), the matrix of the inequality constraints set Φ iq ∈ R 4n×n is: whereas the vector of the lower bounds b iq min ∈ R 4n is: and the vector of the upper bounds b iq max ∈ R 4n is: The solution of the IK problem is obtained for the time interval t 0 , t end , with a given time step ∆t, using Algorithm 1.
Lastly, to include the CLIK in the QP formulation (60), the end effector velocity v k+1 can be defined similarly as in (30): where the end effector errorê k+1 is a function of x d,k+1 and f (q k+1 ), similarly as in (29). For a pure translational task, it is:ê

E. DISCUSSION
Obviously, the idea of the QP formulation of the IK problem is not new. The pseudoinverse-based solution (11) can be obtained analytically by minimizing the quadratic goal function
An efficient QP-based method is proposed in [40]. It can be divided into two steps. First, the reduced joint velocityq p is obtained from (6) where the Jacobian J is replaced with a square matrix J p with only linearly independent columns This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/ACCESS.2022.3167403, IEEE Access Woliński, Wojtyra: A novel QP-based kinematic redundancy resolution method with joint constraints satisfaction Algorithm 1 QP IK solver (Initialization) Set t := t 0 ,û k := 0, z k := q(t 0 ) Tq (t 0 ) T T Set the joint limits q min , q max ,q min ,q max ,q min ,q max Compute constant matrices A (35), B (36), and A 0 , B 0 , (43) Obtain v k+1 from the end effector trajectory generator If secondary tasks are present, obtainĴ i k+1 andv i k+1 Formulate the QP problem (60) Set the initial guess ξ init :=û k Solve the QP problem (60) and obtain ξ * Set u k := ξ * Compute new state z k+1 := Az k + Bu k (33) Setû k+1 := u k Set t := t + ∆t Set k := k + 1 end while of J. Then, the QP problem is formulated to obtain the free variables of the joint velocity which are necessary to compute the full vectorq; conveniently, the equality constraint (6) is no longer needed, the unequality constraints for joint positions and velocities are included, and the number of the optimization variables is reduced to the number of redundant degrees of freedom n − m. However, calculating the partial solution to (6) and combining it with the optimal solution to a reduced QP problem might no longer be necessary-due to the abundance of computing power of modern computers, a full QP problem for a typical 7-degree-of-freedom manipulator can be easily solved.
In [41], the variable joint velocity limits in the QP IK problem for redundant robots are explored. It should be noted, however, that these constraints are derived for one specific manipulator. Meanwhile, the constraints (50) and (51) (or (55) and (56)) are general for manipulators with revolute joints.
The QP formulation of IK problem is not limited to manipulators. It can also be used in mobile robots such as humanoids [42] or hexapods [43].
All of the discussed methods implement constraints only on the joint positions q and velocitiesq. The main advantage of the proposed method is that it directly includes the joint acceleration constraints. A QP-based method that constrains the joint accelerations is also proposed in [44]; however, the inverse kinematics problem is formulated at the acceleration level. On the other hand, in our method, even though the joint accelerationq (or u) is the optimization variable in (60), the manipulator kinematics is defined at the velocity level, represented by the constraint (6).
The different subtasks are weighted in the goal function which simplifies the algorithm; however, in such a formulation, a tradeoff between secondary tasks might occur. In [42], a sequential QP (SQP) formulation is proposed that also handles inequality subtasks (e.g. keeping a part of the manipulator below or above a specified height). A modification of Algorithm 1 will be explored in the future to allow for prioritizing the subtasks in the SQP framework.

A. SIMULATION SETUP
To compare the IK solver proposed in Sec. III with the classic pseudoinverse solution described in Sec. II, several numerical simulations were performed. The test subject was a kinematic model of the KUKA LWR 4+ 7-degree-of-freedom manipulator [45]- [47]. The results of four simulations are presented in the next sections, whereas this section is devoted to the description of the simulation setup. The local coordinate frames were attached to the links of the LWR 4+ manipulator using the modified Denavit-Hartenberg parameters [48], shown in Tab. 1, whereas the end effector position in the last link frame was set as: r In each scenario, the main task consisted of tracking the desired end effector position, i.e., a 3-degree-of-freedom task. Each trajectory consisted of several linear segments and was generated with the method described in detail in Appendix A.
The time step of the simulation was set as ∆t = 0.005 s. The classic IK solution was obtained with (24) and (25), and the gain for the CLIK in (30) was K = 100 1 s . In the proposed approach, the Algorithm 1 was used, and the CLIK gain in (69) was set to K = 50 1 s . The simulations were perfromed in MATLAB R2020b on a computer with the Intel(R) Core(TM) i7-6500U CPU with two 2.6 GHz cores and 16 GB RAM. At each time step, the solution to (60) was obtained by MATLAB function quadprog, utilizing the active-set algorithm with default settings [49].

B. SCENARIO 1A
The start configuration was equal to: corresponding to the end effector position X 0 , and then the path continued through the points X 1 , X 2 , X 3 , X 4 , and again X 1 , shown in Table 2.
The resulting path consisted of 5 linear segments, as shown in Fig. 1. The time to traverse each segment was set equal VOLUME 4, 2016 to T 1 = 1.35 s, T 2 = 1.5 s, T 3 = 1.65 s, T 4 = 1.5 s, and T 5 = 1.9 s, resulting in total motion time t end = 7.9 seconds. The segment times were chosen in such a way that shows the limits of the classic IK approach. The trajectory generation is described in detail in Appendix A.
To account for the joint velocity minimization in the proposed method, there was one additional task (l = 1) defined in (57) as J 1 = I and v 1 = 0 with the weight matrix W 1 = γ 1 I and γ 1 = 10 6 s 2 . Correspondingly, the necessary components of (62) and (63) were set toĴ i k+1 = I and v i k+1 = 0 for each simulation step k. There was no joint acceleration minimization task, and therefore γ 2 = 0. No joint constraints were set.
As can be seen in Fig. 2, both solutions are very closethe maximum difference is in the order of a hundredth of a degree. The norm of the end effector error, computed as: where r d is the desired end effector trajectory and f r (q) describes the forward kinematics, is shown in Fig. 3. The maximum error (71) for the classic method is e = 5.62 · 10 −5 m, whereas for the proposed method it is e = 1.67 · 10 −6 m. The average computation time in each step is 0.14 ms for the classic IK and 1.5 ms for the proposed method.
The results show that the proposed method behaves correctly and provides the solution similar to the pseudoinversebased approach.

C. SCENARIO 1B
In this simulation, the setup from Scenario 1A (Sec. IV-B) was repeated. This time, however, the existence of the following joint constraints was considered: Additionaly, the joint acceleration minimization task was included in the proposed method with a weight set to γ 2 = 10 s 4 . The joint velocity minimimization was still active, with the same weight as in Sec. IV-B, namely γ 1 = 10 6 s 2 .
As can be seen in Fig. 4, the joint positions are within their bounds in both solutions. The same is true for joint velocities, shown in Fig. 5. However, at the acceleration level, the constraints for the 2nd and 4th joint are violated in the classic IK solution, as pictured in Fig. 6. At the same time, the proposed method fulfills the constraints-it is clearly visible in Fig. 6 that the accelerations saturate at their limits.
The maximum end effector error (71) for the proposed method is the same as in the case without the joint limits (i.e., Scenario 1A, Sec. IV-B), namely e = 1.67 · 10 −6 m. The average computation time in each step is 0.14 ms for the classic IK and 1.49 ms for the proposed method.

D. SCENARIO 2
The start configuration q 0 was chosen as: corresponding to the end effector position X 0 . From X 0 , the path continued through the points X 1 , X 2 , X 3 , X 4 , and again X 1 , shown in Table 3. x, y, z (m) The resulting path consisted of 5 linear segments, as shown in Fig. 7. The time to traverse each segment was set equal to T 1 = 2 s, T 2 = 1.5 s, T 3 = 1.2 s, T 4 = 1.2 s, and T 5 = 1.2 s, resulting in total motion time t end = 7.1 seconds. The segment times were chosen in such a way that shows the limits of the classic IK approach. The trajectory generation is described in detail in Appendix A.
The joint limits were set as follows: For the proposed method, the joint velocity minimization and joint acceleration minimization tasks were active with the same weights as in Sec. IV-C: γ 1 = 10 6 s 2 and γ 2 = 10 s 4 .
The joint positions are shown in Fig. 8. In the classic IK solution, the 1st joint exceeds its limit, whereas the proposed method satisfies the position constraints. The same happens at the velocity level, shown in Fig. 9. The joint accelerations are shown in Fig. 10. Again, the classic IK method results in the constraint violation, whereas in the proposed method the constraints are satisfied.
The end effector error (71) is shown in Fig. 11. The maximum error of the classic IK is e = 6.82·10 −5 m, and the maximum error of the proposed method is e = 1.95 · 10 −6 . The average computation time of the classic IK is 0.11 ms and of the proposed method 1.29 ms.

E. SCENARIO 3
In this scenario, the start configuration q 0 was: corresponding to the end effector position X 0 , and then the path continued through the points X 1 , X 2 , X 3 , X 4 , and again X 1 , shown in Table 4.
The resulting path consisted of 5 linear segments, as shown in Fig. 12. The time to traverse each segment was set equal to T 1 = 1.1 s, T 2 = 0.75 s, T 3 = 2.4 s, T 4 = 0.6 s, and T 5 = Woliński, Wojtyra: A novel QP-based kinematic redundancy resolution method with joint constraints satisfaction   1.1 s, resulting in total motion time t end = 5.95 seconds. The trajectory generation is described in detail in Appendix A.
The joint limits were set to:    2nd axis exceeds its limit in the classic IK solution, whereas in the proposed method it just touches its bound. At the acceleration level, pictured in Fig. 15, the joint constraints violations are again the domain of the classic IK, whereas the proposed method satisfies the limit.
The end effector error (71) is shown in Fig. 16. The      order of magnitude bigger than for the classic IK method, it is still-on average-under 2 ms in the tested scenarios. A low-level C/C++ implementation utilizing fast algebraic libraries (like BLAS and LAPACK [50], [51]) and an efficient QP solver (for example qpOASES [52]- [54]), should be able to easily solve the IK problem for a typical redundant manipulator under real-time operation conditions, using a medium performance computer. Of course, the newly proposed method is not a faultless approach that provides an ultimate solution to all redundancy resolution problems. Clearly, a constrained optimization problem might not always be feasible-the required motion may be beyond the robot's capabilities, and a solution that fulfills the constraints may be non-existent. In other words, if the desired trajectory becomes too demanding, it cannot be achieved within the imposed joint limits. In such a case, it might be necessary to relax requirements for the end effector velocities or accelerations while maintaining the desired shape of the trajectory. Technically, a solution to that problem can come in the form of trajectory scaling-the end effector will stay on the path, but the motion duration will be extended [55]. Therefore, a natural continuation of the work presented here is to focus on extending the QP-based approach to trajectory scaling problems. Our preliminary research on this problem indicates that the QP-based framework of redundancy resolution is well suited for trajectory scaling applications. .

APPENDIX A TRAJECTORY GENERATION
For each i-th segment, the end effector position r(t) and velocity v(t) are defined as: where the unit vector µ i along the direction of motion is: whereas the time law s i (ϕ) is defined as: and the local time ϕ, measured from the start to end of the segment, is: where the total duration of motion t end = N T i=1 T i , whereas T i is the duration of the i-th segment, and N T is the number of segments.