Inverse Kinematics of Redundant Manipulators With Dynamic Bounds on Joint Movements

Redundant manipulators are usually required to perform tasks in the operational space, but collision-free path planning is computed in the configuration space. Limiting the deviation with respect to the collision-free configuration-space trajectory may allow the robot to avoid collisions without modifying the primary task. This letter proposes a method to guarantee that the solution of the inverse kinematic problem deviates from the nominal joint-space trajectory less than a desired threshold. The excursion limitation is ensured by means of linear constraints and the automatic regulation of the weights of secondary tasks. Numerical and experimental results prove the validness of the proposed approach.


I. INTRODUCTION
K INEMATICALLY redundant manipulators have high versatility and dexterity thanks to the additional degrees of freedom they have with respect to the task they are required to perform. Extra degrees of freedom can be exploited to endow the robot with desired secondary behaviors. To name a few examples, the redundancy can be exploited to: maximize the manipulability along selected axis [1], [2], keep the joint configuration far from the mechanical limits [3], mimic anthropomorphic motion [4], activate the least number of actuators [5], avoid collisions [6], [7], or maximize the distance from obstacles [8].
The problem of exploiting the redundancy of the robot is typically addressed when solving the Inverse Kinematic (IK) problem: a primary task is given in the operational space (usually, the Cartesian space) and, for all points, the joint coordinates are computed so that the primary task is met and the secondary behavior is performed at best [9]. The most widespread approach consists in solving the IK problem at differential level to exploit the linear mapping between joint and task velocities given by Manuscript received February 24, 2020; accepted July 20, 2020. Date of publication August 4, 2020; date of current version August 14, 2020. This letter was recommended for publication by Associate Editor J. Pan and Editor N. Amato upon evaluation of the reviewers' comments. This work was supported by the European Union H2020-ICT-2017-1 -Pickplace: Flexible, safe and dependable robotic part handling in industrial environment (  the task Jacobian [10]. Joint configurations are then obtained via forward integration and numerical drifts are recovered by means of a closed-loop IK algorithm [10]. Up to few years ago, this was done by pseudo-inverting the Jacobian to find the minimum-norm solution to the problem and by projecting the velocity related to the secondary objective into the null space of the Jacobian [11]. Recently, Quadratic Programming (QP) approaches are preferred because they allow for considering robot limitations explicitly (for example, joint configuration, velocity, and acceleration limits can be written as linear constraints in the QP problem) [12]- [14]. In both cases, the result is that the robot modifies its posture according to the secondary objective, as shown for example in Fig. 1. On the one hand, this approach has the great advantage that the IK problem can be solved at runtime, by taking into account the current state of the system. For example, the robot can adapt its posture to the position of the operator without modifying the primary task [8]. On the other hand, the main drawback is the lack of a non-collision guarantee. As a matter of fact, the new robot posture is usually very different from the initial one and it might be in conflict with obstacles (see Fig. 1). Most works on redundancy handling take into account the collision avoidance problem by maximizing the distance from the closest obstacle (assumed to be a 3D point or a simple 3D shape) [6], [15]. This may work in simplified laboratory examples, but it is hardly scalable to real-case scenarios. For example, industrial robots are often required to work in very cluttered environments or to approach objects with complex surfaces. In these cases, re-configuration based on the closest-obstacle criterion is prone to failure. Most times, collision-free path planning is indeed performed in the configuration space. The most widespread methods are sampling-based (e.g., RRT and its variants [16], [17], and probabilistic roadmaps [18]) and local algorithms that search in the neighborhood of an initial solution (e.g., CHOMP [19] and STOMP [20]). Notice that this planning approach does not explicitly exploit redundancy. As a matter of fact, the presence of redundant degrees of freedom just results in a larger search space, but secondary objectives can not be specified. Moreover, the robot motion can not be modified at high sampling rates during the execution, which is the main advantage of online IK schemes.
Based on these considerations, [21] suggested to set a collision-free joint-space trajectory computed by an online path planner as low-priority task of the IK problem. In this way, the robot should follow the joint-space reference when it is not in conflict with other tasks. In that case, however, the collision-free trajectory results in a soft constraint that is soon violated to meet higher-priority tasks (as also shown in Section IV-A of this letter). Notice that imposing hard constraints on the joint deviation with respect to the reference trajectory would result in a possible failure of the QP problem. This is a well-known issue in online IK schemes, which require additional viability constraints to ensure the feasibility of the QP problem when constant position limits are considered [22], [23]. To the best of the authors' knowledge, however, no prior works address viability of time-varying position limits.
This letter proposes a method to ensure that the IK solution, computed online, has a bounded deviation with respect to a given time-varying signal. By using a configuration-space collision-free trajectory as reference signal, it is possible to avoid collisions with the surrounding environment. To do so, the letter devises a viability constraint valid for time-varying position bounds by extending the viability conditions of constant bounds [22], [23], and by using a task-scaling IK approach [24], [25]. Moreover, it devises a closed-loop algorithm that automatically adapts the weights of the secondary tasks, in order to regulate the deviation of the joint position to a given value. Compared to soft constraint-based approaches (e.g., [3], [8]), the joint movement is guaranteed to be limited to the maximum range also for time-varying bounds. The letter also analytically derives a linear approximation of the new viability constraints, taking out the need of time-consuming numerical approaches (e.g., [23] derives viable polygons for constant bounds by means of nonlinear programming). Numerical and experimental results on a 7-degree-of-freedom manipulator show that the proposed approach is able to hardly limit the excursion with respect to the nominal joint trajectory where soft-constraint approaches fail.
The letter is organized as follows. Section II deals with the formulation of IK problems as QP problems and the related feasibility issues. Proposed method is described in Section III; simulation and experimental results are in Section IV. Conclusions are drawn in Section V. Appendix A derives the polygonal viable set in closed form.

A. QP-Based Inverse Kinematic Problem
Recently, the IK problem of robot manipulators has been addressed by means of quadratic programming to cope with robot limits and/or other limitations that can be modeled as optimization constraints. As an example, consider a manipulator to which a primary task A 1q + b 1 = 0 and a secondary task A 2q + b 2 = 0 are assigned. The primary task is usually the desired Cartesian trajectory; that is, A 1 is the end-effector Jacobian and −b 1 is the desired twist at each time step. Closed-loop IK schemes modifies b 1 to recover from position errors [10]. Secondary tasks are soft constraints the robot should fulfill at best (e.g., manipulability maximization [2] or maximization of the human-robot distance [6]).
At each sampling time t = kT (where T is the sampling period of the system), the joint velocity is chosen by solving the following hierarchical QP problem: where S is a set of linear inequalities that account for the robot limits (details on the derivation of S are given in Section II-C). Problem (1) means that the secondary task (i.e., the cost function) is optimized by searching in the null space of the primary task (i.e., first and second constraints in (1)). This approach has been extended to any number of prioritized tasks and to inequality tasks in [13]. Hierarchical QP problems in the form of (1) can be solved by means of a nested approach, that is: i) solve the inner QP problem and find u 1 = argmin u A 1 u + b 1 2 ; ii) use u 1 to solve the outer QP problem (i.e.,q = argmin u A 2 u + b 2 2 subject to A 1 u = A 1 u 1 , u ∈ S). Although useful to have an insight of QPbased IK, this approach is computationally inefficient; dedicated solvers that exploit the geometric properties of hierarchical QPs are preferred [14], [26], [27].
Methods based on (1) are referred to as local, as they only take into account tasks and constraints at the current time instant. Look-ahead and model predictive control approaches have been also proposed to overcome the drawbacks of local methods [28]- [30].

B. Task Scaling Inverse Kinematics
Recent approaches exploit the path-velocity decomposition of the trajectory to preserve the geometry of the path by acting on the timing law when the nominal trajectory would exceed the robot limits [24], [31]. Problem (1) is modified by adding a scaling variable s ∈ [0, 1] that scales the primary task velocity b 1 so that the geometric direction is not changed. The resulting hierarchical QP is: (2) is similar to (1), but when the primary task is not feasible with respect to the robot limits, s results to be smaller than one and the trajectory automatically slows down to preserve the desired geometrical path.
A receding horizon approach to this problem was also proposed in [25] to overcome the drawbacks of local methods.

C. Viability and Feasibility of the Inverse Kinematic Problem
Referring to (1) and (2), S is the set of admissible velocities that satisfies the robot limits Usually, IK schemes take into account joint configuration, velocity, and acceleration limits such that: where q min and q max are the minimum and maximum joint configuration limits,q max is the maximum joint velocity vector, andq max is the maximum joint acceleration vector. To ensure the feasibility of the QP problem for all possible states in S, an additional viability constraint has to be added as follows [22]: Note: products, quotients, and inequalities between vectors are intended as component-wise.
Equations (4)(5) determine the maximal viable set for a double integral system with bounded configuration, velocity, and acceleration. The equation is quadratic inq. A viable set given by a set of linear constraint is helpful to include such conditions in the online control problem in case of optimization-based algorithms (e.g., model predictive control [25], [32]). In [25], a viable set composed of one linear constraint was derived. The result is that (4)(5) can be substituted by the following linear constraints:q The problem is analyzed more in details in [23], where a numerical method to obtain the maximal polygon given the number of sides is proposed. Equations (4)- (7) are valid if position constraints are static; thus, if q min or q max varies in time, they are no longer sufficient. The case of time-varying position limits is addressed by our method in the next section.

III. PROPOSED METHOD
Consider a joint-space nominal trajectory given by a curve q nom (s) and a timing law s(t). At sampling time t = kT , we aim to ensure that: (8) where Δq max is the vector of maximum allowed joint displacement with respect to the nominal trajectory.
We address this issue by adding a time-varying constraint to the optimization problems (1) and (2) and by adjusting the secondary task cost function dynamically.

A. Constraint-Based Limitation of the Joint Excursion
Equation (8) can be considered as a time-varying jointconfiguration constraint to be added to (1) and (2). Its addition requires a further viability analysis of the admissible set. Similarly to (4), the following viability constraint may be added to the QP problem (for the sake of brevity only the right side of (8) is considered): where q lim = q nom (k + 1) + Δq max (hereafter, dependency on k + 1 will be omitted unless necessary). Feasibility of the new problem depends on the value of q nom (k + 1). To clarify this, consider the example in Fig. 2, in which joint state (q,q) is on the boundary of (4). If q nom (k + 1) < q nom (k), then the viable set shifts to the left and the current state does not belong to the viable set anymore. Nonetheless, feasibility can be strongly guaranteed when a task-scaling IK scheme such as (2) is implemented. In this case, the desired configuration at time k + 1 depends on the scaling factor s, that is: from which: The decision variable s therefore can slow down the original trajectory to ensure the feasibility of the problem. Notice that in the worst case described in Fig. 2, the solution s = 0 does not change the viable set from cycle k to cycle k + 1 (red and green areas coincide), keeping the state within the viable set. 1) Linear Approximation of the Viable Set: Constraint (9) is nonlinear. Nonetheless, the approximation of the viable set recalled in Section II-C still holds. For example, the singlesegment constraint (6) becomes: which is linear in q,q, and s. A less conservative viable set can be obtained by using more linear constraints. Given the number of constraints N , the points P j , j = 0, . . . , N, that delineate a viable polygonal set for each joint can be obtained as follows: The derivation of (13) is given in Appendix A.

B. Self-Regulating Weighting of Secondary Tasks
The mere use of (9) (or its linear approximations) ensures the viability of the admissible set of QP problem (2). However, the IK problem is unaware of the nominal joint-space trajectory and therefore computes the joint variables by only considering the secondary task(s). As a result, the joint configurations are likely to reach the maximum excursion Δq max and the task-scaling mechanism would slow down the robot to respect the excursion limit (even up to the stop of the task, as in the previous example). To avoid this situation, the IK problem should be aware of the desired joint-space trajectory. In [8], the joint-space nominal trajectory was considered as the lowest-priority task in the QP problem. Here, we prefer to adopt a different approach where the priority of the joint-space nominal trajectory changes dynamically. Changing priorities between tasks on the fly is not trivial because an instantaneous swap of the priority of two tasks generates discontinuities in the joint commands and abrupt motions of the robot [33]. This issue can be addressed by gradually swapping the tasks in a weighted fashion during a transition phase [34], [35]. This is useful especially when the priorities of the stack of tasks are strict (e.g., humanoid robots). In industrial robotics, strict priority is usually only between the primary (i.e., the Cartesian trajectory) and the successive tasks, whereas secondary tasks are usually objective behaviors such as dexterity, human-robot distance maximization, etc. For this reason, we use a mixed approach where the secondary task A 2q + b 2 = 0 and the joint-space nominal trajectory are handled in a weighted fashion. The hierarchical QP problem (2) therefore becomes: where λ is a non-negative variable that can be adjusted to regulate the importance of the joint-space trajectory with respect to the secondary task. The automatic choice of λ can be addressed as a regulation problem such that: where Δq = q nom (k) − q(k) and Δq ref is the vector of reference values for each joint excursion such that |Δq refi | ≤ Δq max,i . Variable λ can be therefore considered as a control input to drive Δq to a reference value Δq ref based on the current value of Δq. In this work, we used a discrete-time proportional-integralderivative control law defined as: where y = 1 −max i Δq i Δq ref,i and K P , K I , K D are the proportional, integral, and derivative gains respectively. The output is saturated between 0 and λ max in such a way that: A conditioned integration anti-windup strategy have been implemented to avoid windup issues due to saturation.

IV. RESULTS
We performed simulation and experimental tests on a 7degree-of-freedom robot composed of a Universal Robots UR5 manipulator mounted on an actuated linear track. We consider conservative limits with respect to datasheet values because of the presence of cabling required by the end-effector, namely:

A. Simulations
Simulation tests have been performed by using a constrained kinematic model of the robot in ROS (Robot Operating System). The tests consist in the execution of a point-to-point Cartesian trajectory. A joint-space path is planned by means of the sampling-based algorithm BiTRRT-Connect available in MoveIt!. This path is collision free and is taken as the nominal joint-space task q nom . The corresponding Cartesian path represents the primary task to be followed by the robot. As secondary task, the robot should keep the elbow link as far as possible from the working table positioned in front of it. This is formalized by setting A 2 equal to the second row of the robot elbow Jacobian and −b 2 equal to a velocity directed toward the direction −y of the world frame. The scenario is shown in Fig. 3.
For the purpose of the present work, the actual joint trajectory q should deviate from q nom less than a given threshold Δq max . Four different cases have been analyzed: r Case A: typical IK scheme with constant joint bounds [q min , q max ] [22]; no deviation with respect to a joint-space reference is considered; r Case B: soft-constraint approach [8]; the joint-space reference trajectory is set as lowest-priority task in the IK scheme; r Case C: the self-regulating mechanism described in Section III-B is used, with Δq ref = 0.15 for all joints.
r Case D: the constraint-based method described in Section III-A and the self-regulating method described in Section III-B are used, with q max = 0.2 and Δq ref = 0.15 for all joints. In Cases C and D, the following values of the PID parameters have been used: K P = 1, K I = 10, and K D = 20. Fig. 4 shows the resulting joint trajectories of Joints 2 and 6, and the ratio max i |Δq i /Δq max,i |. Such ratio has to be less than or equal to one to satisfy the maximum deviation Δq max . As expected, in Case A, the joint trajectory deviates from the nominal one without control. The soft-constraint approach (Case B) reduces the deviation from the nominal joint-space reference (that is set as lowest-priority task) but the joint trajectories drift away from the reference as the elbow task has strict higher priority than the joint-space reference. When the adaptive weighting mechanism is activated (Case C), the deviation with respect to the nominal trajectory decreases and eventually converges to Δq ref , but it is not possible to guarantee a maximum deviation value. As a matter of fact, although Δq ref = 0.75Δq max , the maximum deviation value exceeds Δq max during part of the task (at around 10.5 s). On the contrary, Case D always ensures the strict satisfaction of the maximum deviation: the joint trajectory never exits the tube q nom ± Δq max and the ratio is always less than or equal to one. As a drawback, the scaling mechanism has to be activated to recover feasibility and this results in the slowdown of the task. As shown in the figure, the time required to complete the task is around 1.5 times larger than the time taken by Cases A, B, and C. This phenomenon is relevant especially when Δq max is very small or when Δq ref /Δq max is close to one, for the joints are often close to their maximum allowed excursion. It is also evident that the limitation of the joint range of motion comes at the cost of a reduced satisfaction of the secondary task; the smaller is the allowed range of motion, the smaller is the secondary task satisfaction. In this scenario, the secondary task minimizes the y-component of the elbow position. Its level of satisfaction for the different cases is shown in the following table: where the percentage of the secondary task satisfaction (compared with the unbounded Case A) is computed as: where y elb i is the average value of the elbow y-coordinate during the test, i ∈ {A, B, C, D} denotes the case and y elb 0 refers to the average value of the elbow y-coordinate corresponding to the joint-space nominal trajectory.

B. Experiments
A human-robot cooperation application has been implemented on the real 7-degree-of-freedom platform composed of a Universal Robots model UR5, version 3.2, mounted on a linear track actuated via a linear motor and an Elmo Gold Cello servo drive. The proposed IK scheme (Case D in Section IV-A) has  Fig. 4. Simulation results: joint configurations of Joints 2 and 6 (respectively, UR5's base and wrist2 joints, according to the manufacturer naming) and ratio max i |Δq i /Δq max,i | for Cases A (first row), B (second row), C (third row), and D (fourth row). Solid blue: actual joint trajectories; solid red: nominal configuration-space trajectories q nom ; rose area: allowed joint deviations q nom ± Δq max . Cases A and B cannot limit the joint deviation; in Case C the joint deviation converges to Δq ref but the hard constraint is violated during transients (the ratio is grater than 1 at time 10.5 s); in Case D the joint trajectories are always within the allowed tube (the ratio is always less than or equal to 1). been implemented in ROS-control framework as an intermediate controller between MoveIt! and the robot controller. The IK scheme generates the reference signal for the robot controller, which is a cascade of a joint-position and a joint-velocity control loops. The position controller is a proportional gain equal to 7, the velocity controller is the built-in control system of the robot. The IK output is the reference of the joint-position control loop, whose control action is sent to the robot velocity controller via TCP/IP with a frequency of 125 Hz (maximum allowed by the UR5 controller interface). Similarly, the linear track receives the control action with a frequency of 1 kHz by means of a Ethercat field-bus. The real-time position of the humans is acquired by means of two Realsense cameras, model d435. Static obstacles are removed from the point cloud signal by means of the ROS robot self-filter. The filter output is re-sampled by using 10 cm 3 voxels and sent to the IK solver with a frequency of 30 Hz.
The robot has to repeat a point-to-point motion in the Cartesian space, sharing the workspace with two operators. The secondary task consists in the movement of the robot elbow in the opposite direction with respect to the position of the closest body part. Hence, A 2 coincides with the first two rows of the elbow Jacobian and b 2 is equal to the unit vector from the elbow to the human position, multiplied by a gain equal to 0.4. The maximum and reference joint excursions are Δq max = 0.4 and Δq ref = 0.3. To avoid collisions with the operators, the speed of the robot is reduced by a factor between 0 and 1 based on the human-robot relative distance as in [8]. The speed reduction factor is equal to 1 up to a distance equal to 1 m and is equal to 0 when the distance is equal to 0.3 m. Intermediate values are obtained by interpolating via a third-order polynomial. Fig. 5 shows that the ratio max i |Δq i /Δq max,i | is always less than or equal to one, which means that the joint trajectories Fig. 5. Experimental results: ratio max i |Δq i /Δq max,i | for Case D (solid blue). Ratio equal to 1 (dashed red) represents the maximum allowed deviation. Thanks to the proposed method (Case D), the ratio is always less than or equal to 1, that is, the joint trajectories are always within the allowed tube q nom ± Δq max . always lay within the allowed tube q nom ± Δq max , according to the theoretical and simulation results.

V. CONCLUSION
This letter studied the time-varying limitation of the joint movements in the inverse kinematic problem of redundant manipulator. The motivation of the work came from the need of ensuring a bounded deviation of the joint movement with respect to a given nominal configuration-space trajectory. The letter showed that such limitation can be strictly ensured by using a task-scaling inverse kinematics and by deriving linear constraints with respect to the joint command variables. Moreover, a self-regulating weighting strategy was implemented to keep the robot joint close to the reference signal, so that the scaling mechanism activation is reduced. The validity of the proposed approach was proved via simulations and experiments on a 7-degree-of-freedom industrial manipulator.

A. Viable Polygonal Sets for Constrained Double Integral Systems
Imposing the viability of the admissible set is necessary to ensure the feasibility of the optimal control problem for all possible states of the system [36]. The equation of the maximal viable set for a double integral system is quadratic in the system states. Polygonal viable sets are useful because they can be easily implemented in optimization-based control algorithms (e.g., model predictive control [25], [32]). In [23], a polygonal viable set with a given number of sides is obtained by setting up an optimization problem that maximizes the area of the polygon. The output of the optimization is the sequence of points that delineate the polygon. The numerical approach is time consuming and a change in the joint limits would require the re-computation of the set. In this work we derive a viable polygonal set in closed-form.
To this purpose, consider the portion of the convex polygon in the first quarter delineated by the sequence of points {P 0 , . . . , P N }, where P j (x j , y j ) (results for the third quarter of the plane can be derived straightforwardly by mirroring the solution). Starting from point P 0 (0, q max ), the maximal viable polygon can be constructed by maximizing the slope of the segments in the plane qq (see also Fig. 6). The maximum slope aṫ q = x j depends on the maximum accelerationq max and is equal to −x j /q max . Thus, the successive points can be derived with respect to their x j coordinate, for example: For a generic point P j it results: where x 0 = 0. The approach in [23] sets up an optimization problem to minimize the area A in Fig. 6. An analytical solution to the problem is doable only for small values of N (N ≤ 3).
Consider the simpler problem where only the y coordinate of point P N is maximized, that is: Notice that this problem is a simplification of the original one, but the approximation is reasonable also in view of the fact that the maximization of y N tends to maximize the region where the robot is allowed to work at high speed. Problem (24) can be solved analytically by imposing δy N /δx j = 0, that is: that can be rewritten as (26) with x N =q max . It follows that the points have to be equally spaced along the axisq. This can be summarized as: Finally, the points of the polygon can be found by inserting (27) into (23) to obtain: It is interesting to note that: 2q max (29) which means that the polygonal viable set tends to the maximal viable set given by (4), for N → ∞.