Optimizing Mobility of Robotic Arms in Collision-free Motion Planning

A major task in motion planning is to find paths that have a high ability to react to external influences while ensuring a collision-free operation at any time. This flexibility is even more important in human-robot collaboration since unforeseen events can occur anytime. Such ability can be described as mobility, which is composed of two characteristics. First, the ability to manipulate, and second, the distance to joint limits. This mobility needs to be optimized while generating collision-free motions so that there is always the flexibility of the robot to evade dynamic obstacles in the future execution of generated paths. For this purpose, we present a Rapidly-exploring Random Tree (RRT), which applies additional costs and sampling methods to increase mobility. Additionally, we present two methods for the optimization of a generated path. Our first approach utilizes the built-in capabilities of the RRT*. The second method optimize the path with the stochastic trajectory optimization for motion planning (STOMP) approach with Gaussian Mixture Models. Moreover, we evaluate the algorithms in complex simulation and real environments and demonstrate an enhancement of mobility.


Introduction
Motion planning is an essential topic in robotics. The requirements on it are constantly increasing in the last years, as even stronger cooperation between humans and robots is desired. Thus, the industrial workflow will change in the future. In general, the robot has to adapt to humans and not vice versa. In these scenarios, improved movements are required to increase safety instead of simply avoiding obstacles and reducing path lengths in joint space to save energy. An important criterion in motion planning is manipulability and the resulting avoidance of manipulator singularities, where the robot loses its degrees of freedom. Based on the prevention of singularities, it is also possible to enable Cartesian impedance control at any time, which is Sascha Kaden sascha.kaden@etit.tu-chemnitz.de Ulrike Thomas ulrike.thomas@etit.tu-chemnitz.de 1 Robotics and Human-Machine-Interaction Lab, Chemnitz University of Technology, Chemnitz, Germany a necessary condition for cooperation with humans and to guarantee their safety. Moreover, modern robotic arms with seven degrees of freedom are used for cooperative work because they offer better flexibility due to the redundancy of the seventh joint. However, these are often built like the DLR 1 -light-weight arm LWR 1 and have been alternating revolute and torsion joints, which leads to numerous singularities in their working area. Because of these reasons, motion planning with the prevention of singularities is essential. A simple example of a poor configuration is the fully extended manipulator, where a robotic arm loses its degrees of freedom and can only perform very limited movements as shown in Fig. 2.
Yoshikawa [31] first presented the measure of manipulability for a robot. It describes the proximity to singularities with the concept of the manipulability ellipsoid. The measure of manipulability is applied in a variety of fields of robotics. In [28] it is used for manipulability optimization control to enable Robot-Assisted Minimally Invasive Surgery. Other applications include the optimization of redundant manipulators using dynamic neural networks [8], solving the inverse kinematics of redundant robots constructed according to the DLR manipulator scheme [4], or realizing proximity servoing for manipulators with distance sensors [3]. Another field of application is planning of optimal movements for mobile platforms equipped with a manipulator, as shown in [21] and [1].
Sampling-based motion planning was developed for collision-free motion planning of high-dimensional robots, such as redundant manipulators. In this approach, not the entire Cartesian space is mapped into the configuration joint-space, instead only a valid subspace is constructed from random samples, which will be used for the search of a valid path. Since determining the entire configuration space is too complex. An algorithm to perform single queries with sampling is the Rapidly-exploring Random Tree (RRT), which was developed by LaValle & Kuffner [15]. In this approach a tree of random and valid configurations is grown from the start to the target configuration. Over the years this method was adapted and optimized several times. The first extension was the expansion of two trees with the RRT-Connect algorithm [14], beginning at the start and target configuration simultaneously. To solve the query, only a connection between the trees has to be found, which leads to significantly shorter computation times. A second adaptation is the RRT* [11] where local optimization is performed to shorten the path length in joint space. These two changes have been combined in [13] to reduce calculation times and to find optimal paths at the same time. There also exist many other adjustments, such as [22], where an extension for real-time capability is presented, in which the path is reshaped during execution to generate an optimal path. A further adaption is the "Rapidly-exploring Random Vines" [29], where the nodes know whether they are in front of a narrow passage or a passageway. With the help of this information, a quick exploration in a narrow space is possible.
Apart from sampling-based motion planning, other approaches to optimize trajectories have been developed. The most widely known are covariant Hamiltonian optimization for motion planning (CHOMP) [25] and stochastic trajectory optimization for motion planning (STOMP) [10]. All these optimization techniques share the assumption of an initially non-optimized trajectory between the start and end configuration, which will be continuously optimized. The different optimization methods adapt the trajectory in such a way that it complies with constraints such as collision avoidance and joint limitations, while it optimizes simultaneously other parameters such as distance to obstacles or smoothness costs. The CHOMP method applies covariant gradient techniques to adapt the initial discrete trajectory with respect to the optimization and constraint criteria. In contrast, STOMP does not require gradient information; instead, random trajectories are sampled near the initial one.
Based on the costs of these near trajectories, an update of the original motion is performed. CHOMP and STOMP operate iteratively and will be repeated until all criteria are met. In another approach, particle filters are used for the optimization [12]. A further group of trajectory optimization methods are the Gaussian Processes (GP) [19,20], which have been developed recently. A continuous trajectory is first generated using GP and factor graphs, which is then adapted with a gradient-based optimization algorithm.
In the next section, the calculation of the manipulability and joint limits is presented. This is followed by the motion planning algorithms with optimization of mobility in Section 3. In Section 4 our simulation results are presented, and a conclusion and outlook are offered in Section 5.

State Costs of Mobility
We use two additional criteria to optimize motion planning. First, the manipulability λ and second, the distance to the joint limits ψ. In our motion planning algorithms, costs are always normalized and lie between 0 and 1. We guarantee with this that the calculations are general and independent of the applied manipulator. Furthermore, a reasonable scaling can be realized. For subsequent evaluation and visualization, these criteria are defined as qualities and should thus be maximized.

Manipulability Quality
The manipulability is a predictable and scalar measure of the manipulation ability of a robotic arm [31]. It describes its capability to move within the Cartesian freedoms, in other words, to translate or rotate. We consider a manipulator with n joints or degrees of freedom, which are denoted by θ i , i = 1, . . . , n. The transformation between Cartesian task space velocitiesẋ ∈ R m and the joint velocitiesθ ∈ R n of the manipulator is provided by the robot Jacobian J (θ) ∈ R m×n , where the Jacobian can be interpreted as scaling: From the Jacobian we can calculate the scalar manipulability measure as: Where σ 1 , σ 2 , · · · , σ m are the singular values of the Jacobian and λ(θ ) is related to the manipulability ellipsoid with the axes σ i . If one degree of freedom of the robot is lost, then one of the singular values is equal to zero (σ i = 0) and one axis of the ellipsoid has no length. If m = n, as it is the case with non-redundant manipulators, the calculation can be reduced to λ(θ ) = | det (J (θ)) |. A further description and calculation of the manipulability is provided in [9]. It has to be noted that the magnitude of the measurement depends on the manipulator, since J (θ) represents the correlation of the serial chain of the manipulator and its link lengths. To avoid this dependency, we operate with the normalized manipulability in the range of [0, 1]. For this reason, we divide the determined value by the highest measured manipulability λ max , which we estimated from our experiments in Section 4: In contrast to our former work [9], we pay attention to a numerical problem of the manipulability value. There is a variation of the Jacobian eigenvalues from 1x10 6 to 1x10 −6 and therefore they can cancel each other out. Thus, an almost complete loss of individual Cartesian degrees of freedom cannot be detected.
In Eqs. 4 and 5 two Jacobians can be seen with their corresponding eigenvalues and manipulability. It is obvious that although both Jacobians have the same manipulability λ 1,2 = 1, J 2 has almost lost one degree of freedom.
To overcome this problem, λ(θ ) is devided by the manipulability condition number λ cond (θ ) = σ max σ min ≥ 1. With this ratio, the manipulability is weighted so that the highest eigenvalue is removed and the weight of the smallest eigenvalue is squared. This finally results in the following equation to calculate the manipulability:

Joint Limit Quality
To guarantee the mobility of the manipulator, it is necessary to prevent the joints from moving to their limits. For this, we use the joint limit quality. As a basis of our method, we took the distance from mechanical joint limits function from [26]. However, this method, as well as other presented methods come with the shortcomming of not punishing sufficiently isolated joints at their boundaries. Therefore, we developed a method with exponential costs. The calculation of the joint angle costs is done in several steps. The first step is to calculate the normalized distance from the center of the joint angle range: Where θ i is the ith joint value and θ center,i the corresponding joint center and θ range,i the range. Afterward the distanceŝ ψ i are adjusted through an exponentially growing function, so that the costs are increased when positions close to the joint limits are reached: The weights of the quintic function have been chosen to ensure that the joint costs reach a value of 1 for 80 % displacement from the center of the joint. A further displacement is additionally rated, since in this area there may be a strong restriction of the robot's mobility. The progression of the costs is shown in Fig. 1. From these individual joint costs, the mean valueψ = 1 n n i=1 ψ i is then calculated and a final conversion to joint quality and a case differentiation is made to ensure a minimum quality of zero: The purpose of this weighting is to ensure that an aboveaverage cost is assigned when a single joint approaches its limits. This is important since it is already sufficient for the motion restriction if one joint reaches its end position. Therefore, a quality value of 0 does not imply that a manipulator has reached its joint limits with all joints.

Motion Planning
We separate the motion planning into two parts. First, we use sampling-based motion planning to ensure that a valid path is found, even if narrow passages have to be traversed.
In the second step, we use two optimization strategies to smooth the path and to achieve further optimization of the mobility. However, the problem with optimization strategies is that they get stuck in narrow passages or local minima [17]. With our combination, we achieve the ability to find paths in complex environments and additionally optimize the path according to manipulability and joint limits.

Sampling-based Motion Planning
The here developed sampling-based motion planner is based on the RRT*-Connect [13] and its extension the RRT*-Quick method [7], to achieve a faster convergence rate. First, we extended these RRT-algorithms with state and distance costs to achieve additional optimization. The The main process of our RRT is in Algorithm 1 illustrated. Our RRT algorithm is designed for parallel computation to achieve fast and efficient calculations. This means that all modules like graph (tree), collision detection, sampling, trajectory calculation, or distance and state cost calculation of the motion planner are thread-independent and can therefore run in parallel. Because of this structure, we grow a tree always in parallel by several execution threads, since a parallel operation takes advantage of the local optimization of the RRT*. Moreover, we simultaneously expand from the start and target configuration for k nodes and run both threads in completion. We justify this additional separation and simultaneous calculation from the publication [6], which shows a limit on the parallel speedup for the expansion speed of the tree. Therefore, if enough processor cores are available, it is recommended to expand these two trees concurrently. Afterward, we test for a connection between both trees. If this is possible, a valid path was found and as the last step, we run a path optimization, which is described in Section 3.2. Otherwise, we expand again or abort the planning if a maximum number of attempts is exceeded. The EXTENDTHREAD(T , k nodes ) function performs the expansion of the passed tree T , where it calls k nodes × EXTEND(T ). This method then applies for the extension of the RRT algorithm to a new configuration, as illustrated in Algorithm 2. For the generation of samples, we use one of our two developed sampling strategies. For both, we take a random configuration in the first step, where we set a goal bias of 5 %. Afterward, one of our developed sampling strategies either Manipulability bias or Manipulability for i ← 1 to attempts do 5: JOIN(T start , T goal ) 8: if CONNECT(T start , T goal ) then 9: OPTIMIZE(T start , PATH(T start )) 10: return PATH(T start ) 11: return failure Random Gradient Descent (RGD) is used to evaluate or adjust the samples. Both sampling strategies are illustrated in Algorithm 3.

Manipulability Bias
This method is inspired by the manipulability bias of [16], which is constructed for the sampling algorithm: probabilistic roadmap (PRM). However, we do not think their concept is suitable for the RRT approach since samples have to be generated for the entire configuration space like it's the case with PRM. For this reason, we developed a new method to suit the RRT. As first, we define a pseudo probability value ρ that determines the usage of a sample: where δ bias is a user defined threshold value. ρ results from the manipulability value of a configuration and δ bias . We apply an additional δ bias to allow sampling of configurations with low manipulability. For example, the robot may have to pass through a narrow passage, where only a limited manipulability can be achieved, see Fig. 2.

Manipulability RGD
As a second method, we use the manipulability RGD, which shifts the random configuration, such that the manipulability increases. This algorithm is inspired by the RGD algorithm from [27], although their approach is designed for Cartesian constraints. Initially, a random direction vector θ ray with a small length is first generated. A new configuration is then created with: θ new = θ rand +θ ray . When the manipulability increases (|λ(θ new )| > |λ(θ rand )|), a continuing translation into this direction is performed. This is repeated until the maximum displacement δ RGD is reached, or no more enhancement of λ occurs. To set an adaptive value for the maximum displacement, we link it to the step size of the RRT. As a standard case, we apply 50 %.

State and Distance Costs
The next steps are: Cost calculation and linking of θ rand to the tree. From sample θ rand , the closest neighbor in the tree T is searched. The distance criterion in the search is Euclidean distance in joint space and not the introduced cost value of the nodes, as these costs are distinct. Then the new configuration θ new for the linkage is determined using the STEER algorithm of the RRT. Based on θ new a range search within the step size is performed to determine the near nodes Θ near . These nodes are then extended by their ancestor nodes. The degree of the ancestors m is a fixed value and for each θ near ∈ Θ near do 12: if TRAJECTORYCOLFREE(θ new , θ near ) then 13: θ new .cost = CALCCOST(θ new , θ near ) 14: T .ADDNODE(θ new ); 15: T .ADDEDGE(θ near , θ new ); 16: BREAK 17: if valid connection found then 18: return true is set to three by default, as it is in the original publication [7]. In addition to [7], we sort the nodes by their cost to get the best possible connection and thus save computing time. Therefore, the first valid linkage is also the best possible one. If a valid connection was found, a cost calculation of θ new will be performed, which represents another important new aspect in our RRT algorithm. State costs are calculated in addition to the distance costs and assigned to each tree node during the expansion phase using a cost function, see Algorithm 4. We interpret the state costs of the RRT as a for i ← 1 to attempts do 11: θ diff = θ ray ← RANDOMRAY() 12: if λ(θ rand + θ diff ) > λ(θ rand ) then 13: while λ(θ rand + θ diff ) increase and |θ diff | < δ RGD do 14: θ diff += θ ray 15: return θ rand + θ diff ) Algorithm 4 calcCost.
In this publication we use c λ (θ ) and c ψ (θ ) as state costs to optimize the path. After the calculation of the states, the distance costs are determined. Here we again combine two kinds of costs, the usual Euclidean distance in the joint space of the standard RRT* and the manipulability as distance costs. For this purpose, the manipulability is summed in 0.5°increments along the path. The final cost of a node is then calculated from the distance, state, and previous cost of the parent node.
After the cost calculation, the new node and its edge are added to the tree. Finally, the rewiring algorithm of the RRT* [11] is applied, whereby the ancestor nodes are also tested and connected if it cause less cost over the new node. Furthermore, collision checking is always additionally performed to ensure a non-colliding robot. Collision calculations are carried out with the flexible collision library (fcl) [23].
By extension of the state and distance costs, we reach an additional optimization of the RRT algorithm. There are no limitations to types and combinations of different costs. In Section 4 we present the results of the sampling-based motion planning with manipulability and joint limits as state costs and the developed sampling strategies.

Optimization Methods
After a valid path has been found, path optimization is performed to increase the manipulability. We developed two methods to optimize the path. First, an optimization using the ability of the RRT* algorithm of further expansion of the tree, we name it: the RRT* manipulability expansion. Secondly, we developed a stochastic optimization consisting of a combination of STOMP [10] and GMMs [18], which are inspired by the Gaussian Process Motion Planning (GPMP) [20]. These two methods run in parallel to achieve short computation times. In Section 4.1 we discuss their results and whether their application or a combination of them is rewarded.

RRT* Manipulability Expansion
For the enhancement of the path, the local optimization capability of the RRT* is used. We take advantage of this by letting the tree grow locally in the vicinity of the found path. Here we do not consider the often mentioned Informed-RRT* [5], as we discovered in our tests that the hyperspheroid often already occupies the whole joint space of a manipulator, since this space is already very limited through the joint limits. The first step is to find all nodes that are located in a radius of r path to the nodes of the previously calculated goal path. By default, we set r path to twice the width of the RRT* step size. In the second step, potential configurations must be generated to extend the tree. For this, we use a spherical Poisson sampler, which is inspired by [2] and [24]. It generates uniformly distributed samples Θ sphere within a sphere based on Poisson disk sampling. Two parameters the sphere radius r s and the disc radius r d are needed. For this purpose, we first perform an oversampling within r s and then delete unnecessary points using r d . An example sphere and its samples are shown in Fig. 3. For each configuration from the set Θ sphere the manipulability is calculated and Θ sphere sorted accordingly. With the u best configurations, the RRT is then extended and the path is optimized. The process is illustrated in Algorithm 5and as input the following data is required: -Θ path , the nodes of the determined path, -T the tree to be expanded and u the number of extensions of a node. for each θ sphere in Θ near until n Nodes do 8:

Stochastic Optimization
Furthermore, we make use of a stochastic optimization, which is based on the STOMP algorithm [10], but has two major differences to it. First, unlike the original, we do not use a linear connection as the initial path, instead, we take the optimized and collision-free path from the samplingbased motion planner. As a result, this optimization always runs at the end, even if an RRT* manipulability expansion takes place. Second, we do not generate the random trajectories of the stochastic optimization with a uniform distribution like STOMP, instead, we apply a GMM on the input trajectory to generate our trajectories. Hereby, it is possible to set a variable covariance over the individual trajectory steps. Which is to calculate the covariances of the GMM as a function of the individual trajectory step costs. Using the GMM, we create our K random trajectories to optimize the path.
The optimization consist of following steps. First, the initial trajectory is re-interpolated so that there are sufficient grid points. Then K random trajectories are calculated from the GMM for the statistical adjustment. For each grid point, the costs resulting from Eq. 12 are calculated.
These consist of distance (c o (θ ), manipulability (c λ (θ )) and joint (c ψ (θ )) costs. Finally, the path is optimized based on the cost of all random trajectories. This optimization is described in detail in [9], although no joint costs are used in our previous version. In the standard case, we repeat this calculation twice to obtain an optimized trajectory concerning the costs from Eq. 12 and smoothness of the path.

Results and Discussion
We evaluated our algorithms in different simulation environments with the iiwa manipulator from KUKA. First, we selected a cluttered environment to compare the different algorithms of the motion planner. This environment is shown in Fig. 4 and represents a human-robot cooperation cell (HRC), where the robot is attached to the ceiling to provide the option of multi-robot coordination. First, individual sub-algorithms are evaluated to optimize the manipulability. In the second part, the final methods are tested in further simulation and real scenarios, see the scenarios in Figs. 5 and 6. And thirdly, we perform our motion planning in a real environment. For all evaluations, we use a workstation with two Intel Xeon E5-2670 v3 CPUs, each with 12 processor cores and 64 GB Ram. All tests run with 24 threads. For the implementation, we use C++ as programming language.

Evaluation of Sub-algorithms
To compare the individual algorithms for an optimization of the manipulability, we use the simulation environment in Fig. 4. All tests are repeated 15 times, since the calculation with the RRT is not deterministic. First, we perform a motion planning without optimization of manipulability. The results are shown in Fig. 7, in (a) the computation times are shown as box plot, in (b) is the progression of the manipulability of all 15 paths illustrated and in (c) is the course of the joint qualities of the paths illustrated. It can be seen that λ becomes zero several times and therefore a loss of degrees of freedom of the manipulator occurs (Fig. 8).
Next, the different algorithms to enhance the manipulability are compared. The following scales of the cost calculation are defined: c ψ = 1, c λ = 5 and both distance costs have a scaling of 1. In addition, the step size of the RRT* is defined to be 60°.
First, we compare the two sampling algorithms: Manipulability bias and Manipulability RGD. A threshold δ bias = 0.2 is set for the bias method. For the RGD, the maximum displacement is fixed at 50 % of the step size of the RRT*, which corresponds to a value of δ RGD = 30°. The results of the bias sampling are shown in Figs. 9 and in 10 the outcome of the RGD sampling is illustrated. In this context, it can be seen that the bias sampling achieves slightly better results, which is manifested by lower fluctuations and fewer zero values of the progressions of manipulability. In terms of computational time, both algorithms with manipulability costs take twice as long to compute to the non-optimized version, although this is somewhat greater with RGD. This results from the fact that more expensive sampling algorithms and complex calculation of the state and path costs are used because the number of required collision calculations remains constant. Fig. 7 Simulation results of the HRC scenario in Fig. 4 with a simple sampling and without state costs and only Euclidean distance costs in the joint space. a shows the total calculation time, b and c present the progression of manipulability and joint limits of all 15 paths In the second part, we compare bias and RGD sampling in a subsequent improvement with the RRT* manipulability expansion. For the sphere sampler the radii are set to r sphere = 40°and r disk = 20°. This results in about 700 equally distributed samples within the sphere. Furthermore, we set u = 3 and expand the tree with the three best samples according to the state costs from line 2 of Algorithm 4. The results of optimization with a previous bias sampling are shown in Fig. 11 and with RGD sampling in Fig. 12. The additional optimization of the RRT* leads to a major increase in manipulability, with no zero crossings occurring when bias sampling is used. When comparing the two sampling methods, it is also clearly evident that bias sampling delivers better results, which is due to better manipulability values. The slightly longer computing times of the bias sampling are a result of the increased number of required collision calculations.
In the second last test, we compare the two sampling methods with a subsequent improvement with the stochastic optimization. This optimization performs two iterations. The scaling is chosen as in the paper [9] with the values: α s = 0.04, α d = 10 and σ d = 2.5. The scaling of the state Fig. 8 Mean values of manipulability through 50 runs from the evaluation of the sub-algorithms: a without optimization, b bias sampling, c bias and RRT* opt., d bias and stochastic opt., e bias and both opt., f RGD sampling, g RGD and RRT* opt., h RGD and stochastic opt costs is the same as in the RRT*. This optimization leads again to a major increase in manipulability. Thereby bias sampling again offers better results (Figs. 13, 14 and 15).
The final test is a motion planning with bias sampling, as well as RRT* expansion and stochastic optimization. The results are shown in Fig. 16. It can be seen that optimizing with both algorithms leads to a further enhancement of the manipulability, while also requiring longer computing times.
In addition, 50 runs were performed for all methods to determine the average manipulability values. The results are shown in Fig. 8. Here it can also be seen that the developed methods result in a clear increase in manipulability. It is also evident that bias sampling regardless of the optimization method leads to better results than RGD sampling. Moreover, it can be seen that the optimization methods produce an improvement, with similar results. And lastly, performing both optimizations leads to a further increase of manipulability.
From these results, we conclude first that bias sampling should be used, as it provides better results than RGD sampling, no matter what optimization is done afterward. From the last test, we can see that optimizing with both methods leads partly to better values of manipulability. The question of which optimization should be preferred, cannot be answered finally using the environment from the scenario in Fig. 4. Because of this, both optimizations and their combination are tested in the next section.

Evaluation of Further Environments
Finally, we evaluate two more scenarios to demonstrate the capabilities of the motion planner. We prove with these environments that a valid path is always found, and that the manipulability is maximized whenever possible. For each scenario, four cases are calculated, firstly without optimized manipulability, secondly with bias sampling and RRT* expansion, thirdly bias sampling and          Fig. 6 in a a computation without Manipulability, in b with bias sampling and additional RRT* expansion and in c with bias sampling and stochastic optimization stochastic optimization, and fourthly bias sampling and both optimizations. Again, 15 runs are calculated in each case. The joint costs of both scenarios are never close to zero and therefore not presented (Fig. 17).
The first scene with manipulator mounted on a table, shown in Figs. 6 and 18, illustrates the corresponding manipulability of the determined paths. Both optimizations provide an increase in manipulability, but this is significantly more distinct in the RRT* expansion. The last and most complex scene is shown in Fig. 5, where the manipulator has to pass through an archway and therefore a very limited joint space exists. We picked this scenario to show that in difficult environments, like narrow passages, a collision-free path is still found and that the enhancement of the mobility is only an optimization criterion and does not block the path search. In Fig. 19 the diagrams of the manipulability progression of all 15 paths are shown. In addition, the courses of the derived joint values are plotted in Fig 20. In all cases, a valid path was found. Due to the narrow passage of the scenario, the manipulability was optimized only to a small extent. Furthermore, the derivation of the joint values is another feature of our stochastic optimization, which reduces the amplitudes and therefore smooths the contours of the path in joint space. Smoothing occurs in all stochastic optimizations and was therefore shown only once. The results of the average values of the manipulability are shown in Fig. 17. It can be seen that the RRT* optimization leads to a much better enhancement compared to the stochastic optimization. Furthermore, the combination of both optimizations does not lead in these tests to any further improvement of the manipulability. Therefore, we conclude that the choice of the optimization method depends on application and environment of the manipulator.

Execution in Real Environment
Finally, we tested the motion planning on a real iiwa manipulator. The structure of the workcell and a sequence of the robot motion is shown in Fig. 21. For this test, we performed a simple path consisting of three sections, where we used manipulability bias sampling and RRT* optimization. During the execution of the motions, we performed an impedance control in Cartesian space and applied external influences on the manipulator by a human user. We see this realization as proof of maximizing the manipulability. Since a complete loss of manipulability would lead to a singularity, which in turn would lead to an unwanted (infinite) acceleration of the manipulator. Figure 21 shows a sequence of the motions of the robot. Furthermore, it can be seen that the manipulator always tries to use nearly rectangular joint configurations, which is an indication of maximizing the manipulability. The entire video of the motion can be accessed on the journal's website.

Conclusion and Outlook
In this paper, we presented various methods to maximize the mobility of a robot, while still providing a collisionfree operation. For us, mobility consists of manipulability and joint limitation costs. These criteria are integrated into a sampling-based motion planner, which consists of an RRT method and optimization algorithms. They combine state and distance costs to optimize the mobility of a robot arm. In the experiments, we proved that we can find paths in complex environments and simultaneously achieve an improvement in mobility. We see this optimized motion planning as a previous step for dynamic planing. We generate paths that have a high degree of freedom to react to unplanned of dynamic obstacles later during the execution. Because dynamic motion planning can only react to unplanned obstacles if the initial path allows freedom to change. In this way, we want to increase safety and flexibility in human-robot cooperation. The proposed algorithm is not specialized and can be applied to all types Fig. 21 Motion sequence of the top-mounted iiwa manipulator with maximized manipulability and external disturbances of robot manipulators. To reduce the computing times, it shall be investigated to what extent the use of a look-up table of the manipulability is reasonable, as it is partially used in [30]. This involves analyzing what resolution of this table is required and how fast data access and processing is possible since this database will probably be several gigabytes in size. Furthermore, an investigation of methods to shorten the path length while retaining and increasing the manipulability values is needed. Moreover, it should be investigated to what extent a separation of the Jacobian into the orientation and position part is reasonable. For this purpose, suitable applications should be tested in which a preference for orientation or position exists.
Author Contributions All authors contributed to the study conception and design. Data collection and analysis were performed by Sascha Kaden. The first draft of the manuscript was written by Sascha Kaden and all authors commented on previous versions of the manuscript. All authors read and approved the final manuscript.
Funding Open Access funding enabled and organized by Projekt DEAL. No funding was received for conducting this study.

Availability of Data and Material
The raw evaluation data can be viewed at: https://figshare.com/projects/Journal Paper Optimizing Mobility of Robotic Arms in Collision-free Motion Planning/ 94454.
Code Availability Custom code of the chair.

Conflict of Interests
The authors have no relevant financial or nonfinancial interests to disclose.
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons. org/licenses/by/4.0/.