Kai Pfeiffer

Adrien Escande

Abderrahmane Kheddar

Singularity resolution in equality and inequality constrained hierarchical task-space control by adaptive non-linear least-squares

We propose a robust method to handle kinematic and algorithmic singularities of any kinematically redundant robot under task-space hierarchical control with ordered equalities and inequalities. Our main idea is to exploit a second order model of the non-linear kinematic function, in the sense of the Newton's method in optimization. The second order information is provided by a hierarchical BFGS algorithm omitting the heavy computation required for the true Hessian. In the absence of singularities, which is robustly detected, we use the Gauss-Newton algorithm that has quadratic convergence. In all cases we keep a least-squares formulation enabling good computation performances. Our approach is demonstrated in simulation with a simple robot and a humanoid robot, and compared to state-of-the-art algorithms.

I. INTRODUCTION

Hierarchical (or strict priority-based) task-space schemes are a popular approach for the control of kinematically redundant robots, i.e. robots having more degrees of freedom (DoF) than required to fulfil a given task [START_REF] Chiaverini | Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators[END_REF]. For a task-space error function that must be driven to zero, a linearized model is built at the velocity or acceleration level, and solved in a least-squares way. The null space of the linear model can be used to satisfy at best a second task in the same way without interfering with the first one. This generalizes to any number of tasks, as long as some DoF's are remaining [START_REF] Siciliano | The general framework for managing multiple tasks in high redundant robotic systems[END_REF].

While the approach has been long known for equalityonly tasks, it is only with the seminal work of [START_REF] Kanoun | Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks[END_REF] that a full support of inequality tasks at any priority level has been proposed. There, the resolution of the linearized problem is done through a so-called cascade of constrained least-squares programs (LSP): each level of the hierarchy is solved in turn in a least-squares sense under the constraint of keeping all previous levels at their optimal amount of violation. The downside of this approach is a relatively high computation cost because each LSP is redoing some work done by the previous ones, and it is not possible to use a proper warm-start to leverage the similarities between problems in consecutive control iterations. A dedicated solver was proposed in [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF] and further improved in [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF], enabling to solve the same general hierarchical problem very efficiently.

The conjunction of a linear model and a hierarchical leastsquares resolution is however failing when approaching a singularity: the Jacobian matrix of one of the tasks (kinematic singularity case) or its projection onto the nullspace of higher-priority tasks (algorithmic singularity case) is becoming nearly rank-deficient [START_REF] Chiaverini | Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators[END_REF]; its inversion therefore yields very high and unwanted joint velocities or accelerations, leading to instabilities. The first kind of singularity typically arises when a target is out of reach, while the second one is a consequence of conflicts between tasks.

One way to handle singularities is to compute an approximate solution to the linear problem with reasonable values. This is most commonly achieved with damping, where a term minimizing the solution norm is added to the least-squares. It is introduced with a weight that can be either fixed by the user or can be automatically adapted, for example using the so-called manipulability factor [START_REF] Nakamura | Inverse Kinematic Solutions With Singularity Robustness for Robot Manipulator Control[END_REF] or some estimate of the minimum singular value [START_REF] Maciejewski | Numerical filtering for the operation of robotic manipulators through kinematically singular configurations[END_REF]. Another approach prohibits infeasible end-effector targets by clamping and introducing a more refined damping considering all the Jacobian's singular values w.r.t. the robot's joints distinctively [START_REF] Buss | Selectively damped least squares for inverse kinematics[END_REF]. A very simple method avoids the Jacobian inversion and uses its transpose instead, with stable but worse convergence results [START_REF] Wolovich | A computational technique for inverse kinematics[END_REF].

Singularity handling has been mostly studied for equality tasks only, and is still an open problem for a general hierarchy including inequality tasks. This is why, despite their efficiency, the solvers [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF] [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF] have never been used online on a real robot. To the best of our knowledge, the only instance of a general hierarchy tackling singularities is [START_REF] Herzog | Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid[END_REF] where the authors propose an improved cascade of LSP that includes fixed damping at every level. While faster than [START_REF] Kanoun | Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks[END_REF], it is still suffering from the same drawback and from the fact that tuning the damping weights can be difficult: too low gains and the solution will be too large; too high gains and the convergence will be hindered.

There is another way to look at the shortcomings of the linear model with the least-squares resolution approach: rather than approximating the solution, change the model to better specify the solution we want. It can be indeed argued that the linear model is not 'good enough' in the vicinity of a singularity: the near rank deficiency of the Jacobian matrix means that we are losing information in one direction. Since the first derivative proves to be insufficient, we could incorporate second-order information.

It is interesting to draw parallels between control and nonlinear least-squares optimization [START_REF] Deo | Adaptive non-linear least squares for inverse kinematics[END_REF], [START_REF] Wieber | Geometric and numerical aspects of redundancy[END_REF]. For one task, the least-squares minimization of a linear model corresponds to the Gauss-Newton (GN-) method, while the damping approach is akin to the Levenberg-Marquardt (LM-) algorithm [START_REF] Nocedal | Numerical Optimization[END_REF]. When the full second order information is used, we get the Newton (N-) method. In fact, the three methods have a ground commonality: they are all derived from the second order Taylor approximation of the squared norm of a non-linear function, only differing in whether the Hessian is given analytically or by some approximation. Second order derivatives are usually costly to compute, so that several methods have been proposed to approximate them [START_REF] Dennis | An adaptive nonlinear least-squares algorithm[END_REF], [START_REF] Toint | On large scale nonlinear least squares calculations[END_REF], the most widely used being the BFGS formula [START_REF] Broyden | The Convergence of a Class of Double-rank Minization Algorithms[END_REF], derived from the secant equation. Newton methods with Hessian approximation are named Quasi-Newton (QN-) methods.

Neither the N-method nor the QN-method is specifically designed to serve in multi-level hierarchies. In [START_REF] Zhao | Inverse kinematics positioning using nonlinear programming for highly articulated figures[END_REF], a QNmethod using BFGS updates for the second order information, weights its objectives to establish a non-strict hierarchy. LM-algorithms like [START_REF] Sugihara | Solvability-unconcerned inverse kinematics by the levenbergmarquardt method[END_REF], [START_REF] Chiaverini | Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators[END_REF] can be included into a strict hierarchical scheme by damping and handing each objective to a hierarchical least-squares solver like in [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF], though missing the higher accuracy of the N-method.

In this paper we combine the (strict) hierarchy task-space control scheme with the high accuracy and computation speed of QN-methods: we propose a new singularity robust method for prioritized inverse kinematics with any number of hierarchical levels of equalities and inequalities, may they be feasible or not, while encompassing numerical stability. Our QN-approach outperforms current state of the art in singularity robust inverse kinematics methods in terms of accuracy while being computationally competitive.

II. SECOND ORDER INFORMATION IN A HIERARCHY

In this paper, we consider a robot with configuration q, and a set of p geometric functions f (q), one for each priority level. For each function we define a target or bound f d , defining tasks f (q) = f d or f (q) ≤ f d , and the error function e = f d -f . We derive a hierarchical velocitybased controller minimizing those errors. The targets f d can be time varying, but at each control iteration, we consider them as constant (no feed-forward term).

At each control step k, we need to compute a velocity q. The next state q k+1 is then integrated to

q k+1 = q k + ∆q k . (1)
with the increment ∆q = q∆t (∆t = 1).

In the following we will drop the index k for a better reading (other indices k -1, k + 1... are kept).

Given a single function f with Jacobian J , we can look for ∆q yielding a given error decrease ∆e * with the relation ∆e * = -J ∆q.

(

) 2
For the sake of simplicity we take ∆e * = -e.

The required error decrease may not be achievable and we solve the above using a linear least-squares

min ∆q 1 2 J ∆q -e 2 2
(3)

= min ∆q 1 2
e T e -∆q T J T e + 1 2 ∆q T J T J ∆q.

A solution is given by ∆q = J + e, where J + is the Moore-Penrose pseudo inverse. This formulation corresponds to the GN-algorithm.

As discussed above, this approach is not robust in the vicinity of singularities: J is almost loosing a rank and ∆q becomes very large. We now discuss how second order information can be incorporated into the linear model.

Let's define a scalar target function as follows [START_REF] Deo | Adaptive non-linear least squares for inverse kinematics[END_REF]:

Φ(q) = 1 2 f d -f (q) 2 2 = 1 2 e(q) 2 2 = 1 2 e T e. (5)
Φ(q) is non-linear (quadratic, trigonometric functions) and can only be handled by means of non-linear programming. Yet, we can reformulate the problem to be approximated as linear least-squares. We approximate Φ(q) in a small neighbourhood ∆q of point q by a second order Taylor series:

Φ k+1 (q + ∆q) ≈ Φ(q) + ∆q T ∇Φ + 1 2 ∆q T ∇ 2 Φ∆q (6)
= Φ(q) -∆q T J T e + 1 2 ∆q T (J T J+H)∆q.

We have the gradient

∇Φ(q) = -J T e (7)
and the Hessian

∇ 2 Φ(q) = J T J + H = J T J + dim(f ) i=1 e i ∇ 2 f i (8)
where H gathers the second order information (see next section). ∆q can then be computed by

min ∆q Φ k+1 (q + ∆q) (9)
= min ∆q 1 2 e T e -∆q T J T e + 1 2 ∆q T (J T J + H)∆q.

This corresponds to the Newton method applied to non-linear least-squares [START_REF] Nocedal | Numerical Optimization[END_REF]. We see that by neglecting the second order term H, we get back to the GN formulation. Taking H as a multiple of the identity yields the LM approach.

If H is positive definite, or approximated by a positive definite matrix, we can take its Cholesky decomposition H = R T R. This leads to the LSP

min ∆q 1 2 J R ∆q - e 0 2 2 (10)
= min ∆q 1 2 J ∆q -e 2 2 + 1 2 R∆q 2 2 .
This is the approach we propose in this paper. It can be seen as an augmentation of the GN formulation with the second order information in R. R is obtained with the Bunch-Kaufman decomposition LDL T [START_REF] Bunch | Decomposition of a symmetric matrix[END_REF] where the 1×1 blocks of the block diagonal matrix D have to be positive and the 2 × 2 blocks need to be positive definite [START_REF] Moré | On the use of directions of negative curvature in a modified newton method[END_REF]. The factorization

R T R is then retrieved by R = D 1/2 L T .
The regularization ensures positive definiteness of R and therefore the convexity of our problem.

The above derivation can be conducted for each task in the hierarchy (for inequality tasks, it simply requires the introduction of a slack variable, see [START_REF] Kanoun | Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks[END_REF]). We thus end up with a set of priority-ordered least-squares problems of the form (10) that we can pass to the hierarchical solver [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF].

The next section presents our approach to compute H.

III. QUASI-NEWTON SCHEME FOR HESSIAN CALCULATION

A. Hessian calculation for single level with equalities only

The calculation of the second order derivatives ∇ 2 f i in H of ( 8) is of complexity O(d 3 ), d is the number of the robot's DoF. We propose a faster method based on the BFGS algorithm. Our computational complexity for H is O(d 2 ).

The BFGS method iteratively computes an approximation of the whole Hessian B, using only gradient information:

B = B k-1 + yy T y T ∆q k-1 - B k-1 ∆q k-1 ∆q k-1,T B k-1 ∆q k-1,T B k-1 ∆q k-1 .
(11) y is the difference between the current ∇Φ and the previous gradient

∇Φ k-1 : y = ∇Φ -∇Φ k-1 .
The update B is positive definite if B k-1 is also positive definite and the curvature is greater than zero y T ∆q k-1 > ξ, otherwise no update is done. In theory ξ = 0 but in practice we choose a numerical threshold like ξ = 10 -12 .

B k-1 needs to be initialized with a positive definite matrix in the very first iteration k = 1, for example by

B 0 = J 0,T J 0 + µI. ( 12
)
At the start, J 0 is not available so we use J instead. This resembles the true Hessian in [START_REF] Buss | Selectively damped least squares for inverse kinematics[END_REF]. Since J and therefore also J T J might be close to singularity we fill the diagonal with a weighted identity matrix to make B 0 regular.

As in [START_REF] Sugihara | Solvability-unconcerned inverse kinematics by the levenbergmarquardt method[END_REF], we set µ = max(µ, 1 2 e 2 2 ) with µ = 1 • 10 -3 being a lower threshold to handle cases where the error is very small but the task is still augmented.

For our actual least-squares formulation the BFGS update B needs to be reduced by

H = B -J T J ( 13
)
in order to receive the desired value H. However, this results in an indefinite matrix H which can lead to unstable or unsmooth behaviour despite the regularization to a positive definite matrix. We therefore choose to not conduct this reduction (for consistency, the initialization is also done without adding J T J ). We observed that the QN-method is very forgiving in terms of simplifications in the second order information.

As long as the second order information is positive definite we will converge at least linearly. Unfortunately, with the current formulation it is not possible to directly update H.

B. Hessian calculation for hierarchies with equalities only

Solving a p levels hierarchy can be expressed as solving a sequence of constrained least-squares problems where the violation w l of the objective of level l has to be minimized without increasing the (fixed) violation w * i of already minimized objectives of higher priority i = 1, . . . , l -1:

For l = 1, . . . , p : min ∆q,w l 1 2 w l 2 2 (14) subject to J l ∆q -e l = w l J i ∆q -e i = w * i , i = 1, . . . , l -1
The corresponding Lagrange function for constrained optimization can be formulated as follows:

L l = 1 2 e T l e l -∆q T J T l e l + 1 2 ∆q T J T l J l ∆q + l-1 i=1 (J i ∆q -e i -w * i ) T λ i,l (15)
= 1 2 e T l e l -∆q T J T l e l + 1 2 ∆q T J T l J l ∆q + l-1 i=1 ∆q T J T i -(e i + w * i ) T λ i,l = 1 2 e T l e l - l-1 i=1 (e i + w * i ) T λ i,l + ∆q T -J T l e l + l-1 i=1 J T i λ i,l + 1 2 ∆q T J T l J l ∆q.
λ are the Lagrange multipliers where λ i,l is the vector of Lagrange multipliers indicating the conflict of level l with level i. The following equivalence with the task slack variable holds [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF]:

λ l,l = w l . (16)
(15) has the characteristics of a hierarchical version of the GN-algorithm (4). Its gradient can be written as

∇Φ(q) = -J T l e l + l-1 i=1 J T i λ i,l . (17)
This hierarchical GN-gradient is then used to compute y l , and consequently B l .

We only use the Lagrange multipliers of the current iteration in the calculation of y l = (J l -J k-1 l ) T λ l [START_REF] Nocedal | Numerical Optimization[END_REF]. B 0 l of level l is initialized as

B 0 l = J T l J l + l i=1 max(µ, 1 2 e i 2 2 )I * i . ( 18
)
I * is an identity matrix where only diagonal entries are occupied when the corresponding joint is on the kinematic chain of the task. This prevents unnecessarily decreasing the null-space of this task and allows better results with lower priority tasks.

C. Hessian calculation for general hierarchies

When there are also inequality constraints, [START_REF] Dennis | An adaptive nonlinear least-squares algorithm[END_REF] becomes

For l = 1, . . . , p : min ∆q,w l 1 2 w l 2 2 (19) subject to l l ≤ J l ∆q -w l ≤ u l l i ≤ J i ∆q -w * i ≤ u i , i = 1, .
. . , l -1 where l i and u i are lower and upper bounds, i = 1, . . . , p, and an equality constraint is expressed by setting the corresponding lower and upper bound to the same value. Inequality constraints can be handled by the so-called activeset method (see [START_REF] Nocedal | Numerical Optimization[END_REF]). A constraint is active when it holds as an equality at the solution and inactive otherwise. The active set contains all equalities and all active inequalities. Inactive constraints do not impact the solution and can be ignored. Between resolutions, the active set can change.

Active set changes have to be considered due to the iterative nature of the BFGS-algorithm (see [START_REF] Deo | Adaptive non-linear least squares for inverse kinematics[END_REF]). The current BFGS-update B k-1 l of level l is a function of the old active set A k-1 since in a previous iteration k 0 , B k0 was initialized by [START_REF] Moré | On the use of directions of negative curvature in a modified newton method[END_REF]). Without reinitialization, the new BFGS-update B l would unnecessarily occupy rank (or joints for a better visualization) of the inactive inequalities. These joints then can not be used by tasks on lower hierarchical levels, leading to a solution with higher error norms of these tasks. Therefore, when the new active set A differs from the old one A k-1 the BFGSalgorithm is reinitialized from the lowest level h, where an active set change occurred, to the last level p by

B k0 l = • • • + l i∈A k 0 -1 • • • I * i (see
B k-1 l = J k-1,T l J k-1 l + l i∈A max(µ, 1 2 e i 2
2

)I * i ( 20
)
with l = h, • • • , p. Inactive constraints are neglected in the summation. The following initialization

B k-1 l = J k-1,T l J k-1 l + l i∈A max(µ, 1 2 λ i,l 2
2 )I * i (21)
with the Lagrange multipliers λ can lead to instability. Especially when constraints are nearly parallel to each other the Lagrange multipliers can grow unlimitedly. Therefore, it is safer to use the physical task error.

IV. SWITCHING BETWEEN GN-ALGORITHM AND QN-METHOD The analytic expression for H becomes nil inherently for dim(f ) i=1 e i ∇ 2 f i = 0. This is not the case for BFGS but has to be explicitly enforced. Additionally, the GNalgorithm converges quadratically when close to a solution. This is a very desirable quality over the possibly only linear convergence of the QN-method. However, whether the current state is in the proximity of a solution or not needs to be measured accordingly. In this work we propose to observe the model error Φ-Φ of the previous iteration. We have that

Φ(q+∆q)=Φ(q)-∆q T J T e+ 1 2 ∆q T (J T J+H)∆q+O(∆q 3 )
which can be calculated by [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF]. We define Φ as its approximated model. For the GN case we have Φ(q + ∆q) = Φ(q) -∆q T J T e + 1 2 ∆q T J T J ∆q and for the QN-method we have

Φ(q + ∆q)=Φ(q) -∆q T J T e + 1 2 ∆q T (J T J + R T R)∆q.
The approximation error is at least of order O(∆q 3 ) since the Taylor series of our non-linear model function is limited to second order. However, the error will be dominated by a O(∆q 2 ) originating from the second order information either being omitted (GN-method) or approximated (QNmethod). In practice it is sufficient to observe the quadratic norm residual of the GN-algorithm or QN-method

= 1 2 w 2 2 = 1 2 J ∆q -e 2 2 . (22)
In case of the slack being zero, the least squares is solved without residual

f d = f (q) + J ∆q. (23)
However, the third order approximation error O(∆q 3 ) might still be present and we only have

f d ≈ f (q + ∆q). ( 24
)
We switch from the QN-to the GN-method whenever becomes smaller than a certain threshold (typically 10 -12 ), and from GN to QN when it becomes bigger.

Note that for the case of a GN-QN switch on level c we reinitialize the BFGS from level c to the last level l.

V. TRUST REGION

The Taylor series represents the original function well enough only in a small neighbourhood ∆ of the current state q. This can be enforced by subjecting either the GNalgorithm or the QN-method to a constraint of the form

∆q ∞ < ∆ (25)
In practice, this trust region constraint is put on the very first level of the hierarchy as -∆ ≤ ∆q ≤ ∆.

Choosing the trust region ∆ such that a well defined Taylor approximation persists is a delicate process. Selftuning methods from pure optimization are difficult to transfer to the robot control case. Most importantly, we cannot recalculate solutions with different radii due to the real-time constraint. An important point is also that to the best of our knowledge none of these methods are specifically designed for optimization with hierarchical constraints.

Therefore, we adapt the original trust region ∆ with the following method:

• Loop through all variables i = 1, • • • , d of the solution ∆q and check if a single entry ∆q i changed its sign compared to ∆q k-1 i . • If so, η i =min(η, ρ αi dec •η i ), and increase α i by 1.

• If not, η i =max(1, 1/ρ inc •η i )
, and decrease α i by 1.

• Change the trust region radius for joint i by ∆ i = -∆/η i and ∆ i = ∆/η i . η is an upper threshold with for example η = 10 6 . ρ dec = ρ inc = 1.2 are the trust region decrease or increase factors. ∆, ∆, η and α are vectors with d entries. η and α are initialized with 1's.

It is essential that the original trust region ∆ is already chosen as well as possible. From our experience, for every kinematic structure a radius can be found which leads to good convergence for the wide range of applications we tested. In this work, we set ∆ = 0.01[rad] or [m].

Algorithm 1 Lexicographic second order augmentation Input: B, J , J -, e, λ, A, c A , ∆q

1: y = 0, R = 0 2: if t = 0 or c A > -1 then B c A = 0 3: for all i ∈ A do 4: if c A > -1 and i ≥ c A then B i = J k-1,T i J k-1 i 5:
for all j ≥ i and j ∈ A do 6:

y j = y j + (J T i -J T -,i )λ i,j 7:
if c A > -1 and i ≥ c A and j ∈ A then 8:

B j += max(µ, 1 2 e i 2 )I * i 9:

if t = 0 then R i = √ B i 10: else if 1 2 w i 2 > then 11:
if y T i ∆q > ξ then 12:

B i = B i + yiy T i y T i ∆q -Bi∆q∆q T Bi ∆q T Bi∆q 13: R i = LDLT(B i -J T i J i ) return R,

VI. VALIDATION

An algorithm overview for the calculation of R named LexLSAUG2 (Lexicographical Least Squares Augmented with 2 nd order information) is given in Alg. 1. The trust region constraint is on level 0 and is not handled in this algorithm. Any underlined variable contains the values from all levels. B i contains B from level i to the last level l. c A shows the lowest level where a change of the active set occurred. c A is set to -1 if there is no change of the active set. LDLT gives back the regularized LDL T decomposition of the input matrix.

We assess our method GN-QN (which solves with LexLSI [START_REF] Dimitrov | Efficient resolution of potentially conflicting linear constraints in robotics[END_REF] the problem [START_REF] Bunch | Decomposition of a symmetric matrix[END_REF] possibly augmented by LexLSAUG2) in simulations with two different robots, and compare with three solvers that resolve singularities in hierarchical problems:

1) the GN-algorithm without any Hessian augmentation 2) the hierarchical least squares with adaptive damping (ADLS) described in [START_REF] Chiaverini | Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators[END_REF] for hierarchies with any number of levels of equalities. When having one level of equalities, it is equivalent to the LM-algorithm. The maximum damping is set to 2 while the threshold for the minimum singular value is 0.5. 3) the constant damping hierarchical quadratic programming (DHQP) described in [START_REF] Herzog | Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid[END_REF], which handles both equalities and inequalities. For equalities only problems DHQP is identical to ADLS with constant damping. The damping is set to 2.

A. Test bench

We tested our method with 20 different test cases. Their length is limited to 25000 iterations.

The test cases T1 to T19 are performed on a 2D 4 DoF robot with two end-effectors (see Fig. 1). It possesses a translational DoF at its base and three revolute joints, one at the base and two at its 'shoulder' enabling the two arms to rotate. All links are of Hierarchy A (T1, T3 to T19) and B (T2) (2D robot) 0) 4 trust region limits ∆ ≤ I q ≤ ∆ (GN and GN-QN) or 4 velocity limits qvl ≤I q ≤ qvl (DHQP) 1) 2 equality constraints on blue end-effector as follows:

J ef2 q = f ef2 d
2) 2 equality constraints on green end-effector as follows:

J ef3 q = f ef3 d
3) Minimal norm solution q = 0 For hierarchy B, an inequality constraint on the white endeffector / 'shoulder' J ef1 q ∈ f ef1 d is added between level 0 and 1 of hierarchy A.

For ADLS, the joint velocity constraint is omitted since inequalities can not be handled. The initial robot configuration is [0, 0][m] for the free flyer and [-π/2, 0, 0][rad] for the revolute joints.

The end-effectors at the tip of the arms must track two targets oscillating diagonally with amplitude [START_REF] Siciliano | The general framework for managing multiple tasks in high redundant robotic systems[END_REF][START_REF] Siciliano | The general framework for managing multiple tasks in high redundant robotic systems[END_REF][m] and crossing at [0, 0][m]. This means that for T1 the targets are just reachable for the robot in the corner cases. For T3, the targets oscillate with amplitude [START_REF] Siciliano | The general framework for managing multiple tasks in high redundant robotic systems[END_REF][START_REF] Kanoun | Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks[END_REF][m]. For T2 an additional inequality constraint (hierarchy B) is imposed on the position of the shoulder. The constraint box reaches from

[-1, 0][m] to [1, 1][m].
For T4 to T8, we use static targets for level 1 and 2 at

[0, y][m] and [x, 1][m] with (x, y) equal to (1, 2), (1 + , 2 +
), (1 -, 2 -), (1 + 10, 2 + 10) and (1 -0.25, 2 -0.25).

The three first case are slight variations to test robustness with = 0.001[m] (1/1000 of the link length) such that both targets are either just out of reach or just in reach. The two last cases test for targets well out of reach or fully reachable. For T9, both targets change their position randomly within [±2, ±1000][m] at each of the first 12500 iterations, and then are static at [-1.9, -422][m] and [-1.5, 299][m] respectively for the rest of the test. This test shows that the second-order approximation can deal with highly noisy targets.

For T10 to T14, the targets are at [t, y][m] and [-2, 2][m] where t increases linearly by 0.001 with the number of iterations, and y takes the same value as for T4 to T8.

For T15 to T19, the first target is at [0, y][m], with y defined as above, while the second target oscillates as in T1. Fig. 2. T20 screenshots for GN-QN, from left to right: HRP2 reaching for the target on its top left (out of reach, both CoM tasks are active), then the target is in reach (the left hand task is switched to the GN-algorithm) and finally the target is out of reach again in HRP2's far right bottom. The accompanying video also shows how the robot tries to stay as long as possible in the smaller (green) CoM bounding box on the last level as it is tracking the target (made possible by switching to the GN-algorithm when the target is in reach, and which can not be observed for DHQP).

T10 to T19 show that our method is capable of tracking static and dynamic targets at the same time while interchanging their priority and testing for robustness.

T20 shows that our method is scalable to any number of DoF. The robot HRP-2Kai with 38 DoF is standing in a stable position in a cluttered aircraft workspace environment (see accompanying video). Both feet and the left hand are given fixed positions either on the ground or grabbing a pole in front of the robot. On the next level the centre of mass (CoM) projection is asked to remain in a rectangle a bit larger than the polygon spanned by the feet. The right hand is then used to track a target swinging from the robot's far top left to its far right bottom. At the lowest priority level, we give a smaller polygon for the CoM projection, corresponding to the feet only. The hierarchy then looks as follows:

Hierarchy C (T20) (HRP-2Kai) 0) 38 trust region limits ∆ ≤ I q ≤ ∆ (GN and GN-QN) or 38 velocity limits qvl ≤ I q ≤ qvl (DHQP) 1) 38 joint limits qjl ≤ I q ≤ qjl 2) 18 in-reach equality constraints (translation, rotation) on left, right foot and left hand:

J lf q = f lf d , J rf q = f rf d , J lh q = f lh d
3) 3 inequality constraints on the CoM: J c q ∈ f c1 d 4) 3 in/out-of-reach equality constraints (translation, oscillating target in 3D-space) on right hand: J rh q = f rh d 5) 3 stricter inequality constraints on the CoM:

J c q ∈ f c2 d 6) Minimal norm solution q = 0

B. Evaluation criteria

The main problem that can arise near a singularity is oscillations. We measure the presence of oscillations by tracking the change of sign between iterations for each component, and summing the amplitude of the change.

Σ = 25000 k=2 d i=1 |∆q k i | if sgn(∆q k i ) = sgn(∆q k-1 i ) 0 otherwise
A low Σ means few or no oscillations.

We also integrate the normed error of the equality with highest priority ('ef 2 ' task for hierarchy A and B, 'rh' task for hierarchy C) over time and then normalize it by the smallest value of all solvers for this test case (Ξ). The performance of the other tasks is not considered since bad convergence of a task with high priority can lead to better convergence of a task with lower priority, then lacking of any relevance. Since the joint velocity constraint is omitted for ADLS and thus allowing for way faster convergence from the initial position to the targets its performance on Ξ is not considered (grey). For static test cases, the iteration where every entry of the solution vector becomes smaller than 10 -6 [rad] or [m] is noted (Ψ).

C. Evaluation

The evaluation results of all test cases are given in Table I. Our method is stable (low Σ) and performs best in terms of convergence (Ξ ≈ 1). While DHQP and ADLS seem to be unstable for many test cases, it is very important to mention that the damping can be tuned in such a way that Σ = 0[rad] (possibly with very high damping terms for T10 to T15, and thus with very slow convergence). Because this has to be done on a per-case basis, and after running a test, in our work the damping is chosen such that Σ = 0[rad] for T1. These values are applied for all the other test cases to enable a fair comparison for the convergence of T1, T2 and T20.

The test cases show the following qualities of our method:

• Stability in the presence of singularities when the target is far away (T7, T9, T10, T11, T12, T13, T14, T18) when the target is at the border of reachability (T1, T4, T5, T6, T10, T11, T12, T15, T16, T17)

• Fast and good convergence properties

• Stable for all test cases without the need for damping tuning.

• Works for inequalities (T2, T20) • Scalable, applicable for any number of DoF (T20) • Strong self-regulating capabilities (T9) • The hierarchy is not disturbed as it is the case with the damping term which introduces a model discontinuity when the rank of a matrix changes and the damping is not applied in the same null-space (T15, T16). More detailed results for T1 are given in Fig. 3 showing the task error norms. GN-QN tracks the level 1 target very well due to the switching to the GN-algorithm allowing for quadratic convergence. This behaviour can also be observed at times for ADLS when the damping term becomes zero. For level 2, DHQP and ADLS have lower task error norms at times but this is due to their worse convergence of level 1, putting the level 2 end-effector closer to its target. In general, DHQP converges the worst due to the constantly present damping.

The GN-algorithm is unstable with a high Σ = 574 [rad]. It shows that the GN-algorithm alone is not a safe method for a real world robot application.

For T2, the task error norms are given in Fig. 4. GN-QN and DHQP track the level 2 target very well with slight advantage for GN-QN. The inequality constraint is violated at some instances for both methods at a low amplitude. For level 3, DHQP has a lower task error norm at times but this is due to the worse convergence of level 2, putting the level 3 end-effector closer to its target.

For the static test cases T4 to T8, GN-QN behaves in a stable manner while DHQP becomes unstable when the targets are at [0, 2 + 10][m] and [1 + 10, 1][m]. Especially, DHQP converges exponentially whereas the solution of GN-QN decreases much faster to numerical zero by switching to the GN-algorithm.

For T9 and GN-QN, the robot does not follow the random noise of the targets. This is due to the trust region adaptation method. For T10 to T14, GN-QN is stable while DHQP is only stable in the beginning and becoming unstable as the robot moves away from the second target.

For T15 to T16, it can be seen (video) how the hierarchy is violated by DHQP. The first end-effector is reaching for the target at [0, 2][m] and [0, 2 + 0.001] respectively. The robot is kinematically able to reach [0, 2]. While for GN-QN the first three joints do not move since they are fully occupied by the level 1 task, for DHQP these joints visibly move as they are involved in the achievement of the level 2 task.

For T20, both GN-QN and DHQP track the target in a stable manner. Fig. 5 shows the better convergence of GN-QN of the right hand (the remaining tasks are not shown since they are either converged onto their targets or within their prescribed bounds, for both methods). Especially when the target is in reach GN-QN has near zero task error norm due to the switching to the GN-algorithm. DHQP only gets into the proximity of the reachable target. The activation of the tasks and whether they are augmented or not is shown in Fig. 7. If a task is augmented it also means that it is active.

The computation speed on an Intel Core i7-4720HQ CPU @ 2.60GHz with 8 GB of RAM is given in Fig. 6. The computation times lay usually well below 1[ms] even when several tasks are augmented with second order information. However, there are some peaks up to 4.5[ms], especially when either the right hand or the CoM task are switched to the QN-method and a lot of active set iterations take place in LexLSI. This number of iterations seems too large, and we are investigating it. We do not have an optimized version of DHQP for fair comparison, but for a similar size of problem and similar hardware, [START_REF] Herzog | Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid[END_REF] reports a computation time of about 3[ms]. This is coherent with the timing difference reported in [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF] between a dedicated solver and a cascade of LSP.

VII. CONCLUSION

We proposed a new robust method to deal with singularities in prioritized inverse kinematics control schemes. We showed that we are able to solve any robotic setup with any number of hierarchical levels of equalities and inequalities, may they be feasible or not, while most importantly, ensuring numerical stability. Thereby, our approach provides higher accuracy w.r.t current state-of-the-art methods. Furthermore, it allows the use of the fastest off-the-shelf hierarchical leastsquares solvers.

Our approach borrows much inspiration from constrained optimization: the Quasi-Newton method avoids the expensive calculation of the analytic Hessian while being stable, highly accurate with at least linear convergence. Due to our adaptation method, we switch reliably to the quadratically converging GN-algorithm whenever a problem becomes feasible. Thereby, the GN-algorithm and the QN-method are solved by the same fast state of the art hierarchical linear least-squares solver due to proper reformulation.

observed that the QN-method is very forgiving in terms of simplifications in the second order information (we omitted the reduction of the Hessian by J T J ). As long as the second order information is positive definite we will converge at least linearly. However, it would be desirable to either directly update H or even its decomposition R like in [START_REF] Fletcher | A new low rank quasi-Newton update scheme for nonlinear programming[END_REF] in order to improve the convergence behaviour and save computation time.

Our ongoing work is dedicated to extending our method to dynamic control.

Fig. 1 .

 1 Fig. 1. Robot for T1 (left) and T2 (right) for GN-QN. The robot can translate along the long axis of the black slider. The other joints are revolute. The green end-effector is tracking the green target while the blue endeffector is tracking the blue target. In both frames, the targets are at their turning points [2, 2][m] (blue ball) and [-2, 2][m] (green ball). The robot is able to just reach the blue target for T1 while for T2 both targets remain unreachable. This is due to the position of the shoulder being bound inside the white box. length 1[m]. Their task hierarchy then looks as follows:Hierarchy A (T1, T3 to T19) and B (T2) (2D robot) 0) 4 trust region limits ∆ ≤ I q ≤ ∆ (GN and GN-QN) or 4 velocity limits qvl ≤I q ≤ qvl (DHQP) 1) 2 equality constraints on blue end-effector as follows:J ef2 q = f ef2

Fig. 3 .

 3 Fig. 3. Hierarchy T1, norm of task errors for level 1 and 2 with different methods.

Fig. 4 .

 4 Fig. 4. T2, norm of task errors for level 1, 2 and 3 for GN-QN and DHQP.

Fig. 5 .

 5 Fig. 5. T20, norm of task error for the right hand on hierarchy level 3. GN-QN moves closer to the target than DHQP.

Fig. 6 .

 6 Fig. 6. T20, control loop times in [ms]. The maximum loop times are 4.5[ms] for GN-QN, 4.0[ms] for LexLSI and 0.5[ms] for LexLSAUG2. The maximum number of active set iterations in LexLSI is 49.

Fig. 7 .

 7 Fig. 7. T20, map of activity (light gray) and QN-method gray).

This research is supported in part by the CNRS-AIST-AIRBUS Joint Research Program, and the EU H2020 COMANOID project.1 CNRS-AIST JRL (Joint Robotic Laboratory) UMI3218/RL, Japan 2 CNRS-University of Montpellier, LIRMM UMR5506, Interactive Digital Human, France


Andreas Orthey

Adrien Escande

Eiichi Yoshida

Quotient-Space Motion Planning

A motion planning algorithm computes the motion of a robot by computing a path through its configuration space. To improve the runtime of motion planning algorithms, we propose to nest robots in each other, creating a nested quotientspace decomposition of the configuration space. Based on this decomposition we define a new roadmap-based motion planning algorithm called the Quotient-space roadMap Planner (QMP). The algorithm starts growing a graph on the lowest dimensional quotient space, switches to the next quotient space once a valid path has been found, and keeps updating the graphs on each quotient space simultaneously until a valid path in the configuration space has been found. We show that this algorithm is probabilistically complete and outperforms a set of state-of-the-art algorithms implemented in the open motion planning library (OMPL).

I. INTRODUCTION

Motion planning algorithms are fundamental for robotic applications like product assembly, manufacturing, disaster response, elderly care or space exploration.

A motion planning algorithm takes as input a robot, its configuration space, an environment, a start and a goal configuration, and computes as output a path between start and goal if one exists [START_REF] Lavalle | Planning Algorithms[END_REF]. This computation is NP-hard [START_REF] John H Reif | Complexity of the mover's problem and generalizations[END_REF] scaling exponentially with the number of dimensions of the configuration space. Thus, the more degrees-of-freedom (dof) a robot has, the higher the runtime of the motion planning algorithm will be. This can become the limiting factor of any robotics application. It is therefore important to find suitable decompositions of the configuration space, such that a planning algorithm can quickly discover the relevant parts of the configuration space.

We developed a new decomposition of a given configuration space M, which decomposes M into a sequence of nested subspaces. We observe that any configuration space M can be written as a product of subspaces

M = M 1 × • • • × M K (1)
This suggests we can decompose the configuration space in the following way: Start with the product of subspaces and successively remove one subspace after another. This leads to a sequence of nested subspaces as

M 1 ⊂ M 1 × M 2 ⊂ • • • ⊂ M 1 × • • • × M K (2)
Each subspace in this sequence is called a quotient-space, and the sequence itself is called a quotient-space decomposition [START_REF] Munkres | Topology[END_REF].

The authors are with the CNRS-AIST Joint Robotics Laboratory, UMI3218/RL, Tsukuba-shi 305-8560, Japan (e-mail: {andreas.orthey, adrien.escande, e.yoshida}@aist.go.jp This research was supported by Grant-in-Aid for JSPS Research Fellow 16F116701 It turns out that each quotient-space can be represented by nesting a simpler robot inside the original robot. The prototypical example is a rigid body free-floating in space. The configuration space is SE(3) = R 3 × SO(3), and we can decompose it as R 3 ⊂ R 3 × SO [START_REF] Munkres | Topology[END_REF]. The subspace R 3 is called the quotient-space and represents a sphere nested inside the rigid body, abstracting the orientations of the rigid body.

Such a decomposition is advantageous: Imagine the sphere being infeasible at point p ∈ R 3 . Then the rigid body is infeasible at all configurations inside the subspace p×SO [START_REF] Munkres | Topology[END_REF]. We call this the necessary condition of nested robots.

We have developed a new algorithm, called the Quotientspace roadMap Planner (QMP), which is able to exploit such a quotient-space decomposition. We first show how to build a quotient-space decomposition for any robot in Sec. III. We then discuss the inner workings of QMP in Sec. IV, prove that QMP is probabilistically complete (Sec. V), and we develop three heuristics to improve its runtime (Sec. VI). Finally, we demonstrate that QMP (Sec. VII) can be applied to freefloating rigid bodies, free-floating articulated bodies, fixedbase serial chains and fixed-base tree chain robots.

II. RELATED WORK

We review two categories of papers. First, we review quotient-space decompositions and their application to motion planning. Second, we review alternative decomposition methods.

Quotient-space decompositions are ubiquitous in mathematics, appearing as quotient-groups in algebra, filtrations in linear algebra, or nests in functional analysis. The construction of simplicial complexes in algebraic topology is a prominent example of a quotient-space decomposition.

The application of quotient spaces to continuous spaces and decision making has been originally developed by [START_REF] Zhang | The quotient space theory of Fig. 11. Fixed-base serial chain: KUKA LWR 7-dof benchmark. Fig. 12. Fixed-base tree chain: Baxter 14-dof benchmark. problem solving[END_REF] and [START_REF] Yao | Granular computing: perspectives and challenges[END_REF]. In robotics, quotient-spaces have been used, albeit under different names. Bretl proposed a two-level decomposition: first a path on a stance graph is planned and then configurations along the path are sampled [START_REF] Bretl | Motion planning of multi-limbed robots subject to equilibrium constraints: The free-climbing robot problem[END_REF]. The stance graph can be seen as a quotient space of the configuration space by the stance subspaces. Grey et al. [START_REF] Michael X Grey | Footstep and motion planning in semi-unstructured environments using randomized possibility graphs[END_REF] use a twolevel quotient space decompositions, embedding the torso inside a humanoid robot. A similar idea can be found in Tonneau et al. [START_REF] Tonneau | An Efficient Acyclic Contact Planner for Multiped Robots[END_REF], approximating a robot by a simpler model. Both methods are similar to ours, but use only a two-level decomposition, and do not define the nesting procedure for general robots.

The closest approach to ours is the multi-level decomposition scheme by [START_REF] Zhang | Motion planning of human-like robots using constrained coordination[END_REF] whereby a sequence of nested robots is created. Their planning algorithm starts with the lowest dof robot, computes a path, and uses this path as a constraint for the next bigger robot. This approach can neither deal with non-simple paths (see Fig. 4), nor with spurious paths (see Fig. 5, Sec. VI). It is thus not complete.

Closely related is the exploration/exploitation tree (EET) algorithm [START_REF] Rickert | Balancing exploration and exploitation in sampling-based motion planning[END_REF]. The authors compute a sequence of spheres in the workspace, called a tunnel, to approximate the space of collision-free paths. However, this method is not complete, and cannot handle spurious shortest tunnels/paths (see Sec. VI).

The motion planning problem can also be decomposed without using quotient-spaces.

Kunz et al. [START_REF] Kunz | Hierarchical rejection sampling for informed kinodynamic planning in high-dimensional spaces[END_REF] use a hierarchical rejection sampling approach to improve the Informed-RRT* algorithm [START_REF] Gammell | Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[END_REF]. While the focus is different from ours, their method is methodically similar to our algorithm, where samples are discarded if they are not necessary feasible.

Our work is closely related to low-dimensional sampling techniques which guide configuration space sampling. The algorithm by van den Berg and Overmars [START_REF] Jur | Using workspace information as a guide to non-uniform sampling in probabilistic roadmap planners[END_REF] precomputes narrow passages of the workspace and uses thoses passages to sample the space more densely in those areas. A similar idea can be found in [START_REF] Zucker | Adaptive workspace biasing for sampling-based planners[END_REF], where the authors discretize the workspace, compute a shortest path in the workspace between start and goal configuration, and then sample from a cell in workspace in proportion to the cell's distance to the shortest path. Closely related is also the dimensionality reduction method by [START_REF] Orthey | Motion Planning and Irreducible Trajectories[END_REF], which considers ignoring paths not having a minimal swept volume.

The KPIECE algorithm [START_REF] Ioan | Kinodynamic motion planning by interior-exterior cell exploration[END_REF] is another example of a hierarchical decomposition. The environment is divided into smaller and smaller boxes until a certain threshold is reached. A small box corresponds to workspace points near to a boundary. Those areas are sampled more frequently to effectively guide samples towards the configuration space boundary. This algorithm is orthogonal to ours: they decompose the environment, we decompose the robot.

III. QUOTIENT-SPACE DECOMPOSITION

First, we describe the idea of a quotient space and show two applications to the vector space R 2 and the manifold SO(2) 2 . Second, we show how robots can be nested in each other and thereby create a sequence of quotient spaces. Third, we show that being feasible in a quotient space is a necessary condition for being feasible in the configuration space.

A. Quotient Space

Let M be a vector space and C be a subspace of M . Then the quotient space of M by C, denoted by M / C is the space obtained by collapsing all equivalence classes of C in M to zero [START_REF] Munkres | Topology[END_REF]. Collapsing a space is done by creating an equivalence relation ∼ on M : for all x, y ∈ M we have that x ∼ y if x -y ∈ C. This relation creates a partition of equivalence classes on the vector space. The set of equivalence classes is called the quotient space.

y x q 2 R 2 y x q 2 R 2 x R = R 2 / R q 1 q 2
Fig. 1. Visualization of Quotient Space as collapsing of equivalence classes.

M 1 = SO(2) × SO(2) M 0 = M 1 SO(2) Fig. 2.
Quotient Space Decomposition for a 2-dof manipulator. Left Top: Environment with four obstacles. We have visualized three distinct configurations of the robot. Left Bottom: The configuration space with gray areas being infeasible configurations. Right Top: Environment with 1-dof nested manipulator at same configurations. Right Bottom: Configuration space is collapsed onto the quotient space. Being feasible in the quotient space is a necessary condition for feasibility in the configuration space. It can be seen that q 3 is infeasible, thus not fulfilling the necessary condition.

As an example, consider the vector space R 2 and its subspace R. We can partition R 2 into equivalence classes of its subspace R such that two points x, y ∈ R 2 are equivalent if x -y ∈ 0 × R. We visualize this in Fig. 1, where R 2 is first partitioned into the equivalence classes of R (the vertical lines), and then all lines are collapsed to yield R. The point q 2 ∈ R 2 and all points on the dashed line are equivalent and therefore collapsed into the single point q 1 ∈ R. We identify R = R 2 / R to denote the quotient space.

Quotient spaces generalize to manifolds [START_REF] Lee | Introduction to Smooth Manifolds[END_REF]. Consider M = SO(2) × SO(2), the configuration space of a 2-dof fixed-base manipulator in the plane. Two points x, y ∈ SO( 2)

are equivalent if x -y ∈ 0 × SO(2)
. The manifold M can be partitioned into equivalence classes of SO(2), and then each equivalent class is collapsed to yield SO(2) = M SO(2) . This has been visualized in Fig. 2. On the left three different configurations are shown and their position in the configuration space. On the right the quotient space is shown, corresponding to a 1-dof manipulator nested inside the 2-dof manipulator. Two interesting observations can be made in the quotient space: First, the configuration q 3 is infeasible, regardless of how the second joint is moved. Second, there does not exists a path between q 1 and q 2 . This can be inferred solely from the 1-dof manipulators configuration space. This example shows the defining feature of quotient spaces: If a robot is nested inside another robot, then the feasibility of the nested robot is a necessary condition for the feasibility of the other robot 1 . We will proceed to define how robots can be nested in each other, and we will show that this definition indeed leads to the necessary condition for feasibility.

B. Nesting of Robots

A robot R i is nested inside another robot R i+1 if two conditions are fulfilled: First, the configuration space of R i is a subspace of R i+1 , and second, the volume of the body of R i at each configuration must be a subset of the volume of the body of R i+1 .

Let M i be the configuration space of robot R i , and let

V i (p) ⊂ R 3 be the volume of the body of robot R i at configuration p ∈ M i . Definition 1 (Nested Robot). Let R i , R i+1 be given. We say that R i is nested in R i+1 , denoted as R i ⊆ R i+1 , if (1) M i+1 = M i × C i+1 such that M i = M i+1 C i+1 (2) V i (p) ⊆ V i+1 (p • q) for any p ∈ M i and q ∈ C i+1
whereby the operation • is the cartesian product defined as p • q = (p, q) ∈ M i+1 .

Given a robot R and a sequence of nested robots

R 1 ⊆ • • • ⊆ R K = R, the configuration spaces define a decompo- sition of M as M 1 ⊆ • • • ⊆ M K (3)
This decomposition will be called a quotient-space decomposition.

C. Necessary Conditions

From Definition 1 we can infer the key property of the quotient-space decomposition: if a nested robot is infeasible, then so is the original robot.

Let E denote the environment, the subset of R 3 containing obstacles. We say that robot R i is feasible at configuration 1 Zhang and Zhang [START_REF] Zhang | The quotient space theory of Fig. 11. Fixed-base serial chain: KUKA LWR 7-dof benchmark. Fig. 12. Fixed-base tree chain: Baxter 14-dof benchmark. problem solving[END_REF] call this the falseness-preserving property of quotient space decompositions Theorem 1 implies that if a configuration p ∈ M i is infeasible, then so is the subspace p × C i+1 . We can exploit this fact to ignore subspaces of the configuration space, thereby developing a motion planning algorithm with lower runtime.

q ∈ M i if V i (q) ∩ E = ∅. Theorem 1 (Necessary condition of nested robot). If R i ⊆ R i+1 , p ∈ M i , and V i (p) ∩ E = ∅ (robot R i is infeasible at p), then V i+1 (p • q) ∩ E = ∅ for any q ∈ C i+1 (robot R i+1 is infeasible everywhere). Proof. Since V i (p) ∩ E = ∅ and V i (p) ⊆ V i+1 (p • q) for any q ∈ C i+1 , it must follow that V i+1 (p • q) ∩ E = ∅.

IV. QUOTIENT-SPACE ROADMAP PLANNER

The Quotient-Space roadMap Planner (QMP) works as depicted in Fig. 3. On the top right the configuration space of the 2-dof fixed-base manipulator is shown with a start configuration (green) and a goal configuration (red). The corresponding start and goal configurations on the quotientspace are shown in the top left figure. In the first step, a graph is grown on the quotient space (middle left). Once a valid path has been found between start and goal configuration, a second graph is grown on the configuration space (middle right), whereby the samples are constrained to lie above the quotient-space graph. Both graphs are simultaneously grown (bottom left), until a valid path has been found on the configuration space (bottom right), or until a time limit has been reached. For more than two quotient-spaces, this idea is iteratively continued.

A. Quotient-Space Roadmap

To simplify the algorithmic development, we group each quotient-space with its associated objects into a tuple called the quotient-space roadmap.

Let M be the N -dimensional configuration space of robot R. We consider a nested sequence of

K robots R 1 ⊂ • • • ⊂ R K = R, such that M is decomposed into a sequence of quotient spaces as M 1 ⊂ • • • ⊂ M K = M.
To each quotient space M k we associate a start configuration q I k , a goal configuration q G k , a graph G k , a shortest path p k on G k between q I k and q G k , and a density

V k defined on G k-1 × C k as V k = |G k | µ(C k )L k-1 (4)
whereby

|G k | are the number of vertices of G k , L k-1 is the sum of all edge lengths of G k-1 , and µ(C k ) is the n k - dimensional measure of C k .
The density is used to decide which graph should be grown next.

We group all elements together into the quotient-space roadmap

Q k = {q I k , q G k , C k , M k , G k , p k , V k , Q k-1 } (5)
with

C k = M k M k-1 , M 0 = ∅, Q 0 = ∅.

B. Algorithmic Development

QMP is an adapted version of the probabilistic roadmap planner (PRM) for quotient space decompositions. We first summarize the workings of PRM, then show how QMP can be built from it.

A simplified version of PRM is depicted in Algorithm 1. While a planner terminate condition (PTC) 2 has not been reached (Line 2), the algorithm grows a graph G on the configuration space M of robot R (Line 3). If there exists a path on the graph between start and goal configuration (Line 4), then this path is returned (Line 5). If the PTC is reached then PRM fails and returns an empty path.

The growing of the graph is depicted in Algorithm 2. A configuration q rand is sampled from the configuration space M (Line 1). If this configuration is valid (Line 2), then it is added to the graph (Line 3) and the nearest R configurations Q R are searched (Line 4). The graph is extended in a straight line from each q near ∈ Q R (Line 5) towards q rand until it hits an obstacle (Line 6). The last configuration before the obstacle becomes q new , and the edge between q near and q new is added to the graph (Line 7).

QMP is depicted in Algorithm 3. An empty priority queue is constructed (Line 1), and the k-th quotient roadmap is initialized (Line 3) and added to the queue (line 4). While no path between start and goal has been found (Line 5), we pop the quotient roadmap with the smallest density from the queue (line 6), grow its graph (line 7) and push it back onto the queue (line 8). Then we check if there exists a path on the current quotient space (line 9); if yes, we construct its solution (line 10), and we continue to the next quotient roadmap. For k = 1 the algorithm is equivalent to PRM. For k > 1, multiple quotient spaces are inside the queue, and depending on the density function we pop one quotient space and grow its graph. The algorithm terminates if either the path p K has been found, or if the PTC is reached, in which case p K = ∅ is returned.

The growing of the quotient space graph is depicted in Algorithm 5. Instead of sampling M k as in the PRM, we sample instead C k uniformly, and we pick one configuration from the graph G k-1 on M k-1 (Line 1). The SampleGraph samples a uniform vertex from the graph G k-1 . Then a random incoming edge is chosen, and a configuration uniformly on the edge is sampled. This is called Random-Vertex-Edge (RVE) sampling [START_REF] Leskovec | Sampling from large graphs[END_REF]. The cartesian product • merges the two configuration to yield a configuration on M k . The rest of the algorithm (line 2-7) operates as the PRM algorithm, with two exceptions. First, the R-Nearest-Neighbors method measures distance not by euclidean distance on M k , but by the graph distance on G k-1 plus euclidean distance on C k (Line 4). Second, the Connect method does not interpolate along a straight line, but interpolates along the edges of the graph G k-1 , while interpolating on C k using a straight line. For each vertex crossed on G k-1 we add another configuration. The Connect method then returns a piece-wise linear (PL) path on M k . For each edge along this PL-path we add one edge to the graph G k (Line 7).

Interpolating along the graph instead of using a straight line should be seen as a change in the metric on M k . While a standard euclidean metric is agnostic about the graph on M k-1 , our graph interpolation metric utilizes the knowledge about G k-1 to improve the metric computation.

Algorithm 1 PRM(q I , q G , M) [START_REF] Steven | Rapidly-Exploring Random Trees: Progress and Prospects[END_REF] 1: INIT(G, q I ) 2: while ¬PTC do 3:

GROWPRM(G, M)

4:

if ISCONNECTED(q I , q G , G) then 5:

return PATH(q I , q G , G)

6: return ∅ Algorithm 2 GrowPRM(G, M) 1: q rand ← SAMPLE(M) 2: if ¬ISVALID(q rand ) then return 3: ADD VERTEX(q rand , G) 4: Q R ← R-NEARESTNEIGHBORS(q rand , G) 5: for q near ∈ Q R do 6:
q new ← CONNECT(q near , q rand ) 7:

ADD EDGE(q near , q new , G)

C. Implementation Details

Our software uses the Klamp't [START_REF] Hauser | Robust contact generation for robot simulation with unstructured meshes[END_REF] physical simulator, and the open motion planning library OMPL [START_REF] Ioan A S ¸ucan | The open motion planning library[END_REF].

The nesting of robots has to be prespecified as a set of Unified Robot Description Format (URDF) files along with its subspaces. Each subspace is represented by an OMPL space, and our algorithm iterates through

Algorithm 3 QMP(q I 1,••• ,K , q G 1,••• ,K , M 1,••• ,K ) 1: Q ← PRIORITY QUEUE 2: for k = 1 to K do 3: Q k = INIT(q I k , q G k , M k , Q k-1 ) 4: Q.PUSH(Q k ) 5:
while p k == ∅ and ¬PTC do 6:

Q least = Q.POP 7: GROWQMP(Q least ) 8: Q.PUSH(Q least ) 9:
if ISCONNECTED(Q k ) then 10:

p k = PATH(q I k , q G k , Q k ) 11: return p K Algorithm 4 Init(q I k , q G k , M k , Q k-1 ) 1: return {q I k , q G k , M k M k-1 , M k , ∅, ∅, 0, Q k-1 }
them computing the quotient spaces. We currently support the following quotient space computations:

R 3 = SE(3) SO(3) , R 2 = SE(2) SO(2) , R n-m = R n / R m , SE(3) = SE(3) × R n / R n , and SE(3) × R n-m = SE(3) × R n SE(3) × R m , with n, m ∈ N and n > m > 0.
Our algorithm terminates after a path has been found on the configuration space, or a timelimit T has been reached.

V. PROBABILISTIC COMPLETENESS

A motion planning algorithm is probabilistically complete if the probability that the algorithm will find a path if one exists approaches one as the number of sampled points increases. We will show that QMP is probabilistically complete by alluring to the probabilistic completeness of PRM [START_REF] Svestka | On probabilistic completeness and expected complexity for probabilistic path planning[END_REF].

The main difference of QMP and the PRM on the configuration space is the choice of a sampling sequence. For PRM the sampling sequence is dense in the configuration space, while it is not true for the sampling sequence generated by QMP. However, we will show that the sampling sequence by QMP is dense in the space of feasible configurations, thus making QMP probabilistically complete.

Let M be the configuration space and U ⊆ M be a subset. We say that U is dense in M if cl(U ) = M whereby cl is the closure of U . Let α : N → M be a random sequence. We say that the sequence α is dense in Each quotient space M 1 , • • • , M K uses a sampling sequence as α (k) : N → M k . We will show that this sequence is dense in Mk = {q ∈ M k |V(q) ∩ E = ∅}, the feasible space of M k . Theorem 2. α K is dense in MK Proof. By induction for K = 1 α (1) is dense in M 1 , and since M1 ⊆ M 1 , α (1) is dense in M1 . For K > 1, we assume α (K-1) is dense in MK-1 . Consider the sampling domain of α (K) , defined as S = {α

M if the set {α n , n ∈ N} is dense in M [1]. Algorithm 5 GrowQMP(Q k ) 1: q rand ← SAMPLEGRAPH(G k-1 ) • SAMPLE(C k ) 2: if ¬ISVALID(q rand ) then return 3: ADD VERTEX(q rand , G) 4: Q R ← R-NEARESTNEIGHBORS(q rand , G k , G k-1 ) 5: for q near ∈ Q R do 6: {q n1 , • • • , q n J } ← CONNECT(q near , q rand , G k-1 ) 7: ADD EDGES(q n1 , • • • , q n J , G k )
(K-1) n × C K |α (K-1) n ∈ MK-1 , n ∈ N}. Due to the necessary condition of nested robots (Theorem 1), we have that if r • p ∈ MK then r ∈ MK-1 . Thus MK ⊆ S. Since α (K) is dense in S by definition, α (K) is dense in MK .
Finally, we need to make sure that every α (k) is an infinite sequence. This is only guaranteed if each quotient space from the priority queue in Algorithm 3 is chosen infinitely many times. A sufficient condition is to choose a density function strictly monotonically increasing, as we did in Eq. 4.

VI. QUOTIENT-SPACE HEURISTICS

We first discuss intricacies of quotient-spaces, and then use this knowledge to design heuristics for QMP.

A. Quotient-Space Intricacies

Solution paths on quotient-spaces do not behave as nicely as one would expect. We show two examples where the solution path is non-simple or spurious.

1) Non-simple Paths on Quotient-Space: A non-simple path is a path with self-intersections. We say that a path p : [0, 1] → M is simple, if it is injective, i.e. for any t, s ∈ [0, 1]: if p(t) = p(s) then t = s [START_REF] Perdigao | Differential geometry of curves and surfaces[END_REF]. A path not being simple is called non-simple.

While non-simple paths do not occur in configuration spaces, they can occur in quotient spaces. Fig. 4 shows an example, where a rectangular shaped planar robot needs to move from a start configuration (green) to a goal configuration (red). The configuration space is SE(2) and the quotient space is R 2 = SE(2) SO(2) , obtained by nesting a disk inside the rectangle. The solution path is simple on SE(2), but non-simple on R 2 .

2) Spurious Shortest Path on Quotient-Space: Spurious paths are solution paths on the quotient-space which are infeasible on the configuration space. Let M 1 be a quotient space of the configuration space M 2 and let p 1 : [0, 1] → M 1 be the shortest feasible path on the quotient space between q 1 I and q 1 G . We say that p 1 is feasible on the configuration space, if there exists a feasible path p 2 : [0, 1] → p 1 × C 2 . If p 1 is infeasible on the configuration space, we call it spurious.

Consider the example in Fig. 5, where an L-shaped robot needs to move through an environment with two passages above and below a rectangular obstacle (Left). The quotientspace is represented by a disk nested in the L-shaped robot. The shortest path on the quotient space is spurious (Middle), since the only feasible path goes above the obstacle (Right).

B. Heuristics

Keeping the intricacies in mind, we design three heuristics which we found to be beneficial to reduce the runtime of QMP.

1) Diminishing-Time Shortest Path Sampling: If the shortest path is spurious this often can be established quickly. We add a diminishing time heuristic to sample first the shortest path on the quotient-space with probability p = 0.8 and decrease this probability over time with the amount of samples taken from the shortest path.

2) Graph Thickening: Often a solution path on the quotient-space is spurious, but a solution path nearby is not spurious and contains a solution on the configuration space. To alleviate this problem, we introduce an -graph thickening. Given a random sample q from the graph, we add an offset uniformly distributed from a ball around q with radius . This helps in finding nearby solutions. We have set = 0.01 for free-floating robots and = 0.1 for fixed-base robots.

3) Increasing Clearance: In some cases it is advantageous to inflate the shape of the nested robot such that the clearance of the solution path on the quotient space is increased. One has to be careful with this heuristic, since it tradesoff completeness with efficiency. We apply this only for the free-floating robots in our experiments by increasing the size of the inscribed sphere by δ = 1.2.

VII. EXPERIMENTS

We perform experiments on four different environments as shown in Fig. 8: A free-floating rigid body, a freefloating articulated body, a fixed-base serial chain and a fixed-base tree chain robot. The problem is to find a path for each robot from the initial configuration (green) to the goal configuration (red). All experiments have been perfomed on a quad core 2.6 Ghz laptop with 31 GB working memory. We perform collision detection using the proximity query package (PQP) [START_REF] Larsen | Fast proximity queries with swept sphere volumes[END_REF].

A. Free-floating rigid body

The double L-shape is a rigid body consisting of two L-shapes glued together. The environment is a wall with a small square hole in it, as depicted in Fig. 8. This problem was introduced by [START_REF] Jur | Using workspace information as a guide to non-uniform sampling in probabilistic roadmap planners[END_REF] as an example of a narrow passage planning problem. We decompose the double Lshape as R 3 ⊂ SE(3), as depicted in Fig. 6. We compared our algorithm QMP with three state of the art algorithms implemented in the OMPL software framework: PRM [START_REF] Kavraki | Probabilistic roadmaps for path planning in highdimensional configuration spaces[END_REF], bidirectional rapidly-exploring random tree (RRTConnect) [START_REF] James | RRT-connect: An efficient approach to single-query path planning[END_REF] and the expansive space trees planner (EST) [START_REF] Hsu | Path planning in expansive configuration spaces[END_REF]. Fig. 9 shows that only QMP and PRM solved all runs, and that QMP has a median runtime of 2.5s, compared to 27s for PRM.

B. Free-floating articulated body

The mechanical snake is a 10-dof articulated body which has three links, connected each by two revolute joints with limits (See Fig. 6). The snake can freely translate and rotate in space. We found the most effective decomposition to be R 3 ⊂ SE(3) × R 4 . QMP has a median runtime of 30s (67s for EST). However, we can see that QMP has two outliers.

C. Fixed-base serial chain: KUKA LWR

The KUKA Ligthweight Robot (LWR) is a 7-dof manipulator. We consider transporting a windshield through a factory simulating a car manufacturing task. Our decomposition is R 5 ⊂ R 7 . In the experiments, only QMP was able to solve all runs, with a median time of 18s (164s for EST).

D. Fixed-base tree chain: Baxter

Baxter is a two-arm fixed-base robot with a tree kinematic chain having two serial chains of 7 dof for each arm, and having a total of 14 dof. We consider a maintenance task, where Baxter needs to move its arms into small openings of a defect water tank. We decompose Baxter as R 5 ⊂ R 7 ⊂ R 12 ⊂ R 14 as depicted in Fig. 7. Only QMP was able to solve all runs, with a median runtime of 4.5s compared to 94s for RRTConnect.

VIII. CONCLUSION

We have introduced the quotient-space decomposition, a configuration space decomposition based on nested robots. To exploit this decomposition we developed the Quotientspace roadMap Planner (QMP). We showed that QMP is probabilistically complete and that QMP has lower runtime on four environments compared to state-of-the-art motion planning algorithms implemented in the OMPL software.

We like to extend QMP in three directions. First, the quotient-space decomposition of a configuration space is not unique, and has to be specified by a human operator. We like to investigate which decomposition is optimal, and automate its specification. Second, we like to generalize our approach to closed kinematic chains, constraint manifolds, and dynamic constraints. Finally, we like to apply the quotientspace decomposition to environments changing over time, where a roadmap on the quotient space could improve fast decision making.

Fig. 3 .

 3 Fig. 3. Schematic of QMP for a two-level decomposition. See text for clarification.

Fig. 4 .

 4 Fig. 4. Non-simple path on Quotient-Space: See text for clarification.

Fig. 5 .

 5 Fig. 5. Spurious shortest path in Quotient-Space: An environment for an L-shaped robot with configuration space SE(2) and quotient space R 2 . See text for clarification.

Fig. 6 .

 6 Fig. 6. Nesting of robots for the Double L-shape, Snake and KUKA LWR with windshield. The darker shade is the nested robot.

Fig. 7 .

 7 Fig. 7. Nesting of robots for Baxter Robot by Rethinking Robotics.

Fig. 8 .

 8 Fig. 8. The four experiments considered in this paper: For each experiment we show the start configuration of the robot (green) and the goal configuration (red). The left column shows the faces of the environment mesh, while the right column shows the edges of the environment mesh.

Fig. 9 .

 9 Fig. 9. Free-floating rigid body: Double L-Shape 6-dof benchmark.

Fig. 10 .

 10 Fig. 10. Free-floating articulated body: Mechanical Snake 10-dof benchmark.

A planner terminate condition (PTC) has to be chosen by a user and can be a time limit, an iterations limit, or a desired cost of the resulting path.