Task-space separation principle: a force-field approach to motion planning for redundant manipulators

Consider an n-dimensional, kinematically redundant manipulator with configuration vector (joint angles) $\boldsymbol{q}\in \mathcal{Q}\subset {{\mathbb{R}}^{n}}$ and an m-dimensional task-space $\boldsymbol{x}\in \mathcal{X}\subset {{\mathbb{R}}^{m}}$, with m  <  n, defined by a smooth forward kinematics:

$$\begin{eqnarray}&&\boldsymbol{x}=FK\left(\boldsymbol{q}\right)\end{eqnarray} \tag{ 2 }$$

A typical problem is finding a joint-space configuration $\boldsymbol{q}$ leading to a given desired task-space configuration $\boldsymbol{x}$, i.e. as in (2). For kinematically redundant manipulators, characterized by m  <  n, this is typically an under-determined problem as there often exist infinite joint-space configurations solving a given task. Due to nonlinearities, finding the analytical inverse to equation (2) is in general not a viable solution. A typical approach consists of operating on a linearized problem, i.e. considering the relationship between task velocities $\overset{\centerdot}{{\boldsymbol{x}}}\,$ and joint-space velocities $\overset{\centerdot}{{\boldsymbol{q}}}\,$ via the task Jacobian J

$$\begin{eqnarray}&&\overset{\centerdot}{{\boldsymbol{x}}}\,=\frac{\partial FK}{\partial \boldsymbol{q}}\overset{\centerdot}{{\boldsymbol{q}}}\,=J\,\overset{\centerdot}{{\boldsymbol{q}}}\,\end{eqnarray} \tag{ 3 }$$

where J is an $m\times n$ matrix and which, hereafter, shall be assumed of full rank, i.e. $\dim(\,J)=m$. Equation (3) can be interpreted as a kinematic constraint (i.e. a given task-space velocity) applied at the end-effector.

In our previous work [11], we performed a task-constrained minimization of a static cost function $h\left(\boldsymbol{q}\right)$, analogous of an elastic potential, and this allowed for the determination of optimal postures among those compatible with a given task. However, this formulation only considers static postures and does not address movement. We shall now consider a dynamic cost function and consider the analogous of a viscoelastic power as this will also include velocities and determine optimal motions as well. Following [20], a unique and optimal solution for $\overset{\centerdot}{{\boldsymbol{q}}}\,$ can be formulated as a constrained optimization problem:

$$\begin{eqnarray}&&\underset{\overset{\centerdot}{{\boldsymbol{q}}}\,}{{\text{argmin}}}\,\left\{\frac{1}{2}{{\overset{\centerdot}{{\boldsymbol{q}}}\,}^{T}}W\left(\boldsymbol{q}\right)\overset{\centerdot}{{\boldsymbol{q}}}\,+\frac{\text{d}h}{\text{d}t}\right\}~~\text{while}~~\overset{\centerdot}{{\boldsymbol{x}}}\,=J\overset{\centerdot}{{\boldsymbol{q}}}\,\end{eqnarray} \tag{ 4 }$$

where $W\left(\boldsymbol{q}\right)$ corresponds to a positive-definite symmetric matrix for the joint-space velocities (defining thus a metric, or dot-product, for the space $\mathcal{Q}$) and $h\left(\boldsymbol{q}\right)$ is posture-dependent cost function such has those previously shown to account for human postural control [15] and kinematic synergies [11]. Similarly, the rate of change of the potential (i.e. power, in a mechanical analogy) $\frac{\text{d}h}{\text{d}t}$ can be rewritten via the gradient operator (i.e. a torque, in joint-space) as:

$$\begin{eqnarray}&&\frac{\text{d}h}{\text{d}t}={{\nabla}_{\boldsymbol{q}}}~{{h}^{T}}\overset{\centerdot}{{\boldsymbol{q}}}\,\end{eqnarray} \tag{ 5 }$$

Formally, the original constrained-optimal problem can be formulated as an unconstrained optimization over an augmented state ($\overset{\centerdot}{{\boldsymbol{q}}}\,;\boldsymbol{\lambda }$) of the following function

$$\begin{eqnarray}&&H\left(\overset{\centerdot}{{\boldsymbol{q}}}\,;\boldsymbol{\lambda }\right):=\frac{1}{2}{{\overset{\centerdot}{{\boldsymbol{q}}}\,}^{T}}W\left(\boldsymbol{q}\right)\overset{\centerdot}{{\boldsymbol{q}}}\,+{{\nabla}_{\boldsymbol{q}}}~{{h}^{T}}\overset{\centerdot}{{\boldsymbol{q}}}\,+\boldsymbol{\lambda }^{{T}}\left(\overset{\centerdot}{{\boldsymbol{x}}}\,-J\overset{\centerdot}{{\boldsymbol{q}}}\,\right)\end{eqnarray} \tag{ 6 }$$

where $\boldsymbol{\lambda }={{\left[{{\lambda}_{1}},{{\lambda}_{2}},...,{{\lambda}_{m}}\right]}^{T}}$ is the vector of so-called Lagrange multipliers (LM). Local minima or maxima of unconstrained problems can be determined by setting

$$\begin{eqnarray}\left\{\begin{array}{*{35}{l}} {{\nabla}_{\overset{\centerdot}{{\boldsymbol{q}}}\,}}H\left(\overset{\centerdot}{{\boldsymbol{q}}}\,;\boldsymbol{\lambda }\right) & = & 0 \\ {{\nabla}_{\boldsymbol{\lambda }}}H\left(\overset{\centerdot}{{\boldsymbol{q}}}\,;\boldsymbol{\lambda }\right) & = & 0 \end{array}\right. \end{eqnarray} \tag{ 7 }$$

where ${{\nabla}_{\overset{\centerdot}{{\boldsymbol{q}}}\,}}=\left[\begin{array}{*{35}{l}} \frac{\partial}{\partial {{\overset{\centerdot}{{q}}\,}_{1}}} & \frac{\partial}{\partial {{\overset{\centerdot}{{q}}\,}_{2}}} & \ldots & \frac{\partial}{\partial {{\overset{\centerdot}{{q}}\,}_{n}}} \end{array}\right]$ and ${{\nabla}_{\boldsymbol{\lambda }}}=$$\left[\begin{array}{*{35}{l}} \frac{\partial}{\partial {{\lambda}_{1}}} & \frac{\partial}{\partial {{\lambda}_{2}}} & \ldots & \frac{\partial}{\partial {{\lambda}_{m}}} \end{array}\right]$ represent, respectively, the gradient operators over the joint velocities $\overset{\centerdot}{{\boldsymbol{q}}}\,$ (n-dimensional) and over the vector of LM $\boldsymbol{\lambda }$ (m-dimensional).

Considering that terms such as ${{\nabla}_{\overset{\centerdot}{{\boldsymbol{q}}}\,}}\overset{\centerdot}{{\boldsymbol{q}}}\,$ and ${{\nabla}_{\boldsymbol{\lambda }}}\boldsymbol{\lambda }$ are just identity matrices (I_n and I_m respectively, n- and m-dimensional) and that ${{\nabla}_{\boldsymbol{\lambda }}}H\left(\overset{\centerdot}{{\boldsymbol{x}}}\,;\boldsymbol{\lambda }\right)=0$ simply returns the original constraint $\overset{\centerdot}{{\boldsymbol{x}}}\,-J\overset{\centerdot}{{\boldsymbol{q}}}\,=0$, the general system of equation (7) can be simplified and rewritten as

$$\begin{eqnarray}\left\{\begin{array}{*{35}{l}} {{J}^{T}}\boldsymbol{\lambda } & = & W\overset{\centerdot}{{\boldsymbol{q}}}\,+{{\nabla}_{\boldsymbol{q}}}h \\ \overset{\centerdot}{{\boldsymbol{x}}}\, & = & J\overset{\centerdot}{{\boldsymbol{q}}}\, \end{array}\right. \end{eqnarray} \tag{ 8 }$$

Remark (LM as task-space force fields). It should be noted that, in equation (8), if $h\left(\boldsymbol{q}\right)$ represents a mechanical potential, for example due to gravity or elastic fields (e.g. generated by springs at each joint), then W can be interpreted as mechanical damping (e.g. friction at the joints) and the term $W\overset{\centerdot}{{\boldsymbol{q}}}\,$ can be interpreted as damping torque¹ (in the joint-space). For example in the case of a redundant manipulator, if $\overset{\centerdot}{{\boldsymbol{q}}}\,$ and ${{\nabla}_{\boldsymbol{q}}}h$ can be interpreted as joint torques, so is the term ${{J}^{T}}\boldsymbol{\lambda }$ in the first of (8). The Jacobian matrix J not only maps joint velocities into task velocities, as in (3), but it also maps (via its transpose J^T) task-space forces into joint-space torques and, therefore, the vector of Lagrange multipliers $\boldsymbol{\lambda }$ is nothing but a posture-dependent force field in task-space.

In traditional 'velocity-resolution control', the task velocity $\overset{\centerdot}{{\boldsymbol{x}}}\,$ is supposed to be known and this allows to uniquely determine the augmented state as function of the task velocity ($\overset{\centerdot}{{\boldsymbol{x}}}\,$) and joint torques ${{\nabla}_{\boldsymbol{q}}}h$: from (8), since W is invertible, we can rewrite the first as $\overset{\centerdot}{{\boldsymbol{q}}}\,={{W}^{-1}}{{J}^{T}}\boldsymbol{\lambda }-{{W}^{-1}}{{\nabla}_{\boldsymbol{q}}}h$ which substituted in the second gives $\overset{\centerdot}{{\boldsymbol{x}}}\,=J{{W}^{-1}}{{J}^{T}}\boldsymbol{\lambda }-J{{W}^{-1}}{{\nabla}_{\boldsymbol{q}}}h$, which allows to determine $\boldsymbol{\lambda }$. Then, from the first of (8) one derives $\overset{\centerdot}{{\boldsymbol{q}}}\,$ as well:

$$\begin{eqnarray}\begin{array}{*{35}{l}} \overset{\centerdot}{{\boldsymbol{q}}}\, & = & G\overset{\centerdot}{{\boldsymbol{x}}}\,+\left({{I}_{n}}-GJ\right){{W}^{-1}}{{\nabla}_{\boldsymbol{q}}}h \end{array}\end{eqnarray} \tag{ 9 }$$

$$\begin{eqnarray}\begin{array}{*{35}{l}} \boldsymbol{\lambda } & = & B\overset{\centerdot}{{\boldsymbol{x}}}\,+BJ{{W}^{-1}}{{\nabla}_{\boldsymbol{q}}}h \end{array}\end{eqnarray} \tag{ 10 }$$

where G ($n\times m$ matrix) is one of the possible weighted generalized inverses [5] satisfying JG  =  I_m, defined as:

$$\begin{eqnarray}&&G:={{W}^{-1}}{{J}^{T}}{{\left(J{{W}^{-1}}{{J}^{T}}\right)}^{-1}}={{W}^{-1}}{{J}^{T}}B\end{eqnarray} \tag{ 11 }$$

and:

$$\begin{eqnarray}&&B:={{\left(J{{W}^{-1}}{{J}^{T}}\right)}^{-1}}\end{eqnarray} \tag{ 12 }$$

represents the Cartesian damping as perceived in the task-space, whenever W is interpreted as joint damping, for example, for a mechanical manipulator. While equation (9) comprises many forms of velocity-resolution control, as highlighted by English and Maciejewski [20], to the authors' knowledge, (10) has never been explicitly provided (albeit surely considered by other authors to derive (9)). The reason is that the vector of LM ($\boldsymbol{\lambda }$) is usually considered as a dummy variable, while here we are going to stress its physical representation, as shown below.

Inspired by the separation principle, the Lagrange multiplier $\boldsymbol{\lambda }$ in (10) can now be rewritten as a composition of dynamic ($\boldsymbol{\lambda }_{{\text{dyn}}}$) and static ($\boldsymbol{\lambda }_{{0}}$) force-fields:

$$\begin{eqnarray}&&\boldsymbol{\lambda }=\underset{\boldsymbol{\lambda }_{{\text{dyn}}}}{{\underbrace{{B\overset{\centerdot}{{\boldsymbol{x}}}\,}}\,}}\,+\underset{\boldsymbol{\lambda }_{{0}}}{{\underbrace{{BJ{{W}^{-1}}{{\nabla}_{\boldsymbol{q}}}h}}\,}}\,\end{eqnarray} \tag{ 13 }$$

The mechanical interpretation is straightforward, whenever W is regarded as joint damping of a manipulator:

• Posture: the static term $\boldsymbol{\lambda }_{{0}}$ represents the (posture-dependent) task-space force necessary to block any effect in the task-space due joint torques ${{\nabla}_{\boldsymbol{q}}}h$ while leaving the same joint torques free to act in the null-space. Leading to constrained-optimal postures.
• Movement: the dynamic term $\boldsymbol{\lambda }_{{\text{dyn}}}$ is the task-space force required to impose a velocity $\overset{\centerdot}{{\boldsymbol{x}}}\,$ onto the end-effector and overcome the Cartesian damping B.

The postural effects of $\boldsymbol{\lambda }_{{0}}$ can be seen during static task conditions, i.e. $\overset{\centerdot}{{\boldsymbol{x}}}\,=0$, when the Lagrange multiplier reduces to its static component:

$$\begin{eqnarray}&&\boldsymbol{\lambda }_{{0}}=BJ{{W}^{-1}}{{\nabla}_{\boldsymbol{q}}}h\end{eqnarray} \tag{ 14 }$$

or alternatively, from (10), one obtains that $\boldsymbol{\lambda }=\boldsymbol{\lambda }_{{0}}$ whenever $\overset{\centerdot}{{\boldsymbol{x}}}\,=0$, i.e. $\boldsymbol{\lambda }_{{0}}$ is the value of task-space force $\boldsymbol{\lambda }$ to be applied to maintain the end-effector stationary ($\overset{\centerdot}{{\boldsymbol{x}}}\,=0$). It should be noted that in a redundant manipulator one can have $\overset{\centerdot}{{\boldsymbol{x}}}\,=0$ even if $\overset{\centerdot}{{\boldsymbol{q}}}\,\ne 0$. In other words, $\boldsymbol{\lambda }_{{0}}$ blocks the effects of the torque ${{\nabla}_{\boldsymbol{q}}}h$ on the task-space but leaves it unconstrained in the null-space allowing the manipulator to move towards a task-constrained minimum of the potential. This means that, once a potential function $h\left(\boldsymbol{q}\right)$ is defined, Donders' law can be conveniently captured by a static task-space force field $\boldsymbol{\lambda }_{{0}}$.

This perspective offers several advantages. The first advantage is that, the task-space has a lower dimensionality than the joint-space, in particular forces which live in the task-space (as opposed to torques in joint-space) naturally leave the null-space unconstrained. The second advantage is that, viewing postural control as a (task-space) force field leads to generalization of velocity-resolution control framework, as one can then 'softly' induce a Donders' law without freezing the manipulator since additional dynamic forces $\boldsymbol{\lambda }_{{\text{dyn}}}$ can be superimposed and motion can be generated, either in accordance with the Donders' constraint or away from it. This latter feature actually leads to the main advantage in terms of bioinspired control, i.e. the flexibility to comply with or to violate Donders' law. Admiraal et al [1] in fact showed that, for arm movements, neither postural models or transport models alone can explain experimental data and the authors concluded that ' a strict distinction between posture-based models and trajectory-based models is an oversimplification' [1]. In our case, if the dynamic force field is exactly $\boldsymbol{\lambda }_{{\text{dyn}}}=B\overset{\centerdot}{{\boldsymbol{x}}}\,$, then one obtains exactly equation (10). Next section will discuss possible generalizations of the model in terms of motion planning alternatives.

It should be noted that the system (8) and the equations (9)–(10) are equivalent (as the latter are solutions of the former). The system (8) can be captured and, at the same time, more easily generalized by the block diagram in figure 2 where: the task-space force $\boldsymbol{\lambda }$ is now explicitly separated into $\boldsymbol{\lambda }_{{\text{dyn}}}+\boldsymbol{\lambda }_{{0}}$. In turn, $\boldsymbol{\lambda }_{{\text{dyn}}}$ can now be generalized, by the block $\boldsymbol{\Pi}\left(\boldsymbol{x}_{{d}},\boldsymbol{x}\right)$, to implement different types of task-space motion planners, as detailed in table 1.

Zoom In Zoom Out Reset image size

Table 1. Possible implementations of the control schema in figure 2.

No.	$\boldsymbol{\lambda }_{{\text{dyn}}}=\boldsymbol{\Pi}\left(\boldsymbol{x}_{{d}},\boldsymbol{x}\right)$	$\boldsymbol{\lambda }_{{0}}$	Notes
(1)	$B{{\overset{\centerdot}{{\boldsymbol{x}}}\,}_{d}}$	equation (14)	Open-loop velocity resolution control [20] as in equation (13)
(2)	$B{{\overset{\centerdot}{{\boldsymbol{x}}}\,}_{d}}+K\left(\boldsymbol{x}_{{d}}-\boldsymbol{x}\right)$	equation (14)	Closed-loop velocity resolution control [13, 40]
(3)	$K\left(\boldsymbol{x}_{{d}}-\boldsymbol{x}\right)$	$\mathbf{0}$	PMP [36, 37]
(4)	$K\left(\boldsymbol{x}_{{d}}-\boldsymbol{x}\right)$	equation (14)	$\boldsymbol{\lambda }_{{0}}$-PMP (extension of PMP proposed in this work)

Velocity resolution control in the form of (9) is an open-loop control policy. Given a desired task-space trajectory $\boldsymbol{x}_{{d}}(t)$, one can always derive the desired task-space velocity ($\overset{\centerdot}{{\boldsymbol{x}}}\,={{\overset{\centerdot}{{\boldsymbol{x}}}\,}_{d}}$) and determine via (9) the joint-space velocities $\overset{\centerdot}{{\boldsymbol{q}}}\,$ to be commanded at each joint, see the controller $\boldsymbol{\lambda }_{{\text{dyn}}}=B\overset{\centerdot}{{\boldsymbol{x}_{{\boldsymbol{d}}}}}\,$ in table 1-1. In practical applications, open-loop schemes can easily result into instability and/or unpredictable robot behaviour in presence of errors in the robot Jacobian and/or mechanical perturbations arising from the environment. Therefore, in common practice [8, 13], the task-space motion planner is usually augmented with a feedback control term $B{{\overset{\centerdot}{{\boldsymbol{x}}}\,}_{d}}+K\left(\boldsymbol{x}_{{d}}-\boldsymbol{x}\right)$ (see table 1-2), to correct for deviations from the desired end-effector trajectory $\boldsymbol{x}_{{d}}(t)$.

It is worth noting that the original PMP models [36, 37] do not include postural control terms such as $\boldsymbol{\lambda }_{{0}}$ and, without such term, the presence of a potential $h\left(\boldsymbol{q}\right)$ would interfere with the task itself (see appendix). For example, consider a manipulator with 'intrinsic' springs at each joint (leading to an elastic potential $h\left(\boldsymbol{q}\right)$) whose end-effector (located at $\boldsymbol{x}$ in task-space) is pulled towards a target $\boldsymbol{x}_{{d}}$ by an 'extrinsic' spring K, leading to the control law $K\left(\boldsymbol{x}_{{d}}-\boldsymbol{x}\right)$, as in table 1. Due to a competition between intrinsic and extrinsic springs, the equilibrium will, in general, not coincide with $\boldsymbol{x}_{{d}}$, i.e. will not be able to comply with the desired task (see appendix for details). Our framework in figure 2 when the task-space motion controller is as given in table 1-4, i.e. $\boldsymbol{\lambda }_{{\text{dyn}}}=K\left(\boldsymbol{x}_{{d}}-\boldsymbol{x}\right)$ and $\boldsymbol{\lambda }_{{0}}$ as in equation (14), can be seen as a novel extension of the original PMP (represented by the dashed box in figure 2) and will be named throughout this work as $\boldsymbol{\lambda }_{{0}}$-PMP.

The wrist orientation in the endpoint space is described via a $3\times 3$ rotation matrix R belonging to Special Orthogonal Group SO(3). The columns of such matrix define the orientation of the axes of a moving frame attached to wrist $\left\{\boldsymbol{e}_{{1}};\boldsymbol{e}_{{2}};\boldsymbol{e}_{{3}}\right\}$ with respect to a fixed frame $\left\{\boldsymbol{e}_{{x}};\boldsymbol{e}_{{y}};\boldsymbol{e}_{{z}}\right\}$. An equivalent but more compact way to describe the wrist orientation is by means of a 3D rotation vector $\boldsymbol{r}=\phi \,\boldsymbol{n}$ which is defined as a rotation by an amount ϕ about the axis $\boldsymbol{n}$ ($|\boldsymbol{n}|\,=1$). It is possible to convert a rotation matrix R into the corresponding rotation vector via the matrix logarithm:

$$\begin{eqnarray}&&\boldsymbol{r}={{\log}_{\vee}}(R)=\frac{\phi}{2\sin \phi}\left[\begin{array}{*{35}{l}} {{R}_{3,2}}-{{R}_{2,3}} \\ {{R}_{1,3}}-{{R}_{3,1}} \\ {{R}_{2,1}}-{{R}_{1,2}} \end{array}\right]\end{eqnarray} \tag{ 15 }$$

Conversely, given a rotation vector $\boldsymbol{r}$, the corresponding rotation matrix R is computed via the Rodrigues'formula [18]:

$$\begin{eqnarray}&&R=\exp \left(\widehat{\boldsymbol{r}}\right)=I+\sin |\boldsymbol{r}|\frac{\widehat{\boldsymbol{r}}}{|\boldsymbol{r}|}+\left(1-\cos |\boldsymbol{r}|\right)\frac{{{\widehat{\boldsymbol{r}}}^{2}}}{|\boldsymbol{r}{{|}^{2}}},\end{eqnarray} \tag{ 16 }$$

where $\widehat{\boldsymbol{r}}$ is the skew-symmetric matrix representation of the rotation vector $\boldsymbol{r}$.

While the proposed computational framework only needs forward mappings, the experimental pointing task with humans involves motion sensors that measure the 3D orientation of the wrist (in terms of rotation matrices) [11]; therefore an inverse mapping is necessary to convert rotation vectors into joint angles (see figure 4):

$$\begin{eqnarray}&&\boldsymbol{q}(R)=\left[\begin{array}{*{35}{l}} \text{atan}2\left({{R}_{1,3}},{{R}_{1,1}}\right) \\ \arcsin \left(-{{R}_{1,2}}\right) \\ \text{atan}2\left(-{{R}_{3,2}},{{R}_{2,2}}\right) \end{array}\right].\end{eqnarray} \tag{ 17 }$$

Although, in general the solution to equation (17) is not unique, the biomechanical range of motions of the wrist (see [11]) yields a unique joint angle configuration for any reachable orientation. We make use of this inverse mapping in section 2.3 for the nonlinear inverse optimization algorithm.

For any wrist posture $\boldsymbol{q}:={{\left[{{q}^{\text{PS}}}~{{q}^{\text{FE}}}~{{q}^{\text{RUD}}}\right]}^{T}}$ in joint-space, the orientation R of a moving frame attached to wrist with respect to a fixed frame is computed as the ordered product of three rotations:

$$\begin{eqnarray}&&R\left(\boldsymbol{q}\right)=\exp \left(-{{\widehat{\boldsymbol{e}}}_{\boldsymbol{x}}}\,{{q}^{\text{PS}}}\right)\exp \left({{\widehat{\boldsymbol{e}}}_{\boldsymbol{z}}}\,{{q}^{\text{FE}}}\right)\exp \left({{\widehat{\boldsymbol{e}}}_{\boldsymbol{y}}}\,{{q}^{\text{RUD}}}\right)\end{eqnarray} \tag{ 18 }$$

and the corresponding rotation vector can be computed with equation (15).

We consider a virtual hand-held laser aligned with the axis $\boldsymbol{e}_{{1}}$ of the moving frame. The pointing task involves targets presented on a vertical ${{x}_{1}}{{x}_{2}}$ planar screen at unit distance from the wrist along $\boldsymbol{e}_{{x}}$ and with the y-axis of the screen directed as $-\boldsymbol{e}_{{y}}$. Then, for every pointing direction $\boldsymbol{n}=R\,\boldsymbol{e}_{{x}}$ ($|\boldsymbol{n}|=1$) the laser will hit the screen at coordinates:

$$\begin{eqnarray}&&\boldsymbol{x}=\left[\begin{array}{*{35}{l}} {{x}_{1}} \\ {{x}_{2}} \end{array}\right]=FK\left(\boldsymbol{q}\right):=\left[\begin{array}{*{35}{l}} 0 & -1 & 0 \\ 0 & 0 & 1 \end{array}\right]R\left(\boldsymbol{q}\right)\,\left[\begin{array}{*{35}{l}} 1 \\ 0 \\ 0 \end{array}\right],\end{eqnarray} \tag{ 19 }$$

This equation defines the forward kinematics for the pointing task (see figure 4) with the task Jacobian defined as equation (3).

The typical way Donders' law is assessed and quantified is via numerical fitting of experimental data. The motor space for eye, head and wrist (functionally including the forearm DOF) is usually considered as a 3DOF, purely rotational space. Eye/head/wrist postures correspond then to 3D rotations and are usually encoded as points of a 3D space, traditionally expressed in different coordinates such as quaternions, rotation vectors, or Euler angles. Tasks such as gazing/pointing at targets on a screen are two-dimensional but involve three DOF movements, therefore leading to a redundant problem. Existence of a DL is typically demonstrated by showing that a cloud of points (each encoding a 3D posture of eye, head or wrist while gazing/pointing at a set of targets on a screen) can be fitted to a (Donders') surface, in the 3D space of postures. Similar procedures can be applied to the human arm which, however, requires more care in specifying the task, as the arm could be fully extended [27, 32] or with a flexed elbow [19].

It has been shown [10, 11] that pointing with the wrist (at comfortable speed) implies a specific relationship between task-space targets and wrist orientation. In particular, for any desired pointing target the wrist assumes a unique orientation. The set of all wrist orientations measured during a pointing task, can be well approximated with a quadratic Donders' surface:

$$\begin{eqnarray}&&{{r}_{x}}=\mathcal{D}\left({{r}_{y}},{{r}_{z}}\right)={{C}_{1}}+{{C}_{2}}{{r}_{y}}+{{C}_{3}}{{r}_{z}}+{{C}_{4}}r_{y}^{2}\\ &&\quad \quad +2{{C}_{5}}{{r}_{y}}{{r}_{z}}+{{C}_{6}}r_{z}^{2}\end{eqnarray} \tag{ 20 }$$

where r_x,r_y,r_z are the components of the rotation vector representing a particular wrist orientation. The existence of such a bi-dimensional surface embedded into the 3D configuration space also implies that the three wrist DOFs are not controlled independently but instead they are coupled according to a specific kinematic synergy (or Donders'surface). In [11] it has been shown that Donders-like kinematic synergies can be predicted by solving a static constrained optimization problem (postural model) involving a discomfort function which here is defined as:

$$\begin{eqnarray}&&h\left(\boldsymbol{q};\boldsymbol{\theta }\right)=\frac{1}{2}{{\left(\boldsymbol{q}-\boldsymbol{q}_{{0}}\right)}^{T}}{{K}^{J}}\left(\boldsymbol{q}-\boldsymbol{q}_{{0}}\right)\end{eqnarray} \tag{ 21 }$$

where $\boldsymbol{q}_{{0}}=\left[q_{0}^{\text{PS}}~q_{0}^{\text{FE}}~q_{0}^{\text{RUD}}\right]$ is the most comfortable equilibrium posture $\boldsymbol{q}_{{0}}$ and K^J is a $3\times 3$ symmetric joint stiffness matrix which penalizes postures away from the comfortable equilibrium $\boldsymbol{q}_{{0}}$. We choose this type of cost model as it is simple, computational effective from an optimization perspective and it is also widely used in robotics [38] and human motor control [15]. The 9-dimensional parameter vector $\boldsymbol{\theta }=\left[K_{11}^{J},~K_{12}^{J},~\ldots,~K_{33}^{J},~{{q}_{0}}^{\text{PS}},~{{q}_{0}}^{\text{FE}},~{{q}_{0}}^{\text{RUD}}\right]$ (note that the $3\times 3$ stiffness matrix K^J is symmetric, therefore it only requires 6 parameters) has all the parameters of the problem which allow capturing the DL strategies, as in [11]. Note that we are not including W in the parameters, as this would rather affect the task trajectories but not the equilibrium postures. A study of the role of W in the predicted behaviours is therefore beyond the scope of this work and will be addressed in future works.

The postural model proposed in [11] can only predict static wrist configurations (postures) and therefore cannot account for motion. Also, in [11] it was not addressed on how to choose the parameter $\boldsymbol{\theta }$ in order to reproduce a specific human synergy. Therefore, the goal of the next sections is to integrate this postural component according to the task-space separation principle in order to generate both motions and equilibrium (static) postures. An inverse optimization algorithm is then presented to estimate the parameter vector $\boldsymbol{\theta }$ from human data.

NIO requires a human pointing strategy (Q^H, X^H) from experimental data. With reference to figure 5. (1) it is possible to notice that the experimental recording is made of thousands of samples (N) that are also affected by the instrumentation noise. The higher the number of samples, the higher the computational burden needed to run the NIO as, for each human sample a corresponding sample must be generated by the model. Therefore, a reliable method must be implemented in order to filter first and then down-sample human raw data. Figure 5. (2) shows the Donders'surface that best fits the rotation vectors of a representative subject. Apart from being a kinematic pointing synergy, this surface can also be thought as a filter applied to the experimental rotation vectors. Furthermore, it can be used to generalize the subject pointing strategy to pointing directions that have not been measured experimentally. Based on these considerations we propose the following method to down-sample the human pointing strategy: given a subject-specific Donders'surface (equation (20)), we generate a small set of N₀ rotation vectors ${{\rm{\tilde \Gamma }}^{H}}=\left[\begin{array}{*{35}{l}} \boldsymbol{r}_{1}^{H} & \boldsymbol{r}_{2}^{H} & \ldots & \boldsymbol{r}_{{{N}_{0}}}^{H} \end{array}\right]$ by sampling the Donders's surface at N₀ different locations as shown in figure 5. (3); then, the down-sampled rotation vectors are mapped into down-sampled joint configurations, providing a set ${{\tilde{Q}}^{H}}=\left[\begin{array}{*{35}{l}} \boldsymbol{q}_{1}^{H} & \boldsymbol{q}_{2}^{H} & \ldots & \boldsymbol{q}_{{{N}_{0}}}^{H} \end{array}\right]$ of joint angles. This set of joint angles is needed to compute the error function in equation (22) and to update the parameter vector (equations (24) and (25)). The set of N₀ joint angles is mapped into N₀ desired task-space constraints ${{\tilde{X}}^{H}}=\left[\begin{array}{*{35}{l}} \boldsymbol{x}_{1}^{H} & \boldsymbol{x}_{2}^{H} & \ldots & \boldsymbol{x}_{{{N}_{0}}}^{H} \end{array}\right]$ and are used as desired targets to generate motions with the $\boldsymbol{\lambda }_{{0}}$-PMP model (see section 2.3). We decided to use a set of N₀  =  9 points consisting of a central point plus 8 peripheral points (note that the theoretical minimum would be 6 points, in order to capture Quadratic surfaces which can be described by 6 coefficients).

Zoom In Zoom Out Reset image size

The NIO algorithm described in this section is shown as a flowchart in figure 6. The input to the algorithm are the down-sampled human pointing strategy (${{\tilde{Q}}^{H}}$, ${{\tilde{X}}^{H}}$) and an initial guess for the parameter vector $\widehat{\boldsymbol{\theta }}=\boldsymbol{\theta }_{{0}}$ (see next section).

• Step 1.  
  The $\boldsymbol{\lambda }_{{0}}$-PMP model is simulated with joint potential $h\left(\boldsymbol{q},\widehat{\boldsymbol{\theta }}\right)$ defined, initially, by current estimate of the parameter vector $\widehat{\boldsymbol{\theta }}$ and with the desired pointing targets ${{\tilde{X}}^{H}}$ coming from the human down-sampled data. For each desired pointing target, the equilibrium wrist configurations (at steady-state) generated by the $\boldsymbol{\lambda }_{{0}}$-PMP model are collected: ${{\tilde{Q}}^{M}}=\left[\begin{array}{*{35}{l}} \boldsymbol{q}_{1}^{H} & \boldsymbol{q}_{2}^{H} & \ldots & \boldsymbol{q}_{{{N}_{0}}}^{H} \end{array}\right]$.
• Step 2.  
  Given the human pointing strategy ${{\tilde{Q}}^{H}}$, the model pointing strategy Q^M and the actual estimation of the parameter vector $\widehat{\boldsymbol{\theta }}$, a gradient direction is approximated by evaluating ${{\nabla}_{\boldsymbol{\theta }}}e\left(\boldsymbol{\theta }\right)$ (equation (24)).
• Step 3.  
  The parameter vector is updated as:

  $$\begin{eqnarray}&&\widehat{\boldsymbol{\theta }}\leftarrow \widehat{\boldsymbol{\theta }}-\beta {{\nabla}_{\boldsymbol{\theta }}}~e\left(\boldsymbol{\theta }\right)\end{eqnarray} \tag{ 25 }$$

  where the scalar β is computed with a line search method (see [34] for more details). The algorithm is then repeated from step 1 until $e\left(\boldsymbol{\theta }\right)$ becomes less than a specified threshold .

Zoom In Zoom Out Reset image size