Efficient Closed-Form Task Space Manipulability for a 7-DOF Serial Robot

Abstract

With the increasing demand for robots to react and adapt to unforeseen events, it is essential that a robot preserves agility at all times. While manipulability is a common measure to quantify agility at a given joint configuration, an efficient direct evaluation in task space is usually not possible with conventional methods, especially for redundant robots with an infinite number of Inverse Kinematic solutions. Yet, this is essential for global online optimization of a robot posture. In this work, we derive analytical expressions for a conventional 7-degrees of freedom (7-DOF) serial robot structure, which enable the direct evaluation of manipulability from a reduced task space parametrization. The resulting expressions allow array operation and thus achieve very high computational efficiency with vector-optimized programming languages. This direct and simultaneous calculation of the task space manipulability for large numbers of poses benefits many optimization problems in robotic applications. We show applications in global optimization of robot mounting poses, as well as redundancy resolution with global online optimization w.r.t. manipulability.

1. Introduction

It is a common requirement in robotic manipulation tasks to quantify the capabilities of a robot at a given pose. Having such a scalar measure allows comparison of different kinematic configurations in terms of the chosen metric, and can be considered at a path planning as well as at a control level. While these measures are usually defined in terms of a given joint configuration [1,2,3,4,5], the task of the robot is typically not given in this joint space. For a general robot the task space is usually defined in SE(3), i.e., the space of 3D poses consisting of translation and rotation. For many practical problems it is thus relevant to directly evaluate this measure w.r.t. a parametrization of SE(3) rather than the joints. This requires combining the evaluation of the Inverse Kinematic (IK) with the selected capability metric. But direct calculation of the IK is always robot-dependent and general analytic solutions are not possible. This is especially true for redundant robots that have more degrees of freedom (DOF) in joint space than in task space and thus admit an infinite number of IK solutions for a given end-effector pose. While analytic IK solutions are well known for conventional 6-DOF kinematics [6], for general robotic structures numeric IK solvers are applied. However, they require several iterations to find an approximated joint configuration for a given end-effector pose. This is sufficient for calculating single poses, but it is inefficient for optimization problem solvers that require evaluation of large numbers of poses. This especially prevents time-critical computation of global optima. Expressions that can be evaluated directly are thus superior for fast computation. While an analytical IK for a general robot structure does not exist, our work focuses on the most commonly used articulated 6- and 7-DOF robot serial kinematics. Yet, the 6 axis version can be viewed as a finite set of particular null space solutions of the 7-DOF.

1.1. Contribution

In this work, we develop a set of computationally efficient closed-form expressions to evaluate the task space manipulability of a 7-DOF serial robot structure, i.e., the mapping from a task space parametrization directly a manipulability measure. The main contributions of this work consist of:

• a new parametrization of the state- and null space that results in concise IK expressions with symmetric structure in the individual components
• analytical closed-form expressions from task space to manipulability measure w.r.t. joint limits, which allow array operation in vector-optimized programming languages. Note that array operation is also called Vectorization in e.g., MATLAB. It refers to the exploitation of Single Instruction Multiple Data (SIMD) instructions of modern Central Processing Unit (CPUs) and allows to operate on multiple data points simultaneously.
• sensitivity analysis of manipulability in task space
• real-time capable application for evaluating the task space manipulability of the entire null space, for globally optimal redundancy resolution w.r.t. manipulability of single poses and full trajectories on SE(3)

1.2. Related Work

For this concise review, we group previous work on the topic into the three areas: (1) performance measures in robotics, (2) direct methods for IK evaluation and algorithmic strategies on the velocity level, and (3) approaches for optimizing manipulability.

1.2.1. Performance Measures

Arguably the most common performance measure for robot structures is the manipulability measure defined by Yoshikawa [1]. It is proportional to the volume of an ellipsoid, spanned by directional capabilities of a kinematic structure to generate velocities in task space at a given joint configuration. It is purely kinematic and does not consider any dynamic components. Yoshikawa also proposed a dynamic manipulability ellipsoid [2] on the acceleration level, for cases where dynamic effects cannot be neglected. This formulation was improved by Chiacchio et al. [3] to correctly account for gravity. A new formulation of a dynamic manipulability ellipsoid that better depicts the real manipulator capabilities in terms of task space accelerations was proposed by Chiacchio [4].

Besides manipulability on the velocity and acceleration levels due to mere kinematic relations, it is essential for practical applications to also consider joint limits as constraints directly on the position level. Vahrenkamp et al. [5] extended Yoshkawa’s basic manipulability, by directly integrating joint limit penalization into the definition of the kinematic velocity Jacobian. This is achieved via a joint limit potential function.

Bong-Huan Jun et al. [7] introduce a task-oriented manipulability measure. While Yoshikawa’s original measure [1] denotes the manipulability of the whole manipulator system, [7] considers manipulability w.r.t. to sub-tasks that only affect parts of the task space, e.g., axis specific tasks. Karim Abdel-Malek and Wei Yu [8] proposed an alternative dexterity measure for robot placement that does not depend on explicit IK solutions. They analyze an augmented Jacobian matrix that does not only hold information about position and orientation, but also joint limits of the end-effector. It represents the reachable workspace with surface patches and is computationally very demanding.

Our work has the aim of developing closed-form solutions that allow efficient array operation. For this reason, the task space manipulability formulation developed in this work applies Yoshikawa’s original measure from [1]. Because its definition uses a determinant to map the joints to a scalar metric, it thus allows expansion to a continuous polynomial expression for efficient evaluation.

1.2.2. Inverse Kinematics

The IK problem of serial robot structures can be solved very elegantly on the velocity level, due to the linear relation of joint and task space velocities. However, numeric integration of the resulting joint velocities to joint angles needs stabilization against numerical drift and thus results in an iterative scheme. Originally proposed by Wolovich and Elliot [9], this group of IK solvers is nowadays typically referred to as Closed-Loop Inverse Kinematic (CLIK) solvers. Colomé and Torras [10] give an overview of the most common CLIK solvers, with an additional experimental comparison in terms of convergence, numerical error, singularity handling, joint limit avoidance, and the capability of reaching secondary goals. Antonelli [11] conducted a stability analysis of priority-based kinematic CLIK algorithms for redundant kinematics. He provides sufficient conditions for the control gains. While different stabilization schemes for CLIK solvers are proposed, the choice of gain parameters used in the control structure is rarely addressed. In practice these parameters are often empirically tuned. Bjoerlykhaug [12] proposes the use of a genetic algorithm for optimizing the feedback gain used in CLIK solvers, in order to minimize iteration cycles and maximize accuracy. In an experimental evaluation, he achieved a 50% decrease in computation time through his feedback gain tuning. Reiter et al. [13] propose a strategy for finding higher-order time-optimal IK solutions for redundant robots. They lay out solutions for fourth-order time derivatives of joint trajectories, applying a multiple shooting optimization method. This higher-order continuous differentiability is especially important for application on elastic mechanisms.

Siciliano [14] gives a tutorial on early common online IK algorithms. He states the important features of a direct inverse kinematics function, i.e., repeatability, cyclicity, or cyclic behaviour, and online applicability. Shimizu et al. [15] outline an analytical IK computation for a 7-DOF serial robot. The approach directly parametrizes the end-effector pose with Cartesian coordinates for translation and a rotation matrix for orientation. However, the use of the 2-quadrant atan function, as opposed to the 4-quadrant atan2 function, results in two problems. For one, the entire task space is not covered, and two, it results in discontinuous joint functions w.r.t. the null space parameter and thus leads to discontinuous IK solutions and corresponding null space limitations. A similar strategy, but extended to the entire domain, is proposed by Faria et al. [16]. They propose a position-based IK solution for a 7-DOF serial manipulator with joint limit and singularity avoidance.

Besides approaches that use kinematic insight of a structure, several machine learning algorithms are also considered in the literature. A detailed review is beyond the scope of this work, but we want to give a concise overview of research activities. D’Souza et al. [17] apply a locally weighted projection regression to learn the IK of a 30-DOF humanoid robot. This maps the non-convex problem onto a locally convex problem that is suitable for direct learning. Tejomurtula and Kak [18], as well as Köker et al. [19], applied artificial neural networks for finding an IK mapping for 3-DOF robots and showed the feasibility of the problem using conventional error-backpropagation and Kohonen networks. Sariyildiz et al. [20] compare support vector regression and artificial neural networks for learning IK mappings of a 7-DOF serial robot. They find that support vector regression is less prone to local minima and requires very few training data. Genetic algorithms were already early applied by Parker et al. [21]. They pointed out low positioning accuracy, but emphasize its simplicity in application. Köker [22] proposes a hybrid approach combining Elman neural networks with genetic algorithms. He was able to significantly improve accuracy for IK solutions of a 6-DOF mechanism in comparison to pure neural networks. Very recently, Dereli et al. [23] proposed a strategy to apply quantum behaved particle swarm optimization for finding IK solutions of a 7-DOF serial robot.

The IK expressions developed in this work are similar to the analytical approaches in [15,16] in terms of parametrizing the null space as arm angle. However, the new task space parametrization that we introduce results in more concise and, more importantly, fully vectorizable expressions that allow efficient array operations. In contrast to existing approaches in the literature, this computational advantage makes our approach suitable for simultaneous evaluation of a large number of poses.

1.2.3. Optimizing Manipulability

In conventional industrial contexts, optimizing cycling time is always of interest. Several publications deal with this problem, e.g., Kamrani et al. [24] use the Response Surface Method [25] to optimize robot placement w.r.t. cycling time. Chan and Dubey [26], as well as Dariush et al. [27], use a projection method of the joint limit gradient potential function. This is used for local manipulability optimization on the velocity level. Dufour and Suleiman [28] present an approach of integrating the manipulability index into an optimization-based IK solver, by using linear approximations of the nonlinear manipulability measure with numeric gradient calculations at every time step. Jin et al. [29] mention the difficulty of real-time manipulability optimization that is related to a high computational burden since the manipulability is a non-convex function to the joint angles of a robotic arm. Due to the capability of high-speed parallel distributed processing, they propose an approach using dynamic neural networks in order to implement manipulability optimization in real-time. Conducting computer simulations, they show that the proposed method raises the manipulability by almost 40% on average compared to existing methods.

Besides local optimization of a given joint configuration, for many robotic tasks it is required to include manipulability as criteria for optimization of the whole trajectory. Lee [30] shows that a required motion can be approximated by a series of manipulability ellipsoids. Guilamo et al. [31] present an algorithm for trajectory generation that maximizes the volume of the manipulability ellipsoid. Yoshikawa [1] already observed that the optimal postures of various manipulators form the viewpoint of manipulability, and often show resemblance of those naturally taken by human arms. This motivates the idea of manipulability transfer using a learning by demonstration strategy that is introduced by Rozo et al. [32]. Their approach allows robots to learn and reproduce a continuous set of manipulability ellipsoids by an expert’s demonstration. In order to encode and retrieve those ellipsoids, they apply Gaussian Mixture Models and Gaussian Mixture Regression. In Jaquier et al. [33] the same authors exploit tensor-based representation, to consider that manipulability ellipsoids lie on the manifold of symmetric positive definite matrices. Faroni et al. [34] present an approach that maximizes the average manipulability of the overall task. Their method is based on the optimization of a cost function that depends on various points along a predetermined path. In particular, if the task of the manipulator is known a priori, this approach provides global manipulability optimization.

An approach for directly quantifying manipulability of a redundant robot in task space is proposed by Zacharias et al. [35]. They introduce a capability map, to guide the decision on how to place a mobile robot relative to an object. It is a sampling-based approach, based on the manipulability index. While the approach reveals in which regions the robot is capable of grasping objects from different angles, the information of optimal approaching directions is lost.

The task space manipulability approach in this work enables for the first time global manipulability optimization with real-time capabilities, due to its efficient formulation.

1.3. Outline

The remainder of the paper is organized as follows. The problem of a closed-loop task space manipulation framework is outlined in Section 2. In Section 3, the derivation of all analytical mappings is explained. Evaluation and analysis of the resulting task space manipulability is discussed in Section 4 and applied in global optimization formulations in Section 5. We conclude the work and outline future directions of development in Section 6.

2. Problem Formulation

Given a n-DOF serial robot, its forward kinematics

$$\begin{array}{c}\hfill \mathrm{FK}:{\mathbb{R}}^n\to \mathrm{SE}\left(3\right)\times {\mathbb{R}}^{n-6},\mathit{q}\mapsto (\mathit{z},\mathit{\lambda })\end{array}$$

(1)

maps the joints $\mathit{q}$ onto the 3D end-effector pose $\mathit{z}$ at a particular null space solution parametrized by $\mathit{\lambda }$. To quantify the capability of moving in the SE(3) task space at a given joint configuration $\mathit{q}$, a manipulability metric function

$$\mathrm{M}:{\mathbb{R}}^n\to {\mathbb{R}}^1,\mathit{q}\mapsto \mu$$

(2)

is applied. A proper choice of parametrization for $\mathit{z}$ and $\mathit{\lambda }$ assures the existence of the inverse function

$$\begin{array}{c}\hfill \mathrm{IK}:\mathrm{SE}\left(3\right)\times {\mathbb{R}}^{n-6}\to {\mathbb{R}}^n,(\mathit{z},\lambda )\mapsto {\mathrm{FK}}^{-1}(\mathit{z},\lambda )=:\mathit{q}.\end{array}$$

(3)

We define the task space manipulability as the direct mapping

$$\mathrm{M}\circ \mathrm{IK}:\mathrm{SE}\left(3\right)\times {\mathbb{R}}^{n-6}\to {\mathbb{R}}^1,(\mathit{z},\mathit{\lambda })\mapsto \mu$$

(4)

of a desired pose $\mathit{z}$ in task space onto the manipulability measure $\mu$, considering all null space solutions parametrized by $\mathit{\lambda }$. $(\mathrm{M}\circ \mathrm{IK})(\mathit{z},\mathit{\lambda })$ denotes the function composition $\mathrm{M}\left(\mathrm{IK}\right(\mathit{z},\mathit{\lambda }\left)\right)$. Figure 1 illustrates the task space manipulability for a certain end-effector pose $\mathit{z}$. Considering real-time critical online applications and feasibility of global optimization formulations, the development of the task space manipulability map can be broken down into three problems:

Problem 1:

Find a parametrization of the task- and null space that exploits the kinematic structure for concise expressions.

Problem 2:

Find closed-form expressions for all mappings from task space to manipulability that allow efficient array operation in vector-optimized programming languages.

Problem 3:

Let $\mathcal{Q}\subset {\mathbb{R}}^7$ be the space of admissible joint configurations. Find an analytical expression of the range of the null space solutions $\mathsf{\Lambda }\left(\mathit{z}\right):=\{\mathit{\lambda }\in {\mathbb{R}}^{n-6}|\mathrm{IK}(\mathit{z},\mathit{\lambda })\in \left(Q\right)\}$, for which the inverse kinematics function $\mathrm{IK}(·,\mathit{\lambda })$ results in an admissible joint configuration $\mathit{q}\in \mathcal{Q}$.

In this work, we investigate in detail the case of a 7-DOF serial robot kinematics in conventional Spherical-Revolute-Spherical (S-R-S) structure, such as the KUKA LBR series. In this context, S-R-S refers to a kinematic 7-DOF structure with alternating revolute joints, of which the rotation axes of the first and last 3 joints intersect. These two groups of intersecting axes behave kinematically like a spherical joint and are often referred to as shoulder and wrist. This type of kinematic structure leads to a 1-dimensional null space of solutions and thus $\lambda \in {\mathbb{R}}^1$.

3. Technical Approach

This section outlines the derivation of the closed-form task space manipulability for the considered special case of a 7-DOF serial robot kinematics. We first discuss the chosen manipulability mapping and possible reductions in joint space. Motivated by these reductions, we propose a task space projection onto a parameter space, which yields concise expressions for the IK. Figure 2 summarizes all developed mappings that are developed in this section. The section concludes with an analytic definition of the admissible null space at a given parameter end-effector pose.

Notational Notes

Scalars are written in plain lower case, vectors in bold-face lower case. Matrices are bold-face upper case, while plain upper case symbols refer to coordinate frames, mathematical spaces, and sets.

For vector indices, we use the common anthropomorphic analogy of a human arm. We refer to the origin of the kinematic as base B, and to positions of joint 2, joint 4, and joint 6 as shoulder S, elbow E, and wrist W respectively. Body-fixed frames of the individual robot links are numbered 1 to 7 and relate to the bodies after the corresponding joints. The end-effector will be referred to as tool T.

Coordinate transformation matrices are written as ${\mathit{A}}_{kj}$ with 2 indices and are read from right to left, e.g., ${\mathit{A}}_{43}$ transforms the coordinate system from body-fixed frame of joint $q_3$ to joint $q_4$, whereas vector indices are read from left to right and their reference frame is written as left-hand side subscript. The notation ${}_B{\mathit{r}}_{SW}$ thus describes a vector $\mathit{r}$ pointing from shoulder S to wrist W, expressed in base frame B. Cartesian base vectors of the coordinate systems are written as ${}_{}\widehat{\mathit{x}}{,}_{}\widehat{\mathit{y}}$, and ${}_{}\widehat{\mathit{z}}$. If a vector does not have a lower left index, it always refers to the base B.

3.1. Manipulability Measure

The central equation in robot kinematics is the linear forward velocity kinematic map

$$\begin{array}{c}\hfill \dot{\mathit{z}}(\mathit{q},\dot{\mathit{q}}):{=}_{}\mathit{J}\left(\mathit{q}\right)\dot{\mathit{q}}\end{array}$$

(5)

that relates general joint space velocities $\dot{\mathit{q}}\in {\mathbb{R}}^n$ to task space velocities $\dot{\mathit{z}}\in {\mathbb{R}}^6$, where the linear map ${}_{}\mathit{J}\left(\mathit{q}\right)\in {\mathbb{R}}^{6\times n}$ describes the velocity propagation from joint to task space at a given joint configuration $\mathit{q}\in {\mathbb{R}}^n$. It is defined by the kinematic chain and represents the derivative

$$\begin{array}{c}\hfill {}_{}\mathit{J}\left(\mathit{q}\right)=\frac{\partial \dot{\mathit{z}}(\mathit{q},\dot{\mathit{q}})}{\partial \dot{\mathit{q}}},\end{array}$$

(6)

hence it is often referred to as Robot Jacobian.

Yoshikawa’s manipulability measure [1], which we use in this work, is defined as

$$\mathrm{M}:{\mathbb{R}}^n\to \mathbb{R},\mathit{q}\mapsto \sqrt{det\left(\mathit{J}\left(\mathit{q}\right)\mathit{J}{\left(\mathit{q}\right)}^{\top }\phantom{\int }\right)}=:\mu$$

(7)

and is a measure, proportional to the volume of the velocity manipulability ellipsoid

$$\begin{array}{cc}\hfill {\dot{\mathit{q}}}^{\top }\dot{\mathit{q}}& =1\hfill \end{array}$$

(8a)

$$\begin{array}{cc}\hfill {\dot{\mathit{z}}}^{\top }{{(}_{}{\mathit{J}}_{}{\mathit{J}}^{\top })}^{-1}\dot{\mathit{z}}& =1.\hfill \end{array}$$

(8b)

Note that (7) does not consider hardware-related joint limits. However, joint configurations that violate these constraints must not be considered.

Zlatanov et al. [36] explain that the forward velocity kinematic map (5) is not sufficient for exhaustive characterization of the singularities of a manipulator. Further, Staffetti et al. [37] show that many of these often-used manipulability indices are not invariant to change of reference frames, scale, or physical units. However, the big advantage of Yoshikawa’s original manipulability metric is the fact that it can be expanded to a polynomial expression and thus qualifies for computationally efficient array operation. Further, derivatives can be calculated analytically. As outlined by Staffetti et al. [37], it is not a true metric for distance to a singularity but nonetheless serves as a relative comparison of manipulability qualities between joint configurations [38].

For a n-DOF serial robot kinematics, we refer to the $i=[1,n]$ absolute angular and translational velocities of the individual links, i.e., the velocity between the robot base B and the body-fixed frame of link i, as ${}_{}{\mathit{\omega }}_{Bi}$ and ${}_{}{\mathit{v}}_{Bi}$. Expressed w.r.t. the link frame i, the velocities of the kinematic chain are calculated with

$$\begin{array}{cc}\hfill {}_i{\mathit{\omega }}_{Bi}& ={\mathit{A}}_{}{ip}_p{\mathit{\omega }}_{Bp}{+}_i{\mathit{\omega }}_{pi}\hfill \end{array}$$

(9a)

$$\begin{array}{cc}\hfill {}_i{\mathit{v}}_{Bi}& ={\mathit{A}}_{}ip\left({}_p{\mathit{v}}_{Bp}{+}_p{\mathit{\omega }}_{Bp}{\times }_p{\mathit{r}}_{pi}\right),\hfill \end{array}$$

(9b)

where $p=i-1$ is the predecessor link of i and $(\times )$ denotes the cross product ${\mathbb{R}}^3\times {\mathbb{R}}^3\to {\mathbb{R}}^3$. In the following, manipulability refers to Yoshikawa’s manipulability measure [1].

3.1.1. Reduction of First Joint

While Yoshikawa’s manipulability measure is not invariant w.r.t. scale or physical units, it is in fact invariant to change of reference frames.

Proof. 

Given a vector of joint velocities $\dot{\mathit{q}}$ and task space velocities $\dot{\mathit{z}}$ w.r.t. to a reference frame A, the Jacobian matrix

$$\begin{array}{c}\hfill {}_A\mathit{J}\left(\mathit{q}\right)=\frac{{\partial }_A\dot{\mathit{z}}(\mathit{q},\dot{\mathit{q}})}{\partial \dot{\mathit{q}}}\end{array}$$

(10)

is used to define the manipulability index

$$A\mu \left(\mathit{q}\right)=\sqrt{det\left({}_A\mathit{J}{\left(\mathit{q}\right)}_A\mathit{J}{\left(\mathit{q}\right)}^{\top }\phantom{\int }\right)}.$$

(11)

If this manipulability index is expressed in terms of a new reference frame B via the block transformation matrix

$$\begin{array}{c}\hfill {\mathit{A}}_{BA}^{\mathrm{blk}}\left(\mathit{q}\right)=\left[\begin{array}{cc}{\mathit{A}}_{BA}\left(\mathit{q}\right)& \mathit{0}\\ \mathit{0}& {\mathit{A}}_{BA}\left(\mathit{q}\right)\end{array}\right],\end{array}$$

(12)

consisting of rotation matrices ${\mathit{A}}_{BA}$, the manipulability index reads

$$\begin{array}{c}\hfill {}_B\mathit{\mu }\left(\mathit{q}\right)=\sqrt{det\left({\mathit{A}}_{BA}^{\mathrm{blk}}{\left(\mathit{q}\right)}_A\mathit{J}\left(\mathit{q}\right){\left({\mathit{A}}_{BA}^{\mathrm{blk}}{\left(\mathit{q}\right)}_A\mathit{J}\left(\mathit{q}\right)\right)}^{\top }\phantom{\int }\right)}.\end{array}$$

(13)

Considering the fact that Euclidean transformation matrices have $det\left({\mathit{A}}_{}\phantom{\int }\right)=1$, we find

$$\begin{array}{c}\hfill {}_B\mathit{\mu }\left(\mathit{q}\right)=\sqrt{det\left({}_A\mathit{J}{\left(\mathit{q}\right)}_A\mathit{J}{\left(\mathit{q}\right)}^{\top }\phantom{\int }\right)}{=}_A\mathit{\mu }\left(\mathit{q}\right)\end{array}$$

(14)

i.e., the manipulability measure $\mu$ is invariant to change of reference frames. □

If the reference frame is chosen to be fixed to any link after the first joint, it results in an expression for the manipulability measure that is independent of the first joint. This results from the fact that the first joint rotates the whole kinematic structure including the reference frame, but does not alter any geometric relations.

We consequently choose to formulate the Jacobian matrix w.r.t. to the end-effector frame, as this does not only lead to the independence of $q_1$, but also results in the most concise expression.

3.1.2. Reduction of Last Joint

For a special case of a 7-DOF serial kinematic, the parameter space of the manipulability can be further reduced. This special case consists of kinematic structures, whose origin of the end-effector frame lies on the rotation axis of the last joint $q_n$. The purely angular contribution of $q_n$ does not alter the kinematic configuration but only rotates the reference frame and with it the manipulability ellipsoid. The shape of the ellipsoid is not affected and so $q_n$ can also not influence the manipulability measure.

3.1.3. Closed-Form Expression

Exploiting these two reductions by formulating the ${}_T\mathit{J}$ w.r.t. to the end-effector frame T and assuming the tool center point (TCP) along the last joint axis, it is possible to expand the entire determinant expression of the matrix ${}_T{\mathit{J}}_T{\mathit{J}}^{\top }\in {\mathbb{R}}^{6\times 6}$ from (7) to a symbolic polynomial expression using, e.g., MATLAB Symbolic Math Toolbox™. The advantage being that, unlike the original matrix expression, the polynomial form allows array operation in vector-optimized programming languages. This enables simultaneous evaluation of an entire set of joint configurations. The full manipulability function is listed in Appendix A.

3.2. Task Space Parametrization

The decision of choosing a parametrization for the SE(3) pose, as well as the 1D null space, is essential for the derivation of concise analytical formulations. We propose the following parameter requirements (PR) for a suitable parametrization in regard to the IK functions. The parameter set must

PR1:

uniquely define the null space parameter for the entire space of SE(3).

PR2:

result in a minimal number of parameters for the components of the IK vector map $\mathit{p}\mapsto \mathit{q}$.

PR3:

allow direct application of the above-mentioned reductions.

Different approaches for null space parametrization were proposed in the literature. The redundancy is either directly parametrized by a redundant joint [39,40], or more commonly by a joint-independent arm angle [15,41]. Shimizu et al. [15] argued that joint-based parametrization is not suitable for the discussed 7-DOF S-R-S mechanism due to possible ambiguous results. Kreutz-Delgado et al. [41] define the arm angle as the angle between an arm and a reference plane. The arm plane is spanned by shoulder, elbow, and wrist locations. The reference plane is defined by a fixed vector and the vector from shoulder to wrist. Shimizu et al. [15] point out arithmetic singularities in the original definition whenever the two vectors are collinear. They enhance the robustness of the definition by defining the reference plane in terms of a particular solution $q_3\stackrel{!}{=}0$, which resembles the solution of conventional non-redundant 6-DOF mechanisms. While this definition is unique w.r.t. the conservative joint limits of their analyzed robot structure, it is ambiguous whenever the reduced non-redundant 6-DOF mechanism admits multiple configurations that result in the same end-effector pose.

In this work, we introduce a parametrization that fulfills all the above-discussed parameter requirements. Figure 3 illustrates the following discussion. Independent of a desired end-effector pose, positions of the base B and shoulder S are always stationary, where

$$\begin{array}{c}\hfill {}_B{\mathit{r}}_{BS}:={(l_B+l_1)}_{}\widehat{\mathit{z}}\end{array}$$

(15)

with link lengths of the base link $l_B$ and the first link $l_1$. Additionally, defining a desired end-effector pose relative to the robot base in SE(3), consisting of ${}_B{\mathit{r}}_{BT}$ for translation and ${\mathit{A}}_{TB}$ for orientation, determines not only the location of the tool-center-point T but also the wrist position

$$\begin{array}{c}\hfill {}_B{\mathit{r}}_{BW}:{=}_B{\mathit{r}}_{BT}-{\mathit{A}}_{B6}{(l_6+l_7+l_T)}_{}\widehat{\mathit{z}},\end{array}$$

(16)

with link lengths $l_6$ and $l_7$, and a potential tool length $l_T$. This wrist position is used for define the translational component of the end-effector pose $\mathit{z}$. The position ${}_B{\mathit{r}}_{SW}$ is parametrized by spherical coordinates $(r_{\mathrm{ref}},{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}})$ with coordinate plane ${}_B{\widehat{\mathit{x}}}_{}\widehat{\mathit{z}}$, origin S and ${}_B\widehat{\mathit{z}}$ as polar axis. The parameters are radius $r_{\mathrm{ref}}$, longitudinal angle ${\gamma }_{\mathrm{ref}}$, and azimuthal angle ${\beta }_{\mathrm{ref}}$. Note that ${\gamma }_{\mathrm{ref}}$ and ${\beta }_{\mathrm{ref}}$ directly align with the rotation axis of $q_1$ and $q_2$. These two angles also define the reference frame R with

$$\begin{array}{c}\hfill {\mathit{A}}_{RB}({\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}}):={\mathit{A}}_y\left({\beta }_{\mathrm{ref}}\right){\mathit{A}}_z\left({\gamma }_{\mathrm{ref}}\right).\end{array}$$

(17)

The orientation is parametrized along a consecutive Euler angle sequence $Z\to Y^{{}^{\prime }}\to Z^{{}^{″}}$, which again corresponds to the sequence of the joint structure. However, instead of directly parametrizing ${\mathit{A}}_{TB}$, we parametrize the end-effector orientation with respect to the reference frame, i.e.,

$$\begin{array}{c}\hfill {\mathit{A}}_{TR}({\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}}):={\mathit{A}}_z\left({\psi }_{\mathrm{EE}}\right){\mathit{A}}_y\left({\beta }_{\mathrm{EE}}\right){\mathit{A}}_z\left({\gamma }_{\mathrm{EE}}\right).\end{array}$$

(18)

Regarding the stated parameter requirement PR2, this makes the IK functions of the wrist angles $(q_5,q_6,q_7)$ as independent of the shoulder parameters $(r_{\mathrm{ref}},{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}})$ as possible, as will be seen in the IK Section 3.3.

The 1D null space is parametrized by the arm angle $\lambda$. In contrst to Shimizu et al. [15] we do not define the arm angle w.r.t. to the non-redundant solution $q_3\stackrel{!}{=}0$, but w.r.t. to the introduced reference frame R. Let $\lambda$ be the arm angle, which defines a new frame L with

$$\begin{array}{c}\hfill {\mathit{A}}_{LB}({\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}},\lambda ):={\mathit{A}}_z\left(\lambda \right){\mathit{A}}_{RB}({\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}}),\end{array}$$

(19)

such that the negative frame base vector $\left({-}_L\widehat{\mathit{x}}\right)$ points in direction of the elbow E. This uniquely defines the null space parameter as required in PR1. The full set of parameters is thus given with tuple $(\mathit{p},\lambda )\in {\mathbb{R}}^6\times \mathbb{R}$, consisting of the parameter vector

$$\begin{array}{c}\hfill \mathit{p}:={\left[r_{\mathrm{ref}},{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}},{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}}\right]}^{\top }\end{array}$$

(20)

and arm angle $\lambda$. The individual parameter range definitions are

$$\begin{array}{c}\hfill \begin{array}{cccc}\hfill r_{\mathrm{ref}}\in [& \hfill r_{\mathrm{ref}}^{\min },& r_{\mathrm{ref}}^{\max }\hfill & ]\\ \hfill {\gamma }_{\mathrm{ref}}\in [& \hfill -\pi ,& +\pi \hfill & ]\\ \hfill {\beta }_{\mathrm{ref}}\in [& \hfill 0,& +\pi \hfill & ]\\ \hfill {\gamma }_{\mathrm{EE}}\in [& \hfill -\pi ,& +\pi \hfill & ]\\ \hfill {\beta }_{\mathrm{EE}}\in [& \hfill 0,& +\pi \hfill & ]\\ \hfill {\psi }_{\mathrm{EE}}\in [& \hfill -\pi ,& +\pi \hfill & ]\\ \hfill \lambda \in [& \hfill -\pi ,& +\pi \hfill & ]\end{array}\end{array}$$

(21)

and form the parameter space $\mathcal{P}\subset {\mathbb{R}}^7$. Note that the two parameters ${\gamma }_{\mathrm{ref}}$ and ${\psi }_{\mathrm{EE}}$ solely affect joints $q_1$ and $q_7$, which do not influence manipulability. The task space manipulability developed in this work can thus without loss of information be represented by the reduced parameter vector ${\mathit{p}}^{\mathrm{red}}\in {\mathcal{P}}^{\mathrm{red}}\subset {\mathbb{R}}^4$ consisting of

$$\begin{array}{c}\hfill {\mathit{p}}^{\mathrm{red}}:={\left[r_{\mathrm{ref}},{\beta }_{\mathrm{ref}},{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}}\right]}^{\top }.\end{array}$$

(22)

This complies with the stated requirement PR3. The presented parametrization is the fundamental core for the concise mappings developed in the remaining section.

3.2.1. Task Space Projection

We refer to the extraction of the parameter vector $\mathit{p}={\left[r_{\mathrm{ref}},{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}},{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}}\right]}^{\top }$ from a given end-effector pose $\mathit{z}\in \mathrm{SE}\left(3\right)$ as Task Space Projection. Without loss of generality, we assume the pose $\mathit{z}\in \mathrm{SE}\left(3\right)$ is described with Cartesian Coordinates $(x,y,z)$ for translation ${}_B{\mathit{r}}_{BT}$ together with a Rotation matrix ${\mathit{A}}_{TB}$ for orientation. As a reference matrix for extracting the parameter space angles $({\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}})$ we state the rotation matrix for a general $ZYZ$ Euler sequence

$$\begin{array}{c}{\mathit{A}}_{zyz}(\gamma ,\beta ,\psi ):={\mathit{A}}_z\left(\psi \right){\mathit{A}}_y\left(\beta \right){\mathit{A}}_z\left(\gamma \right)=\hfill \\ \hfill \left(\begin{array}{ccc}\mathrm{c}\left(\beta \right)\mathrm{c}\left(\gamma \right)\mathrm{c}\left(\psi \right)-\mathrm{s}\left(\gamma \right)\mathrm{s}\left(\psi \right)& \mathrm{c}\left(\gamma \right)\mathrm{s}\left(\psi \right)+\mathrm{c}\left(\beta \right)\mathrm{c}\left(\psi \right)\mathrm{s}\left(\gamma \right)& -\mathrm{c}\left(\psi \right)\mathrm{s}\left(\beta \right)\\ -\mathrm{c}\left(\psi \right)\mathrm{s}\left(\gamma \right)-\mathrm{c}\left(\beta \right)\mathrm{c}\left(\gamma \right)\mathrm{s}\left(\psi \right)& \mathrm{c}\left(\gamma \right)\mathrm{c}\left(\psi \right)-\mathrm{c}\left(\beta \right)\mathrm{s}\left(\gamma \right)\mathrm{s}\left(\psi \right)& \mathrm{s}\left(\beta \right)\mathrm{s}\left(\psi \right)\\ \mathrm{c}\left(\gamma \right)\mathrm{s}\left(\beta \right)& \mathrm{s}\left(\beta \right)\mathrm{s}\left(\gamma \right)& \mathrm{c}\left(\beta \right)\end{array}\right)\end{array}$$

(23)

that shows that we can define a mapping ${\mathrm{eul}}_{ZYZ}:\mathrm{SE}\left(3\right)\to {\mathbb{R}}^3$ as

$$\begin{array}{c}\hfill {\mathrm{eul}}_{ZYZ}:\mathrm{SE}\left(3\right)\to {\mathbb{R}}^3,{\mathit{A}}_{zyz}\mapsto \left[\begin{array}{c}\mathrm{atan}2\left({\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(3,2)},{\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(3,1)}\right)\\ arccos\left({\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(3,3)}\right)\\ \mathrm{atan}2\left({\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(2,3)},-{\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(1,3)}\right)\end{array}\right]=:\left[\begin{array}{c}\gamma \\ \beta \\ \psi \end{array}\right]\end{array}$$

(24)

that extracts the Euler angles from a rotation matrix in SE(3). The operator ${\left[\phantom{\int }·\right]}_{(i,j)}$ returns the element at row i and column j of a matrix.

The Task Space Projection

$$\begin{array}{c}\hfill \mathrm{TSP}:\mathrm{SE}\left(3\right)\to {\mathbb{R}}^6,\mathit{z}\mapsto \mathit{p}\end{array}$$

(25a)

consists of the mappings

$$\begin{array}{cc}\hfill r_{\mathrm{ref}}\left(\mathit{z}\right)& :={∥{}_B\mathit{r}{}_{SW}∥}_2\hfill \end{array}$$

(25b)

$$\begin{array}{cc}\hfill {\beta }_{\mathrm{ref}}\left(\mathit{z}\right)& :=\frac{\pi }{2}-arctan\frac{{\left[{}_B\mathit{r}{}_{SW}\right]}_{\left(3\right)}}{{\left[{}_B\mathit{r}{}_{SW}\right]}_{\left(1\right)}}\hfill \end{array}$$

(25c)

$$\begin{array}{cc}\hfill {\gamma }_{\mathrm{ref}}\left(\mathit{z}\right)& :=\mathrm{atan}2\left({\left[{}_B\mathit{r}{}_{SW}\right]}_{\left(2\right)},{\left[{}_B\mathit{r}{}_{SW}\right]}_{\left(1\right)}\right)\hfill \end{array}$$

(25d)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\gamma }_{\mathrm{EE}}\\ {\beta }_{\mathrm{EE}}\\ {\psi }_{\mathrm{EE}}\end{array}\right]\left(\mathit{z}\right)& :={\mathrm{eul}}_{ZYZ}\left({\mathit{A}}_{7R}(\mathit{z},{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}})\right).\hfill \end{array}$$

(25e)

With the shoulder-wrist vector

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {}_B\mathit{r}{}_{SW}& :={}_B\mathit{r}{}_{BR}-{}_B\mathit{r}{}_{BS}\hfill \\ \hfill & \phantom{:}={}_B\mathit{r}{}_{BT}-{\mathit{A}}_{B66}\widehat{\mathit{z}}(l_6+l_7+l_T){-}_B\widehat{\mathit{z}}(l_B+l_1).\hfill \end{array}\end{array}$$

(26)

and the rotation matrix

$$\begin{array}{c}\hfill {\mathit{A}}_{7R}(\mathit{z},{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}}):={\mathit{A}}_{7T}{\mathit{A}}_{TB}\left(\mathit{z}\right){\mathit{A}}_{BR}({\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}}),\end{array}$$

(27)

derived from the desired task space pose. Rotation ${\mathit{A}}_{7T}$ is the constant rotation matrix from body fixed frame of link 7 to the TCP frame.

3.2.2. Task Space Surjection

We refer to the inverse mapping, i.e., from the parameter vector $\mathit{p}$ to the task space pose $\mathit{z}$, as Task Space Surjection (TSS)

$$\begin{array}{c}\hfill \mathrm{TSS}:{\mathbb{R}}^6\to \mathrm{SE}\left(3\right),\mathit{p}\mapsto \mathit{z}.\end{array}$$

(28a)

The relations are given with

$$\begin{array}{cc}\hfill {}_B{\mathit{r}}_{BT}& :={(l_B+l_1)}_B\widehat{\mathit{z}}+{\mathit{A}}_{BR}({\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}})\left({}_R\widehat{\mathit{z}}{r}_{\mathrm{ref}}+{\mathit{A}}_{R7}({\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}})(l_6+l_7+{\mathit{A}}_{7T}{l}_T)\right)\hfill \end{array}$$

(28b)

$$\begin{array}{cc}\hfill {\mathit{A}}_{TB}& :={\mathit{A}}_{7T}{\mathit{A}}_{7R}({\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}}){\mathit{A}}_{RB}({\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}})\hfill \end{array}$$

(28c)

using the established definitions in the previous sections.

3.3. Inverse Kinematics

In this section we derive closed-form expressions for the IK map. After discussing the choice of the default manipulator configuration, we derive the individual IK mappings of the robot joints. Corresponding to the S-R-S structure, we group the joints into shoulder angles $\{q_1,q_2,q_3\}$, the elbow angle $\left\{q_4\right\}$, and wrist angles $\{q_5,q_6,q_7\}$.

3.3.1. Manipulator Configuration

Due to the possible reconfiguration of the robot kinematics, i.e., whenever 3 revolute joint axes intersect in one point, with 2 being coaxial and the third being perpendicular to the links, there exists an alternative configuration

$$\begin{array}{c}\hfill \mathrm{FK}({\mathrm{coaxial}}_1,\mathrm{perpendicular},{\mathrm{coaxial}}_2)=\mathrm{FK}({\mathrm{coaxial}}_1+\pi ,-{\mathrm{perpendicular}}_2,{\mathrm{coaxial}}_2+\pi )\end{array}$$

(29)

that results in the same FK. In the 7-DOF S-R-S structure considered in this work, this is the case for the tuples $(q_1,q_2,q_3)$, $(q_3,q_4,q_5)$, and $(q_5,q_6,q_7)$. Therefore, defining only the end-effector pose as well as the elbow position results in 8 possible configurations. Of course, it is important to derive an IK map that results in one specific configuration for the entire parameter space. The following derivation is designed to yield in a configuration as depicted in Figure 3 for the default case $q_1=q_3=q_5=0$. This is achieved by choosing the joint angle ranges

$$\begin{array}{c}\hfill \begin{array}{cccc}{q}_1\in [& \hfill -\pi ,& +\pi \hfill & ]\\ q_2\in [& \hfill 0,& +\pi \hfill & ]\\ q_3\in [& \hfill -\pi ,& +\pi \hfill & ]\\ q_4\in [& \hfill 0,& +\pi \hfill & ]\\ q_5\in [& \hfill -\pi ,& +\pi \hfill & ]\\ q_6\in [& \hfill 0,& +\pi \hfill & ]\\ q_7\in [& \hfill -\pi ,& +\pi \hfill & ].\end{array}\end{array}$$

(30)

We refer to this definition as ${\mathcal{Q}}_{\mathrm{sc}}\subset {\mathbb{R}}^7$, i.e., the space of joints in standard configuration.

3.3.2. Elbow Angles

The central geometric shape to express the arm portion of joints is the triangle $\overline{SEW}$ as depicted in Figure 3. It is fully defined by the parameter $r_{\mathrm{ref}}$, as well as the robot related constant link lengths

$$\begin{array}{cc}\hfill r_{SE}& :=l_3+l_4\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill r_{EW}& :=l_5+l_6.\hfill \end{array}$$

(32)

The law of cosines in this triangle allows direct calculation of joint 4

$$\begin{array}{cc}\hfill r_{\mathrm{ref}}^2& =r_{SE}^2+r_{EW}^2-r_{SE}{r}_{EW}cos\left(\pi -q_4\right)\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill q_4\left(r_{\mathrm{ref}}\right)& :=\pi -arccos\left(\frac{r_{SE}^2+r_{EW}^2-r_{\mathrm{ref}}^2}{2r_{SE}{r}_{EW}}\right)\hfill \end{array}$$

(34)

as well as the adjoint angles

$$\begin{array}{cc}\hfill r_{SE}& =r_{\mathrm{ref}}^2+r_{EW}^2-r_{\mathrm{ref}}{r}_{EW}cos\left({\theta }_S\right)\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\theta }_S\left(r_{\mathrm{ref}}\right)& :=arccos\left(\frac{r_{\mathrm{ref}}^2+r_{EW}^2-r_{SE}}{2r_{\mathrm{ref}}{r}_{EW}}\right)\hfill \end{array}$$

(36)

and

$$\begin{array}{cc}\hfill r_{EW}& =r_{\mathrm{ref}}^2+r_{SE}^2-r_{\mathrm{ref}}{r}_{SE}cos\left({\theta }_W\right)\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\theta }_W\left(r_{\mathrm{ref}}\right)& :=arccos\left(\frac{r_{\mathrm{ref}}^2+r_{SE}^2-r_{EW}}{2r_{\mathrm{ref}}{r}_{SE}}\right).\hfill \end{array}$$

(38)

The latter are used to define alternative rotation frame compositions for the derivation of the remaining joints. See Figure 4 for an overview of the relations between all introduced coordinate frames.

3.3.3. Shoulder Angles

Reusing the $ZYZ$ Euler sequence extraction function (24) makes it possible to directly define the IK function of the shoulder angles $\{q1,q2,q3\}$. The parameter-related frames R and L (cf. Figure 4) are used to compose the transformation matrix

$$\begin{array}{c}\hfill {\mathit{A}}_{3B}(\mathit{p},\lambda ):={\mathit{A}}_y\left(-{\theta }_W\right){\mathit{A}}_z\left(\lambda \right){\mathit{A}}_{RB}\left({\beta }_{\mathrm{ref}},{\gamma }_{\mathrm{ref}}\right)\end{array}$$

(39)

and extract

$$\begin{array}{c}\hfill \left[\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right](\mathit{p},\lambda ):={\mathrm{eul}}_{ZYZ}\left({\mathit{A}}_{3B}(\mathit{p},\lambda )\right).\end{array}$$

(40)

3.3.4. Wrist Angles

Analogously to the shoulder angles, the wrist angles $\{q_5,q_6,q_7\}$ can be calculated by composing the transformation matrix

$$\begin{array}{c}\hfill {\mathit{A}}_{74}(\mathit{p},\lambda ):={\mathit{A}}_{7R}\left({\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}}\right){\mathit{A}}_z(-\lambda ){\mathit{A}}_y\left(-{\theta }_S\right)\end{array}$$

(41)

and extracting the wrist angles with

$$\begin{array}{c}\hfill \left[\begin{array}{c}{q}_5\\ q_6\\ q_7\end{array}\right](\mathit{p},\lambda ):={\mathrm{eul}}_{ZYZ}\left({\mathit{A}}_{74}\left(\mathit{p}\right)\right).\end{array}$$

(42)

3.3.5. Overview

All closed-form expressions resulting from the IK mapping are fully listed in Appendix B. The parameter dependencies of the individual function components are

$$\begin{array}{cccc}\hfill {\mathrm{IK}}_1& :{\mathbb{R}}^4\to {\mathbb{R}}^1,\hfill & \hfill ({\theta }_{\mathrm{S}}\left(r_{\mathrm{ref}}\right),{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}},\lambda )& \mapsto q_1\hfill \end{array}$$

(43a)

$$\begin{array}{cccc}\hfill {\mathrm{IK}}_2& :{\mathbb{R}}^3\to {\mathbb{R}}^1,\hfill & \hfill ({\theta }_{\mathrm{S}}\left(r_{\mathrm{ref}}\right),{\beta }_{\mathrm{ref}},\lambda )& \mapsto q_2\hfill \end{array}$$

(43b)

$$\begin{array}{cccc}\hfill {\mathrm{IK}}_3& :{\mathbb{R}}^3\to {\mathbb{R}}^1,\hfill & \hfill ({\theta }_{\mathrm{S}}\left(r_{\mathrm{ref}}\right),{\beta }_{\mathrm{ref}},\lambda )& \mapsto q_3\hfill \end{array}$$

(43c)

$$\begin{array}{cccc}\hfill {\mathrm{IK}}_4& :{\mathbb{R}}^1\to {\mathbb{R}}^1,\hfill & \hfill \left(r_{\mathrm{ref}}\right)& \mapsto q_4\hfill \end{array}$$

(43d)

$$\begin{array}{cccc}\hfill {\mathrm{IK}}_5& :{\mathbb{R}}^3\to {\mathbb{R}}^1,\hfill & \hfill ({\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right),{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}}-\lambda )& \mapsto q_5\hfill \end{array}$$

(43e)

$$\begin{array}{cccc}\hfill {\mathrm{IK}}_6& :{\mathbb{R}}^3\to {\mathbb{R}}^1,\hfill & \hfill ({\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right),{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}}-\lambda )& \mapsto q_6\hfill \end{array}$$

(43f)

$$\begin{array}{cccc}\hfill {\mathrm{IK}}_7& :{\mathbb{R}}^4\to {\mathbb{R}}^1,\hfill & \hfill ({\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right),{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}}-\lambda )& \mapsto q_7\hfill \end{array}$$

(43g)

and show the low dimensional dependency as required by PR2. Note that parameters ${\gamma }_{\mathrm{ref}}$ and ${\psi }_{\mathrm{EE}}$ do solely influence $q_1$ and $q_7$ resp., and thus do not influence manipulability. Further, in this formulation the shoulder and wrist joints result in equivalent mappings, with symmetrical assignments. Their relations are given as

$$\begin{array}{cc}\hfill {\mathrm{IK}}_5& ={\mathrm{IK}}_3({\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right),{\beta }_{\mathrm{EE}},{\gamma }_{\mathrm{EE}}-\lambda )\hfill \end{array}$$

(44a)

$$\begin{array}{cc}\hfill {\mathrm{IK}}_6& ={\mathrm{IK}}_2({\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right),{\beta }_{\mathrm{EE}},{\gamma }_{\mathrm{EE}}-\lambda )\hfill \end{array}$$

(44b)

$$\begin{array}{cc}\hfill {\mathrm{IK}}_7& ={\mathrm{IK}}_1({\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right),{\psi }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\gamma }_{\mathrm{EE}}-\lambda ).\hfill \end{array}$$

(44c)

This is an interesting geometrical insight that results from the chosen parameter set.

3.4. Forward Kinematics

Although not used in the task space manipulability mapping, we state—for the sake of completeness—the forward mapping

$$\begin{array}{c}\hfill \mathrm{FK}:{\mathbb{R}}^7\to {\mathbb{R}}^6\times \mathbb{R},\mathit{q}\mapsto (\mathit{p},\lambda )\end{array}$$

(45)

using the developed relations from the previous section on the IK problem. From the elbow angle $q_4$ and the relation (33), $r_{\mathrm{ref}}$ is mapped by

$$r_{\mathrm{ref}}\left(q_4\right):=\sqrt{r_{SE}^2+r_{EW}^2-r_{SE}{r}_{EW}cos\left(\pi -q_4\right)}.$$

(46)

The Euler angle extraction function (24) allows again a concise definition of the remaining mappings. The shoulder joints $\{q_1,q_2,q_3\}$ with the adjoint shoulder angle ${\theta }_{\mathrm{S}}\left(r_{\mathrm{ref}}\right)$ from (35) parametrize

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\gamma }_{\mathrm{ref}}\\ {\beta }_{\mathrm{ref}}\\ \lambda \end{array}\right]& :={\mathrm{eul}}_{ZYZ}\left({\mathit{A}}_{LB}(\mathit{q},r_{\mathrm{ref}})\right)\hfill \end{array}$$

(47a)

where

$$\begin{array}{cc}\hfill {\mathit{A}}_{LB}(\mathit{q},r_{\mathrm{ref}})& :={\mathit{A}}_y\left({\theta }_{\mathrm{S}}\left(r_{\mathrm{ref}}\right)\right){\mathit{A}}_z\left(q_3\right){\mathit{A}}_y\left(q_2\right){\mathit{A}}_z\left(q_1\right).\hfill \end{array}$$

(47b)

Analogously, the wrist joints $\{q_5,q_6,q_7\}$ and the adjoint wrist angle ${\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right)$ from (37) define the end-effector parameters

$$\begin{array}{cc}\hfill \left[\begin{array}{c}\lambda +{\gamma }_{\mathrm{EE}}\\ {\beta }_{\mathrm{EE}}\\ {\psi }_{\mathrm{EE}}\end{array}\right]& :={\mathrm{eul}}_{ZYZ}\left({\mathit{A}}_{7L}(\mathit{q},r_{\mathrm{ref}}\right)\hfill \end{array}$$

(48a)

where

$$\begin{array}{cc}\hfill {\mathit{A}}_{7L}(\mathit{q},r_{\mathrm{ref}})& :={\mathit{A}}_z\left(q_7\right){\mathit{A}}_y\left(q_6\right){\mathit{A}}_z\left(q_5\right){\mathit{A}}_y(-{\theta }_{\mathrm{W}}\left(r_{\mathrm{ref}}\right)).\hfill \end{array}$$

(48b)

The composition of rotations is in accordance with the structural relation depicted in Figure 4. This concludes the FK problem.

3.5. Admissible Parameter Space

The compact analytical expressions also allow solving analytically for an upper and lower bound of $\lambda$, given maximal joint angles $q_i^{\max }$. Let $\mathcal{Q}:=\left\{\mathit{q}|\mathit{q}\in {\mathcal{Q}}_{\mathrm{sc}},|q_i|\le q_i^{\max }\right\}$ be the space of admissible joint configurations. In this section, we determine the space of admissible parameters

$$\begin{array}{c}\hfill \mathcal{A}:=\left\{(\mathit{p},\lambda )|\mathrm{IK}(\mathit{p},\lambda )\in \mathcal{Q}\right\}.\end{array}$$

(49)

Recall the definition of the parameter vector $\mathit{p}:={\left[r_{\mathrm{ref}},{\gamma }_{\mathrm{ref}},{\beta }_{\mathrm{ref}},{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},{\psi }_{\mathrm{EE}}\right]}^{\top }$ from (20). Only $r_{\mathrm{ref}}$ is of linear nature and thus has a limited range. The remaining parameters describe angles and hence need not be limited. While ${\mathrm{IK}}_4$ directly relates joint limits of the elbow joint with the admissible range of $r_{\mathrm{ref}}$, the null space parameter $\lambda$ is related to all remaining joints. Each of which can potentially exclude partitions of the full range of $\lambda$. The set of admissible parameters $\mathcal{A}$ must consider all joint limits and results from the intersection

$$\begin{array}{c}\hfill \mathcal{A}=\bigcap _{i=1}^n{\mathcal{A}}_i,\end{array}$$

(50)

of the n individual joint-related portions.

3.5.1. Shoulder-Wrist Distance $r_{\mathrm{ref}}$

Elbow joint 4 directly limits the parameter $r_{\mathrm{ref}}$. Solving (43d) for $r_{\mathrm{ref}}$ gives

$$\begin{array}{c}\hfill r_{\mathrm{ref}}\left(q_4\right):=\sqrt{r_{SE}^2+r_{EW}^2-2r_{SE}{r}_{EW}cos\left(\pi -q_4\right)}\end{array}$$

(51)

and defines the lower and upper bounds

$$\begin{array}{c}\hfill \begin{array}{ccc}{r}_{\mathrm{ref}}\left(q_4^{\max }\right)& \le r_{\mathrm{ref}}\le & r_{\mathrm{ref}}\left(0\right)\end{array}\end{array}$$

(52)

with the upper boundary $r_{\mathrm{ref}}\left(0\right)$ being the stretched out configuration of the robot. This defines

$$\begin{array}{c}\hfill {\mathcal{A}}_4:=\left\{(\mathit{p},\lambda )\in \mathcal{P}|\sqrt{r_{SE}^2+r_{EW}^2-2r_{SE}{r}_{EW}cos\left(\pi -q_4^{\max }\right)}\le r_{\mathrm{ref}}\le r_{SE}+r_{EW}\right\}\end{array}$$

(53)

as the admissible parameter set w.r.t. joint 4.

3.5.2. Null Space Parameter $\lambda$

All remaining joints, i.e., shoulder joints $\{q_1,q_2,q_3\}$ and wrist joints $\{q_5,q_6,q_7\}$, limit parts of the null space parameter $\lambda$. The 4-quadrant $\mathrm{atan}2\left(·\right)$ functions from (43), however, are difficult to symbolically rewrite in terms of $\lambda$ due to there piecewise definition. To circumvent this, we further introduce IK mappings that calculate the absolute joint angles. We define the extraction map of absolute values of the Euler sequence $|{\mathrm{eul}}_{ZYZ}|:\mathrm{SE}\left(3\right)\to {\mathbb{R}}_{+}^3$ as

$$\begin{array}{c}\hfill |{\mathrm{eul}}_{ZYZ}|:\mathrm{SE}\left(3\right)\to {\mathbb{R}}_{+}^3,{\mathit{A}}_{zyz}\mapsto \left[\begin{array}{c}arccos\left(\frac{{\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(3,1)}}{sin\left(arccos\left({\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(3,3)}\right)\right)}\right)\\ arccos\left({\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(3,3)}\right)\\ arccos\left(\frac{-{\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(1,3)}}{sin\left(arccos\left({\left[\phantom{\int }{\mathit{A}}_{zyz}\left(\mathit{z}\right)\right]}_{(3,3)}\right)\right)}\right)\end{array}\right]=:\left[\begin{array}{c}|\gamma |\\ |\beta |\\ |\psi |\end{array}\right]\end{array}$$

(54)

which is used to find the absolute angles of the shoulder and wrist joints

$$\begin{array}{cc}\hfill \left[\begin{array}{c}|q_1+{\gamma }_{\mathrm{ref}}|\\ |q_2|\\ |q_3|\end{array}\right](\mathit{p},\lambda )& :=|{\mathrm{eul}}_{ZYZ}|\left({\mathit{A}}_{3B}(\mathit{p},\lambda )\right)\hfill \end{array}$$

(55a)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}|q_5|\\ |q_6|\\ |q_7+{\psi }_{\mathrm{EE}}|\end{array}\right](\mathit{p},\lambda )& :=|{\mathrm{eul}}_{ZYZ}|\left({\mathit{A}}_{74}(\mathit{p},\lambda )\right)\hfill \end{array}$$

(55b)

analogously to the mapping ${\mathrm{eul}}_{ZYZ}$ from the previous Section 3.3. See Appendix C for the full definition of the absolute valued IK functions. Note that the mappings admit the same symmetrical assignments between the shoulder and wrist portion as the actual IK mapping discussed in Section 3.3.5.

Due to the concise formulations of the IK (55a), all functions can be solved for the null space parameter $\lambda$. By substituting the joint parameters with their respective limit, closed-form expressions are formed that deliver $s_i$ candidates for lambda ranges

$$\begin{array}{c}\hfill {\mathit{\lambda }}_i^{lim}:{\mathbb{R}}^7\times \mathbb{R}\to {\mathbb{C}}^{s_i},(\mathit{p},q_i^{\max })\mapsto {\mathit{\lambda }}_i^{lim}(\mathit{p},q_i^{\max })\forall i\in [1,7]\setminus 4\end{array}$$

(56)

according to the $i=[1,7]$ joints. For $q_2$ and $q_6$, the respective middle joints of the shoulder and wrist angle tuples $(q_1,q_2,q_3)$ and $(q_5,q_6,q_7)$, we directly find $s_2=s_6=2$ symmetric solutions for a positive and negative null space limit. However, solving the remaining mappings from IK (55a) for $\lambda$, results in more solution candidates. This results from the fact that, depending on the parameter configuration, these joints have the potential for cyclic behaviour for a linear increase in $\lambda$ at a fixed pose (discussed in [15]). Joints $q_3$ and $q_5$ can thus reach up to $s_3=s_5=4$ null space angles marking a joint limit. The first joint $q_1$ and last joint $q_7$ do also offer up to 4 critical values for $\lambda$, however, due to additional additive parameters ${\gamma }_{\mathrm{ref}}$ and ${\psi }_{\mathrm{EE}}$ resp., it is necessary to additionally consider solutions for $|-q_1+{\gamma }_{\mathrm{ref}}|$ and $|-q_7+{\gamma }_{\mathrm{ref}}|$. These solutions are evaluated with ${\mathit{\lambda }}_1^{lim}(\mathit{p}{|}_{-{\gamma }_{\mathrm{ref}}},q_1^{\max })$ and ${\mathit{\lambda }}_7^{lim}(\mathit{p}{|}_{-{\psi }_{\mathrm{EE}}},q_7^{\max })$. Consequently, $s_1=s_7=8$ solution candidates for the first and last joint of the kinematic have to be considered.

Besides knowing the value of a critical limit, it is further essential for many applications to know if it expresses an upper or a lower limit. Similar to the approach in [16], the partial derivatives of the null space range mappings ${\mathit{\lambda }}_i^{lim}$ w.r.t. the corresponding joint angle limit are used to characterize each limit candidate. For every $\ell \in {\mathit{\lambda }}_i^{lim}$, the corresponding partial derivative is evaluated to decide

$$\begin{array}{c}\hfill \ell \in \left\{\begin{array}{cc}\mathrm{is}\mathrm{upper}\mathrm{limit}\hfill & \mathrm{if}\mathrm{sign}\left(\ell \right)\frac{\partial {\mathit{\lambda }}_i^{lim}}{\partial q_i^{\max }}>0\hfill \\ \mathrm{is}\mathrm{lower}\mathrm{limit}\hfill & \mathrm{if}\mathrm{sign}\left(\ell \right)\frac{\partial {\mathit{\lambda }}_i^{lim}}{\partial q_i^{\max }}<0\hfill \\ \mathrm{is}\mathrm{no}\mathrm{limit}\hfill & \mathrm{otherwise}.\hfill \end{array}\right.\end{array}$$

(57)

In a second step, all solution candidates in ${\mathit{\lambda }}_i^{lim}$ are tested for validity, to define the sets of actual upper and lower null space limit angles

$$\begin{array}{cc}\hfill {\mathcal{L}}_i^{\mathrm{up}}\left(\mathit{p}\right)& :=\left\{\lambda \in {\mathit{\lambda }}_i^{lim}|\lambda \in \mathbb{R}\wedge |{\mathrm{IK}}_i(\mathit{p},\lambda )|=q_i^{\max }\wedge \mathrm{sign}\left(\lambda \right)\frac{\partial \lambda }{\partial q_i^{\max }}>0\right\}\forall i\in [1,7]\setminus 4\hfill \end{array}$$

(58a)

$$\begin{array}{cc}\hfill {\mathcal{L}}_i^{\mathrm{low}}\left(\mathit{p}\right)& :=\left\{\lambda \in {\mathit{\lambda }}_i^{lim}|\lambda \in \mathbb{R}\wedge |{\mathrm{IK}}_i(\mathit{p},\lambda )|=q_i^{\max }\wedge \mathrm{sign}\left(\lambda \right)\frac{\partial \lambda }{\partial q_i^{\max }}<0\right\}\forall i\in [1,7]\setminus 4.\hfill \end{array}$$

(58b)

These upper and lower limits form j pairwise ranges ${\mathsf{\Lambda }}_{i,j}$ and define the remaining admissible parameter sets

$${\mathcal{A}}_i:=\left\{(\mathit{p},\lambda )\in \mathcal{P}|\lambda \in \bigcup _j{\mathsf{\Lambda }}_{i,j}\right\}\forall i\in [1,7]\setminus 4,$$

(59)

related to shoulder and wrist joints.

The full intersection set $\mathcal{A}$, as defined in (50), may consist of several separate regions. Directly evaluating all critical values of $\lambda$ is especially interesting whenever planning a continuous path in task space. We apply the admissible parameter space in application Section 5.1.3 and Section 5.2.2. All full function definitions of the limit candidates ${\mathit{\lambda }}_i^{lim}$ are summarized in Appendix D.

4. Results

This section contains an evaluation of the task space manipulability framework developed in this work. We first give a run-time comparison to show the computational advantage of our closed-form expression in comparison to general numerical solutions. We show that uniform sampling in the new parameter space results in a superior probability distribution of the manipulability in comparison with direct sampling in joint space. Further, the sensitivity of the manipulability measure w.r.t. the parameters is analyzed.

4.1. Accuracy

Unlike numerical IK solvers that approximate the inverse mapping iteratively [42], or CLIK solvers [10,11,12] that converge to the solution from a control point of view, the analytical nature of our closed-form task space manipulability expression delivers exact results in a single iteration.

4.2. Run-Time Comparison

Complete evaluation of the closed-form IK and M mapping as single expressions allows automatic code generation of the symbolic expressions with e.g., the MATLAB Coder™ toolbox. These expressions allow array operations, or vectorization in MATLAB, such that a large number of solutions can be evaluated simultaneously. This leads to a significant computational boost, compared to algorithms that rely on matrix arithmetic and consequently have to sequentially evaluate multiple evaluations in programmatic loops. This property makes it further straightforward to calculate the task space manipulability of multiple samples on a powerful Graphics Processing Unit (GPU). The following run-time comparison was conducted in MATLAB 2019a, on a computer with Intel(R) Core(TM) i9-9900X CPU @ $3.50$ $\mathrm{G}$$\mathrm{Hz}$, 128 $\mathrm{G}$ memory, and a NVIDIA TITAN V graphics card.

Besides different versions of our presented algorithm, we also tested the run-time of [15], representing typical analytical IK approaches in the literature, and the nonlinear optimization-based IK algorithm from the Robotics System Toolbox™ for MATLAB, representing iterative solver approaches. Figure 5 shows a run-time comparison of calculating the manipulability measures

$$\begin{array}{c}\hfill {\mu }_n:=(\mathrm{M}\circ \mathrm{IK})\left({\mathit{p}}_n\right),\mathrm{for}n=[1,N]\end{array}$$

(60)

of N random samples ${\mathit{p}}_n$.

As expected, the iterative optimization algorithm (applying BFGS Gradient Projection with solution tolerance 0.01) is the computationally most expensive solution method. It required an average of 37 iterations per pose and did not allow for direct selection of the arm angle. Two orders of magnitude faster and in addition producing exact inverse solutions are the analytical IK solvers found in the literature. They rely on matrix calculus and thus a for-loop structure for evaluation of multiple poses.

Our approach, which is entirely reduced to direct individual expressions, is over 10 times faster when implemented with the same conventional for-loop structure. Already for 200 evaluated samples, a simultaneous vectorized evaluation achieves another performance increase of factor 10. At the maximal evaluated amount of 10⁷ samples, vectorization enables an even 50 times faster computation, compared to the implementation using for-loops. The advantage of calculating the task space manipulability on a GPU starts at an amount of 10⁵ sample points. For a smaller number of samples, the overhead of initializing the data on the GPU does not pay off. Processing 10⁷ samples, calculations on the GPU are 10 times faster then vectorized treatment on the CPU, and even 700 times faster than for conventional loop structures.Note that all time measurements include the generation of random samples on the CPU and GPU respectively.

Considering real-time application for a robot with a typical 1 $\mathrm{k}$$\mathrm{Hz}$ sampling rate, our approach allows evaluation of 1000 end-effector poses for their task space manipulability.

4.3. Sampling in Task Space

Not having to calculate the IK in an iterative fashion as done by CLIK solvers, evaluating manipulability directly in task space is computationally not much more expensive than directly calculating manipulability in joint space. However, choosing a different space for sampling random poses do have an influence on the probability distribution of resulting manipulability measures.

Before analyzing this difference, we first discuss the used sampling strategies. For a fair comparison, we cover the entire space without consideration of possible limits on the individual joints or parameters.

Let $u\in \mathbb{R}$ be a random number drawn from a uniform distribution in the range of $[0,1]$. Uniform sampling in joint space is straightforward with

$$\begin{array}{cc}\hfill q_i^{\mathrm{uniform}}& :\mathbb{R}\to \mathbb{R},u\mapsto -\pi +2\pi u\forall i\in [1,7]\hfill \end{array}$$

(61)

due to the independence of its joints $\mathit{q}\in {\mathbb{R}}^7$.

For a random end-effector pose sample $({\mathit{p}}^{\mathrm{red}},\lambda )={[r_{\mathrm{ref}},{\beta }_{\mathrm{ref}},{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},\lambda ]}^{\top }$ from the parameter space, one can choose the same strategy

$$\begin{array}{cc}\hfill p_1^{\mathrm{naive}}& :\mathbb{R}\to \mathbb{R},u\mapsto r_{\mathrm{ref}}^{\min }+(r_{\mathrm{ref}}^{\max }-r_{\mathrm{ref}}^{\min })u\hfill \end{array}$$

(62a)

$$\begin{array}{cc}\hfill p_i^{\mathrm{naive}}& :\mathbb{R}\to \mathbb{R},u\mapsto -\pi +2\pi u\forall i\in [2,5]\hfill \end{array}$$

(62b)

with respective scaling for the linear parameter $r_{\mathrm{ref}}$. However, this naive form of sampling does not lead to a uniform distribution of samples in the task space SE(3), due to the interdependence of the coordinate components.

Recall that the first two parameters $r_{\mathrm{ref}}$ and ${\beta }_{\mathrm{ref}}$ describe translation in polar coordinates. Unlike in Cartesian coordinates, the base vectors are not constant. Consequently, direct uniform sampling of the radial coordinate $r_{\mathrm{ref}}$, leads to sparser sampling further from the origin, due to the increasing circumference proportionally to $r_{\mathrm{ref}}$. Proper uniform sampling of the translational part can be achieved by

$$\begin{array}{cc}\hfill p_1^{\mathrm{uniform}}& :\mathbb{R}\to \mathbb{R},u\mapsto \sqrt{{\left(r_{\mathrm{ref}}^{\min }\right)}^2+\left({\left(r_{\mathrm{ref}}^{\max }\right)}^2-{\left(r_{\mathrm{ref}}^{\min }\right)}^2\right)u}\hfill \end{array}$$

(63a)

$$\begin{array}{cc}\hfill p_2^{\mathrm{uniform}}& :\mathbb{R}\to \mathbb{R},u\mapsto -\pi +2\pi u.\hfill \end{array}$$

(63b)

An efficient method of uniform sampling on SO(3), i.e., 3D orientations, is proposed by Kuffner [43]. Uniform sampling of the individual angles of the Euler sequence results in a bias towards the polar regions of the unit sphere. He proposes to use an arctan function on the second angle to compensate for this bias. Uniform sampling of the end-effector orientation, parametrized by ${\gamma }_{\mathrm{EE}}$ and ${\beta }_{\mathrm{EE}}$, is thus achieved with

$$\begin{array}{cc}\hfill p_3^{\mathrm{uniform}}& :\mathbb{R}\to \mathbb{R},u\mapsto -\pi +2\pi u\hfill \end{array}$$

(63c)

$$\begin{array}{cc}\hfill p_4^{\mathrm{uniform}}& :\mathbb{R}\to \mathbb{R},u\mapsto arccos\left(1-2u\right).\hfill \end{array}$$

(63d)

The last portion in our parameter tuple $(\mathit{p},\lambda )$ is the null space parameter $\lambda$ that is independent and thus remains

$$\begin{array}{cc}\hfill p_5^{\mathrm{uniform}}& :\mathbb{R}\to \mathbb{R},u\mapsto -\pi +2\pi u.\hfill \end{array}$$

(63e)

Figure 6 illustrates the uniform sampling of the task space applying the uniform sampling strategy (63).

The above-discussed sampling strategies are now analyzed in conjunction with their respective mapping to the manipulability measure. Figure 7 shows the approximated cumulative distribution function (CDF) of manipulability resulting from 10⁷ random samples. It shows that random sampling in joint space according to (61) is more likely to result in a joint configuration with poor manipulability of the robot. Uniform sampling in parameter space (63) produces much fewer joint configurations with poor manipulability, while at the same time more configurations with high manipulability. Naive sampling in parameter space (61) performs similarly good in the low manipulability section. However, it produces also fewer configurations with high manipulability. Considering a conventional 6-DOF robot, i.e., fixing the null space parameter $\lambda$ to 0 or $\pi$, results in a slightly better probability density function (PDF) than for the discussed 7-DOF mechanism. This is a surprising result, as it is always argued that the redundancy improves manipulability. While it is true that the additional DOF has the potential to improve performance measures, poor exploitation might achieve the opposite.

Kuhlemann et al. [44] showed in different use-cases that the seventh DOF of the KUKA LBR iiwa increased the average dexterity by 16% in comparison to a conventional 6 DOF KUKA KR 10. Both the shortcomings of the naive parameter sampling strategy and the apparent advantage of the 6-DOF mechanism are discussed in Section 4.4.4.

The average normalized manipulabilities achieved are 37% for uniform joint space sampling, 43% for naive parameter space sampling, and 50% for uniform sampling in parameter space. All numbers are w.r.t. the maximal encountered manipulability.

4.4. Parameter Sensitivity Analysis of Manipulability in Parameter Space

The sensitivity of the task space manipulability w.r.t. its parameters are analyzed by generating 10⁷ random samples according to (63). These samples represent a uniform distribution of task space configurations. Figure 8 shows the bi-variate histograms of manipulability $\mu ({\mathit{p}}^{\mathrm{red}},\lambda )$ w.r.t. to the individual parameters.

Colors approximate the PDF of $\mu (r_{\mathrm{ref}},{\beta }_{\mathrm{ref}},{\gamma }_{\mathrm{EE}},{\beta }_{\mathrm{EE}},\lambda )$ at fixed values of the respective parameter. For all parameter values we find unimodal distributions, i.e., distributions with a single maxima.

4.4.1. Translation Parameters $r_{\mathrm{ref}}$ and ${\beta }_{\mathrm{ref}}$

The PDF of $\mu$ along the shoulder-wrist distance $r_{\mathrm{ref}}$ shows a preferred value of $0.57$ $\mathrm{m}$. Although a manipulability optimizing configuration cannot be found at this given value, the mode of the corresponding PDF, i.e., its local maxima, has the highest value of manipulability. Further, the probability of good manipulation is decreasing with $r_{\mathrm{ref}}$ towards the workspace singularity, i.e., a fully stretched arm of robot configuration.

The polar angle ${\beta }_{\mathrm{ref}}$ between the vertical and the shoulder-wrist reference vector has the highest manipulability mode at $\frac{\pi }{2}$$\mathrm{rad}$, although manipulability maximizing configurations are not found. For values approaching 0 and $\pi$$\mathrm{rad}$, i.e., placing the wrist in line with the axis of base joint $q_1$, typically cause so-called shoulder singularities on conventional 6-DOF robots. While the 7-DOF kinematics do not necessarily result in a kinematic singularity, high manipulability is not possible either.

4.4.2. Orientation Parameters ${\gamma }_{\mathrm{EE}}$ and ${\beta }_{\mathrm{EE}}$

The third parameter ${\gamma }_{\mathrm{EE}}$, which describes a rotation around the shoulder-wrist vector, is the only one that seems to cause little variation in the manipulability PDF and does not allow a conclusion over a preferred configuration.

The consecutive rotation angle ${\beta }_{\mathrm{EE}}$ shows a similar influence as the reference angle ${\beta }_{\mathrm{ref}}$. However, the mode of these PDFs is less prominent and tendentiously marks a lower manipulability.

4.4.3. Null Space Parameter $\lambda$

The null space parameter $\lambda$ reveals that the highest manipulabilities can be found at $\lambda =\{0,\pm \pi \}$$\mathrm{rad}$, i.e., the conventional upper and lower elbow configuration of 6-DOF kinematics. Although missing the absolute top manipulability poses, only small deviations of about $\pm 0.1$ $\mathrm{rad}$ from these configurations result in a decrease of the manipulability mode of 25%, i.e., from $0.8$ to $0.6$. Better modes are found at $\lambda =\{\pm \frac{\pi }{2}\}$$\mathrm{rad}$. Not only is their peak at a slightly higher manipulability of $0.85$, but they are also less sensitive to a parameter change in $\lambda$. The latter is especially valuable for staying agile during unforeseen events.

4.4.4. Discussion of Manipulability in Different Sampling Strategies

The different sampling strategies discussed in Section 4.3 result in differences in the approximated CDFs, cf. Figure 7.

Naive vs. Uniformly Distributed Sampling

The difference between naive and uniform sampling solely affects parameters $r_{\mathrm{ref}}$ and ${\beta }_{\mathrm{EE}}$. That is, the corresponding uniform sampling functions (63a) and (63d) correct the biases of the radial coordinate $r_{\mathrm{ref}}$ towards the origin, and the orientation towards the pole regions with azimuthal angle ${\beta }_{\mathrm{EE}}=\{0,\pi \}$$\mathrm{rad}$, respectively. Consequently, these regions are sparser sampled in the uniformly distributed strategy. While this correction is negligible for the range of $r_{\mathrm{ref}}$ in this particular robot example, the improvement of the CDF towards better manipulability stems from a sparser sampling of the boundary regions of ${\beta }_{\mathrm{EE}}$. Because exactly these boundaries lack high manipulability poses, as visible in the according bi-variate histogram in Figure 8.

6-DOF vs. 7-DOF Kinematics

According to Section 4.4.3, the apparent slight advantage of uniform distributed sampling of a conventional 6-DOF robot only holds for the over-all manipulability distribution illustrated in Figure 7. The parameter-specific histogram w.r.t. to the arm angle $\lambda$ in Figure 8, on the other hand, reveals that the conventional 6-DOF configurations $\lambda =\{0,\pm \pi \}$$\mathrm{rad}$ do have a good manipulability distribution, but $\lambda =\{\pm \frac{\pi }{2}\}$$\mathrm{rad}$ configurations are preferable. A 7-DOF kinematics hence not only enables agile adaptation of the kinematic structure, but also contains arm angles that have a better PDFs of manipulability than its 6-DOF counterpart. At the same time, other arm angles show higher variability in the histogram and are more prone to decrease performance. An increase in manipulability by the additional DOF thus relies on a well-conceived utilization of such.

4.5. Number of Local Optima

While the analysis shown in the previous section gives insight in the probability distribution of the manipulability measure, it does not allow conclusions on how manipulability changes along the null space.

Table 1. Distribution of local optima among 10⁷ samples.

# optima	1	2	3	4
percentage	20%	41%	27%	12%

lists the number of local optima for a given end-effector pose. It shows that 80% of the robot poses do not have a unique manipulability maximizing null space solution, but up to 4 distinct optima.

5. Applications

Two application directions that benefit from the closed-form expressions of the task space manipulability are outlined in this section. First, we demonstrate how global optimization problems can be formulated that profit from massive multi-start point pre-evaluation. Second, we propose a novel way of real-time redundancy resolution on the position level, which enables global manipulability optimization of single poses as well as for provided end-effector trajectories in SE(3).

5.1. Optimal Robot Placement

The analytic results from the previous Section 3 allow formulating interesting questions in terms of optimization problems. We consider the problem of optimal placement of the robot.

5.1.1. Best Overall Robot Configuration

The most basic optimization problem we considere is the question of finding the best overall robot configuration w.r.t. to manipulability. Mathematically, this problem can be stated as an unconstrained optimization problem

$$\begin{array}{c}\hfill \underset{\mathit{q}}{\mathrm{maximize}}\mu \left(\mathit{q}\right)\end{array}$$

(64)

directly finding the optimal joint configuration w.r.t. the manipulability measure. The global optimum is found with a multi-start strategy [45], where random samples are drawn from the admissible parameter space $\mathcal{P}$ and used as starting points for local optimizations. Figure 9, left side, shows the results of such a global optimization process with 1000 starting points. Note that the same problem can be formulated in parameter space and does yield the same result. All optimization iterations result in one of 8 equally good global optima, which can be reduced to 4 solutions due to symmetry of the shoulder joint. They further describe configurations in the pure $xz$-plane with $\lambda \in \{0,\pm 180\}$${}^{\circ }$. This is equivalent to the configurations achievable by a conventional 6-DOF robot.

5.1.2. Best Robot Configuration for Multiple Task Poses

In industrial settings, robots are often required to work at a certain number $i\in {\mathbb{Z}}^{+}$ of different task poses ${\mathit{z}}_i$. While the relative distances $\Delta {\mathit{z}}_i={\mathit{z}}_i-{\mathit{z}}_1$ between this poses is defined, the optimal placement of the robot can be found by solving the optimization problem

$$\begin{array}{c}\hfill \underset{\mathit{z},\lambda }{\mathrm{maximize}}\sum _i(\mathrm{M}\circ \mathrm{IK}\circ \mathrm{TSP})(\mathit{z}+\Delta {\mathit{z}}_i,\lambda )\end{array}$$

(65)

to find the relative pose $\mathit{z}$ that maximizes the average manipulability of all i poses. Solving this problem directly, results in an infinite number of global poses. These solutions are rotationally symmetric around the base joint $q_1$ as well as the last joint $q_7$, as both these joints do not have an influence on the manipulability of the 7-DOF robot structure at consideration (discussed in Section 3.1).

The complexity of the optimization problem, as well as the number of global optima, can be drastically reduced by formulating the same problem in the lower dimensional parameter space

$$\begin{array}{c}\hfill \underset{\mathit{p},\lambda }{\mathrm{maximize}}\sum _i(\mathrm{M}\circ \mathrm{IK})(\mathit{p}+\Delta {\mathit{p}}_i,\lambda )\end{array}$$

(66)

where ${\mathit{p}}_i=\mathrm{TSP}\left({\mathit{z}}_i\right)$. The resulting optimal $\mathit{p}$ can eventually be mapped to the corresponding task space parameter $\mathit{z}=\mathrm{TSS}\left(\mathit{p}\right)$. This result is useful for deciding on how to mount a robot relative to a given set of task poses ${\mathit{z}}_i$, or recalculating it online if task poses are time-variant and the robot structure is e.g., mounted on a mobile platform.

5.1.3. Optimizing Robot Mounting Positions Regarding a Workspace Envelope

In a modern scenario where robots are not only expected to repetitively execute the same tasks, a set of pre-defined task poses cannot always be formulated. But it is rather necessary for the robot to perform well in a defined workspace volume, e.g., given as a cubical volume $V=[-\Delta \frac{x}{2},+\Delta \frac{x}{2}]\times [-\Delta \frac{y}{2},+\Delta \frac{y}{2}]\times [0,+\Delta z]$. Due to all mappings involved in the task space manipulability being continuous, formulating a cost function for such a volume can be done using Fubini’s theorem [46]. It allows calculation of the volume integral as triple integral. The objective for this optimization problem in task space reads

$$\begin{array}{c}\begin{array}{cc}\underset{{\mathit{z}}_0,\lambda }{\mathrm{maximize}}\hfill & \underset{V}{\int \int \int }(\mathrm{M}\circ \mathrm{IK}\circ \mathrm{TSP})({\mathit{z}}_0+\mathit{z}(x,y,z),\lambda )\mathrm{d}x\mathrm{d}y\mathrm{d}z\hfill \\ \mathrm{subject}\mathrm{to}\hfill & \mathrm{TSP}\left({\mathit{z}}_0,+\mathit{z}(x,y,z),\lambda \right)\in \mathcal{A},\hfill \end{array}\hfill \end{array}$$

(67)

where the optimal task space volume origin ${\mathit{z}}_0$ needs to be found. This optimization can again be transformed to the lower dimensional parameter space

$$\begin{array}{c}\begin{array}{cc}\underset{{\mathit{p}}_0,\lambda }{\mathrm{maximize}}\hfill & \underset{V}{\int \int \int }(\mathrm{M}\circ \mathrm{IK}\circ \mathrm{TSP})(\mathrm{TSS}\left({\mathit{p}}_0\right)+\mathit{z}(x,y,z),\lambda )\mathrm{d}x\mathrm{d}y\mathrm{d}z\hfill \\ \mathrm{subject}\mathrm{to}\hfill & \mathrm{TSP}\left(\mathrm{TSS}({\mathit{p}}_0,\lambda )+\mathit{z}(x,y,z),\lambda \right)\in \mathcal{A}\hfill \end{array}\hfill \end{array}$$

(68)

with the condition that the whole Volume projected to parameter space must be within the set of admissible parameters. Figure 9, right side, shows the result of such a global optimization.

5.2. Redundancy Resolution

Solving for optimal robot poses online is essential for a robot to stay agile at all times. We demonstrate how the task space manipulability expressions developed in this work can be applied for real-time global manipulability optimization of single poses as well as full trajectories. The following run-time evaluations were conducted in MATLAB 2019a, on a computer with Intel(R) Core(TM) i3-7100 CPU @ $3.9$ $\mathrm{G}$$\mathrm{Hz}$ and 32 $\mathrm{GB}$ memory.

5.2.1. Redundancy Resolution for Global Manipulability Optima

Approaches typically found in the literature focus on local optimization of manipulability based on local gradient information. Analysis of the number of existing local optima from Section 4.5, however, revealed that only 20% of end-effector poses have a unique global optimum. The computational advantage of our approach permits evaluating the manipulability of many poses simultaneously. Given a current robot pose $\mathit{z}$, our framework makes it possible to not only locally improve manipulability, but solve

$$\begin{array}{c}\hfill \begin{array}{cc}\underset{\lambda }{\arg \max }& (\mathrm{M}\circ \mathrm{IK}\circ \mathrm{TSP})(\mathit{z},\lambda )\end{array}\end{array}$$

(69)

with a representative number of null space solution at a high resolution in real-time. Given the information of this greedy optimization strategy, the close-to-global optimum configuration can simply be picked. Solving for global optima in $0.25$ $\mathrm{m}$$\mathrm{s}$ at a resolution of 1° for $\lambda$ enables application at typical robot sampling rates of 1 $\mathrm{k}$$\mathrm{Hz}$.

Figure 10 shows manipulability of the full null space at a particular configuration.

This is an example of a pose with 4 local optima. If the current configuration of the robot is the solution for the given pose with the null space parameter $\lambda \in [0,85]$${}^{\circ }$, a local optimization will only drive the redundancy resolution into a sub-optimal minima. In contrast, our approach allows finding the globally best configuration w.r.t. the admissible parameter space.

5.2.2. Optimizing Null Space Solution of Given End-Effector Trajectory

Several approaches can be found in the literature that maximize either the volume of a manipulability ellipsoid [31,47,48,49] or a predefined shape of the ellipsoid [33]. Yet all these approaches consider only local optimization.

Finding the best joint configuration for a given pose in task space simplifies to a 1D line search. However, given a full path in SE(3) it is also possible to find an optimal elbow trajectory that maximizes, e.g., the average manipulability while avoiding getting trapped in regions of poor manipulability. Note that a real manipulation task relies on a sophisticated path planner, capable of generating task-related paths that avoid obstacles while potentially fulfilling additional criteria. Knowledge about the task space manipulability, e.g., provided by our approach, may even be exploited by such a planner. This is, however, not the direct scope of this work. Instead, for a minimal working example, we use direct interpolation

$$\begin{array}{c}\hfill \mathit{p}\left(s\right)=s{\mathit{p}}^{\mathrm{start}}+(s-1){\mathit{p}}^{\mathrm{end}}\mathrm{with}s=[0,1]\end{array}$$

(70)

between two poses as a simple path planner. Given are two random poses as depicted in Figure 11 to the left. On the right side of Figure 11, a contour plot of the manipulability of the full null space along the trajectory is shown. Red lines indicate not passable values in the null space due to joint limits, cf. Section 3.5. The blue line marks the trajectory that results from local optimization of manipulability. Note that at $s=0.4$, the local optimization hits a joint limit of $q_2$. We stopped the line here, because it depends on a potential strategy for joint limit avoidance, which is not the scope of this work. A global optimization strategy that has predictive knowledge of the full null space development can exploit an initially sub-optimal path toward negative values of $\lambda$ to circumvent the region of poor manipulability between $s=[0.6,1]$. But this is usually not feasible in an online scenario with conventional global optimization strategies.

The computational advantage of our strategy, as seen in Figure 5, allows the computation of such a map with, e.g., a resolution of 100 steps in both parameters, s and $\lambda$, in under 5 $\mathrm{m}$$\mathrm{s}$. In combination with an online trajectory generator directly on SE(3), e.g., [50], this qualifies our task space manipulability approach to be used for predictive online manipulability optimization, e.g., with a receding horizon.

6. Conclusions

Today’s demand for adaptive and reactive robot behaviour requires sustaining the agility of a kinematic structure at all times. While manipulability is a common metric in robot research to quantify the capabilities of a robot at a given joint configuration, the robot task is directly defined in end-effector poses, which allows for multiple possible solutions. Unlike common metrics, which do not include the robot IK, a task space manipulability formulation is required to directly map an end-effector pose together with its null space solution onto the manipulability metric.

To achieve reactive robot behaviour, optimization of the null space at given poses must be performed online. In general, this requires efficient evaluation of a large number of configurations, especially in the case of redundant robots. In this work we developed a new closed-form approach for calculating manipulability directly from task space poses, for a redundant 7-DOF S-R-S serial robot kinematics. A novel parametrization of the task- and null space leads to concise IK, as well as admissible parameter mappings, which show symmetry in the structures of their individual expressions. Analysis of the resulting task space manipulability further revealed that the majority of end-effector poses do not have a unique, manipulability-maximizing null space solution. We thus argue that local optimization of the manipulability measure is not sufficient. A global optimization at high sampling frequencies, however, is not feasible with current approaches in the literature. The entire composition of the task space manipulability map proposed in this work allows for efficient array operations that can be exploited in vector-optimized programming languages, as well as GPU computing. Consequently, the simultaneous computation of a large number of poses in real-time is made possible. Our method, therefore, enables global online optimization of manipulability for single poses and even full SE(3) trajectories.

Future work will focus on further application development of our framework. Combining our task space manipulability approach with online planners opens an interesting field of predictive redundancy resolution for global manipulability optimization.