A human-like learning control for digital human models in a physics-based virtual environment

Abstract

This paper presents a new learning control framework for digital human models in a physics-based virtual environment. The novelty of our controller is that it combines multi-objective control based on human properties (combined feedforward and feedback controller) with a learning technique based on human learning properties (human-being’s ability to learn novel task dynamics through the minimization of instability, error and effort). This controller performs multiple tasks simultaneously (balance, non-sliding contacts, manipulation) in real time and adapts feedforward force as well as impedance to counter environmental disturbances. It is very useful to deal with unstable manipulations, such as tool-use tasks, and to compensate for perturbations. An interesting property of our controller is that it is implemented in Cartesian space with joint stiffness, damping and torque learning in a multi-objective control framework. The relevance of the proposed control method to model human motor adaptation has been demonstrated by various simulations.

Appendices

Appendix A: Relation between Cartesian space and joint space

Using Eqs. 1, the interaction dynamics is:

$$\begin{aligned} M \dot{T} \!+\! N T \!+\! G \!=\! L \tau \! +\! J_{c}^\mathrm{T} W_{c} \!+\! J_\mathrm{end}^\mathrm{T} W^{i}_\mathrm{end} \!=\! J^\mathrm{T} W^{i}_\mathrm{end} \end{aligned}$$

(33)

Given an interaction wrench $W^{i}_\mathrm{end}$.

In this paper, we treat the DHM control where the floating base is the foot. We consider cases with the foot fixed to the ground. In this way, we obtain a completely actuated DHM with fixed-base robots characteristics. The dynamic model of DHM is:

$$\begin{aligned} M_{q} \ddot{q} + N_{q} \dot{q} + G_q = \tau + J_{c,q}^\mathrm{T} W_{c} + J_{\mathrm{end},q}^\mathrm{T} W^{i}_\mathrm{end} \end{aligned}$$

(34)

with $M_{q} = L^\mathrm{T} M L$, $N_{q} = L^\mathrm{T} N L$, $G_{q} = L^\mathrm{T} G$, $J_{c,q}^\mathrm{T} = L^\mathrm{T} J_{c}^\mathrm{T}$ and $J_{\mathrm{end},q}^\mathrm{T} = L^\mathrm{T} J_\mathrm{end}^\mathrm{T}$. When the only contact with the ground is the foot and it is the root, we obtain $J_{c} L = 0$.

Since $\rho = Sq$ and $S$ is a matrix to select a part of the actuated degrees of freedom ($S=[I\,0]$) to obtain a dynamic model independent of non-sliding contact forces at known fixed locations in Eq. 1 such as the contacts between the feet and the ground, we can write the system as:

$$\begin{aligned} M_{\rho } \ddot{\rho } + N_{\rho } \dot{\rho } + G_{\rho } = \tau _{\rho } + J_{\mathrm{end},\rho }^\mathrm{T} W^{i}_\mathrm{end} \end{aligned}$$

(35)

with $M_{\rho } = S M_{q} S^\mathrm{T}$, $N_{\rho } = S N_{q} S^\mathrm{T}$, $G_{\rho } = S G_{q}$ and $J_{\mathrm{end},\rho }^\mathrm{T} = S J_{\mathrm{end},q}^\mathrm{T}$.

From Eq. 35 and since $\delta W^{i}_\mathrm{end} = K_\mathrm{end} \hbox {vec}(H^{-1}_\mathrm{end}\delta H_\mathrm{end}) = K_\mathrm{end} J_{\mathrm{end},q} \delta q = K_\mathrm{end} J_{\mathrm{end},q} \delta (S^t \rho ) = K_\mathrm{end} J_{\mathrm{end},\rho } \delta \rho$, we obtain:

$$\begin{aligned}&\delta \tau _{\rho } + \delta (J_{\mathrm{end},\rho }^\mathrm{T} W^{i}_\mathrm{end})\nonumber \\&= \delta \tau _{\rho } + (\delta J_{\mathrm{end},\rho }^\mathrm{T}) W^{i}_\mathrm{end} + J_{\mathrm{end},\rho }^\mathrm{T} \delta W^{i}_\mathrm{end} \nonumber \\&= \delta \tau _{\rho } + (\delta J_{\mathrm{end},\rho }^\mathrm{T}) W^{i}_\mathrm{end} + J_{\mathrm{end},\rho }^\mathrm{T} K_\mathrm{end} J_{\mathrm{end},\rho } \delta \rho = 0 \end{aligned}$$

(36)

Since $\delta \tau _{\rho } = - K_{\rho } \delta \rho$ and Eq. 36, we obtain:

$$\begin{aligned} K_{\rho } = -\frac{\delta \tau _{\rho }}{\delta \rho } = J_{\mathrm{end},\rho }^\mathrm{T} K_\mathrm{end} J_{\mathrm{end},\rho } + \frac{\partial J_{\mathrm{end},\rho }^\mathrm{T}}{\partial \rho } W^{i}_\mathrm{end} \end{aligned}$$

(37)

Finally, the Cartesian impedance is:

$$\begin{aligned} K_\mathrm{end} = J_{\mathrm{end},\rho }^{\dag T} \left( K_{\rho } - \frac{\partial J_{\mathrm{end},\rho }^\mathrm{T}}{\partial \rho } W^{i}_\mathrm{end}\right) J_{\mathrm{end},\rho }^{\dag } \end{aligned}$$

(38)

with $J^{\dag }_{\mathrm{end},\rho }$ the dynamic pseudo-inverse [38] defined as:

$$\begin{aligned} J^{\dag }_{\mathrm{end},\rho } = M_{\rho }^{-1} J_{\mathrm{end},\rho }^\mathrm{T} (J_{\mathrm{end},\rho } M_{\rho }^{-1} J_{\mathrm{end},\rho }^\mathrm{T})^{-1} \end{aligned}$$

(39)

It can be similarly obtained $B_\mathrm{end} = J_\mathrm{end,\rho }^{\dag T} B_{\rho } J_{\mathrm{end},\rho }^{\dag }$.

Appendix B: Convergence analysis

1.1 B.1 Motion error cost function

The first derivative of $M_\mathrm{E}$ (Eq. 5) can be calculated as follows:

$$\begin{aligned} \dot{M}_\mathrm{E}&= \frac{1}{2}\frac{\hbox {d}}{\hbox {d}t}[\epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag })\epsilon ]\nonumber \\&= \frac{1}{2}[\dot{\epsilon }^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag })\epsilon \nonumber \\&+ \epsilon ^\mathrm{T}(\dot{J}_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag } + J_{\mathrm{end},\rho }^{\dag T} \dot{M_{\rho }} J_{\mathrm{end},\rho }^{\dag } \nonumber \\&+ J_{\mathrm{end},\rho }^{\dag T} M_{\rho } \dot{J}_{\mathrm{end},\rho }^{\dag })\epsilon + \epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag })\dot{\epsilon }] \end{aligned}$$

(40)

with

$$\begin{aligned} \epsilon = V - V^{*}, \quad \dot{\epsilon } = A - A^{*} \end{aligned}$$

(41)

and $V^{*}=V^{d} - b\delta (H^{d},H^\mathrm{r})$. $V^{d}$ is the velocity obtained by minimum jerk planner. $A^{*}$ is the derivative of $V^{*}$.

Matrix $M_{\rho }$ is symmetric, we therefore obtain:

$$\begin{aligned} \dot{M}_\mathrm{E}&= [\epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag })\dot{\epsilon }]+\frac{1}{2}[\epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} \dot{M_{\rho }} J_{\mathrm{end},\rho }^{\dag })\epsilon ] \nonumber \\&+ [\epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } \dot{J}_{\mathrm{end},\rho }^{\dag })\epsilon ] \end{aligned}$$

(42)

The relationship between $\rho$ velocity and Cartesian space velocity can be expressed as:

$$\begin{aligned} V = J_{\mathrm{end},\rho } \dot{\rho } \Rightarrow \dot{\rho } = J_{\mathrm{end},\rho }^{\dag } V \end{aligned}$$

(43)

Differentiating Eq. 43, the Cartesian acceleration term can be found as:

$$\begin{aligned} A = J_{\mathrm{end},\rho }\ddot{\rho }+\dot{J}_{\mathrm{end},\rho }\dot{\rho } \end{aligned}$$

(44)

then the equation of robot motion in joint space can also be represented in Cartesian space coordinates by the relationship:

$$\begin{aligned} \ddot{\rho } = J_{\mathrm{end},\rho }^{\dag }(A-\dot{J}_{\mathrm{end},\rho }\dot{\rho }) = J_{\mathrm{end},\rho }^{\dag }(A-\dot{J}_{\mathrm{end},\rho }J_{\mathrm{end},\rho }^{\dag } V)\nonumber \\ \end{aligned}$$

(45)

Substituting Eqs. 45 and 43 into Eq. 35 yields:

$$\begin{aligned}&M_{\rho } J_{\mathrm{end},\rho }^{\dag }[A - \dot{J}_{\mathrm{end},\rho }J_{\mathrm{end},\rho }^{\dag } V] + N_{\rho } J_{\mathrm{end},\rho }^{\dag }V + G_{\rho } \nonumber \\&\quad = \tau _{\rho } + J_{\mathrm{end},\rho }^\mathrm{T} W^{i}_\mathrm{end} \end{aligned}$$

(46)

Multiplying both side by $J_{\mathrm{end},\rho }^{\dag T}$, we obtain:

$$\begin{aligned}&(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag })A\nonumber \\&\quad =[-J_{\mathrm{end},\rho }^{\dag T} N_{\rho } J_{\mathrm{end},\rho }^{\dag }+ J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag }\dot{J}_{\mathrm{end},\rho } J_{\mathrm{end},\rho }^{\dag }]V \nonumber \\&\qquad - J_{\mathrm{end},\rho }^{\dag T} G_{\rho } + J_{\mathrm{end},\rho }^{\dag T} \tau _{\rho } + W^{i}_\mathrm{end} \end{aligned}$$

(47)

Using Eq. 10, we obtain:

$$\begin{aligned}&(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag })A\nonumber \\&\quad = [-J_{\mathrm{end},\rho }^{\dag T} N_{\rho } J_{\mathrm{end},\rho }^{\dag } + J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag }\dot{J}_{\mathrm{end},\rho } J_{\mathrm{end},\rho }^{\dag }]V \nonumber \\&\qquad - J_{\mathrm{end},\rho }^{\dag T} G_{\rho }+ J_{\mathrm{end},\rho }^{\dag T} \tau ^\mathrm{f\!f}_{\rho } - W^{d}_\mathrm{end}- J_{\mathrm{end},\rho }^{\dag T} \tau ^{l}_{\rho } + W^{i}_\mathrm{end}\nonumber \\ \end{aligned}$$

(48)

where $\tau _{\rho }^\mathrm{f\!f}$ is the torque to compensate for DHM dynamics. By definition, it can be written as:

$$\begin{aligned}&\tau _{\rho }^\mathrm{f\!f} \equiv M_{\rho } \ddot{\rho }^* + N_{\rho } \dot{\rho }^* + G_{\rho }\nonumber \\&\quad \equiv M_{\rho } J_{end,\rho }^{\dag } A^* + [N_{\rho } J_{\mathrm{end},\rho }^{\dag }- M_{\rho } J_{\mathrm{end},\rho }^{\dag } \dot{J}_{\mathrm{end},\rho } J_{\mathrm{end},\rho }^{\dag }]V^* + G_{\rho }\nonumber \\ \end{aligned}$$

(49)

Using Eq. 41 and substituting Eq. 49 into Eq. 48 yields:

$$\begin{aligned}&(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag })\dot{\epsilon }\nonumber \\&\quad = [-J_{\mathrm{end},\rho }^{\dag T} N_{\rho } J_{\mathrm{end},\rho }^{\dag }+J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag } \dot{J}_{\mathrm{end},\rho } J_{\mathrm{end},\rho }^{\dag }]\epsilon \nonumber \\&\qquad + W^{i}_\mathrm{end} - J_{\mathrm{end},\rho }^{\dag T} \tau ^{l}_{\rho } - W_\mathrm{end}^{d} \end{aligned}$$

(50)

Substituting Eq. 50 into Eq. 42 yields:

$$\begin{aligned} \dot{M}_\mathrm{E}&= \epsilon ^\mathrm{T}[(-J_{\mathrm{end},\rho }^{\dag T} N_{\rho } J_{\mathrm{end},\rho }^{\dag } + J_{\mathrm{end},\rho }^{\dag T} M_{\rho } \dot{J}_{\mathrm{end},\rho }^{\dag } \nonumber \\&+ J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag } \dot{J}_{end,\rho } J_{\mathrm{end},\rho }^{\dag })\epsilon \nonumber \\&+ W^{i}_\mathrm{end}- J_{\mathrm{end},\rho }^{\dag T} \tau ^{l}_{\rho } - W_\mathrm{end}^{d}]\nonumber \\&+\frac{1}{2}[\epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} \dot{M_{\rho }} J_{\mathrm{end},\rho }^{\dag })\epsilon ] + [\epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} M_{\rho } \dot{J}_{\mathrm{end},\rho }^{\dag })\epsilon ]\nonumber \\&= \frac{1}{2}\epsilon ^\mathrm{T}[J_{\mathrm{end},\rho }^{\dag T} (\dot{M_{\rho }}-2 N_{\rho }) J_{\mathrm{end},\rho }^{\dag }]\epsilon \nonumber \\&+ \epsilon ^\mathrm{T}[W^{i}_\mathrm{end} - J_{\mathrm{end},\rho }^{\dag T} \tau ^{l}_{\rho } - W_\mathrm{end}^{d}] + \epsilon ^\mathrm{T}[J_{\mathrm{end},\rho }^{\dag T} M_{\rho } \dot{J}_{\mathrm{end},\rho }^{\dag } \nonumber \\&+ J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag } \dot{J}_{\mathrm{end},\rho } J_{\mathrm{end},\rho }^{\dag }]\epsilon \end{aligned}$$

(51)

Matrix $\dot{M_{\rho }} - 2N$ is skew-symmetry [60] and for this reason, we have:

$$\begin{aligned} \epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} (\dot{M_{\rho }}-2 N_{\rho }) J_{\mathrm{end},\rho }^{\dag })\epsilon = 0 \end{aligned}$$

(52)

Let us now analyze the third term of Eq. 51. Using Eq. 39, since $J_{\mathrm{end},\rho }J^{\dag }_{\mathrm{end}, \rho }=I$ and $\dot{J}_{\mathrm{end},\rho }J_{\mathrm{end},\rho }^{\dag }+J_{\mathrm{end},\rho }\dot{J}^{\dag }_{\mathrm{end},\rho }=0$, we obtain:

$$\begin{aligned}&J_{\mathrm{end},\rho }^{\dag T} M_{\rho } \dot{J}_{\mathrm{end},\rho }^{\dag }\nonumber \\&= (J_{\mathrm{end},\rho } M^{-1}_{\rho } J^\mathrm{T}_{\mathrm{end},\rho })^{-1} J_{\mathrm{end},\rho } M^{-1}_{\rho } M_{\rho } \dot{J}^{\dag }_{\mathrm{end},\rho } \nonumber \\&= (J_{\mathrm{end},\rho } M^{-1}_{\rho } J^\mathrm{T}_{\mathrm{end},\rho })^{-1} J_{\mathrm{end},\rho } \dot{J}^{\dag }_{\mathrm{end},\rho }\nonumber \\&\quad \times J_{\mathrm{end},\rho }^{\dag T} M_{\rho } J_{\mathrm{end},\rho }^{\dag } \dot{J}_{\mathrm{end},\rho } J_{\mathrm{end},\rho }^{\dag }\nonumber \\&= (J_{end,\rho } M^{-1}_{\rho } J^\mathrm{T}_{end,\rho })^{-1}\nonumber \\&\quad \times J_{\mathrm{end},\rho } M^{-1}_{\rho } M_{\rho } J_{\mathrm{end},\rho }^{\dag } \dot{J}_{\mathrm{end},\rho } J^{\dag }_{\mathrm{end},\rho }\nonumber \\&= - (J_{\mathrm{end},\rho } M^{-1}_{\rho } J^\mathrm{T}_{\mathrm{end},\rho })^{-1} J_{\mathrm{end},\rho } \dot{J}^{\dag }_{\mathrm{end},\rho } \end{aligned}$$

(53)

Substituting Eqs. 52 and 53 into Eq. 51, we obtain:

$$\begin{aligned} \dot{M}_\mathrm{E} = \epsilon ^\mathrm{T}[W^{i}_\mathrm{end} - J_{\mathrm{end},\rho }^{\dag T} \tau ^{l}_{\rho } - W_\mathrm{end}^{d}] \end{aligned}$$

(54)

Using Eqs. 54, 11 and 38, we have:

$$\begin{aligned} \dot{M}_\mathrm{E}&= -\epsilon ^\mathrm{T} B^\mathrm{ini}_\mathrm{end}\epsilon -\epsilon ^\mathrm{T} K^{l}_\mathrm{end}\delta (H^{d},H^\mathrm{r}) \nonumber \\&- \,\epsilon ^\mathrm{T}B^{l}_\mathrm{end}\delta (V^{d},V^\mathrm{r}) - \epsilon ^\mathrm{T} J_{\mathrm{end},\rho }^{\dag T} \tau ^{l}_{\rho } + \epsilon ^\mathrm{T}W^{i}_\mathrm{end}\nonumber \\&= -\,\epsilon ^\mathrm{T} B^\mathrm{ini}_\mathrm{end}\epsilon \nonumber \\&-\,\epsilon ^\mathrm{T}\left[ J_{\mathrm{end},\rho }^{\dag T} \left( K_{\rho } - \frac{\partial J_{end,\rho }^\mathrm{T}}{\partial \rho } W^{i}_\mathrm{end}\right) J_{\mathrm{end},\rho }^{\dag }\right] \nonumber \\&\times \,\delta (H^{d},H^\mathrm{r})- \epsilon ^\mathrm{T}(J_{\mathrm{end},\rho }^{\dag T} B_{\rho }^{l} J_{\mathrm{end},\rho }^{\dag })\delta (V^{d},V^\mathrm{r}) \nonumber \\&- \,\epsilon ^\mathrm{T} J_{\mathrm{end},\rho }^{\dag T} \tau ^{l}_{\rho }+ \epsilon ^\mathrm{T} W^{i}_\mathrm{end} \end{aligned}$$

(55)

We can derive $\delta M_E(t) = M_E(t)-M_E(t-D)$ from Eqs. 55 and 8 as:

$$\begin{aligned} \delta M_\mathrm{E}(t)&= \int _{t-D}^{t} \{- \epsilon ^\mathrm{T}(\sigma )B^\mathrm{ini}_\mathrm{end}(\sigma )\epsilon (\sigma ) \nonumber \\&- \,\epsilon ^\mathrm{T}(\sigma )[J_{\mathrm{end},\rho }^{\dag T} \tilde{K} J_{\mathrm{end},\rho }^{\dag }](\sigma )\delta (H^{d},H^\mathrm{r})(\sigma ) \nonumber \\&- \,\epsilon ^\mathrm{T}(\sigma )[J_{\mathrm{end},\rho }^{\dag T} \tilde{B} J_{\mathrm{end},\rho }^{\dag }](\sigma )\delta (V^{d},V^\mathrm{r})(\sigma )\nonumber \\&- \,\epsilon ^\mathrm{T}(\sigma )[J_{\mathrm{end},\rho }^{\dag T} \tilde{\tau }](\sigma )\nonumber \\&- \,\epsilon ^\mathrm{T}(\sigma )[J_{\mathrm{end},\rho }^{\dag T} K^\mathrm{min}_{\rho } J_{\mathrm{end},\rho }^{\dag }](\sigma )\delta (H^{d},H^\mathrm{r})(\sigma )\nonumber \\&- \,\epsilon ^\mathrm{T}(\sigma )[J_{\mathrm{end},\rho }^{\dag T} B_{\rho }^\mathrm{min} J_{\mathrm{end},\rho }^{\dag }](\sigma )\delta (V^{d},V^\mathrm{r})(\sigma ) \nonumber \\&- \,\epsilon ^\mathrm{T} (\sigma )[J_{\mathrm{end},\rho }^{\dag T} \tau ^\mathrm{min}_{\rho }](\sigma )+ \epsilon ^\mathrm{T}(\sigma ) W^{i}_\mathrm{end}(\sigma )\}\,\hbox {d}\sigma \nonumber \\ \end{aligned}$$

(56)

Any smooth interaction force can be approximated by the linear terms of its Taylor expansion along the reference trajectory as follows:

$$\begin{aligned} W^{i}_\mathrm{end} (t)&= W^{i,0}_\mathrm{end}(t) + [J_{\mathrm{end},\rho }^{\dag T} K^{i}_{\rho } J_{\mathrm{end},\rho }^{\dag }](t) \delta (H^{d},H^\mathrm{r}) \nonumber \\&+ [J_{\mathrm{end},\rho }^{\dag T} B^{i}_{\rho } J_{\mathrm{end},\rho }^{\dag }](t)\delta (V^{d},V^\mathrm{r}) \end{aligned}$$

(57)

where $W^{i,0}_\mathrm{end}$ is the zero order term compensated by $J_{\mathrm{end},\rho }^{\dag T} \tau ^\mathrm{min}$; $[J_\mathrm{end}^{\dag T} K^{i}_{\rho } J_\mathrm{end}^{\dag }]$ and $[J_\mathrm{end}^{\dag T} B^{i}_{\rho } J_\mathrm{end}^{\dag }]$ are the first-order coefficients. From Eqs. 57 and 41, we can obtain the values for $K^\mathrm{min}_{\rho }(t)$, $B^\mathrm{min}_{\rho }(t)$ and $\tau ^\mathrm{min}_{\rho }(t)$ to guarantee stability (Eq. 58). Different $W^{i}_\mathrm{end}$ will yield different values of $K^\mathrm{min}_{\rho }(t)$, $B^\mathrm{min}_{\rho }(t)$ and $\tau ^\mathrm{min}_{\rho }(t)$ and when $W^{i}_\mathrm{end}$ is zero or is assisting the tracking task $||\epsilon (t)|| \rightarrow 0$, $K^\mathrm{min}_{\rho }(t)$, $B^\mathrm{min}_{\rho }(t)$ and $\tau ^\mathrm{min}_{\rho }(t)$ will be $0$.

$K^\mathrm{min}_{\rho }(t)$, $D^\mathrm{min}_{\rho }(t)$ and $\tau ^\mathrm{min}_{\rho }(t)$ represent the minimal required effort of stiffness, damping and feedforward force required to guarantee

$$\begin{aligned}&\int _{t-D}^{t} \{- \epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T} K_{\rho }^\mathrm{min} J_\mathrm{end,\rho }^{\dag })(\sigma )\delta (H^{d},H^\mathrm{r})(\sigma ) \nonumber \\&\quad -\epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T} B_{\rho }^\mathrm{min} J_{\mathrm{end},\rho }^{\dag })(\sigma )\delta (V^{d},V^\mathrm{r})(\sigma )\nonumber \\&\quad -\epsilon ^\mathrm{T}(\sigma )J_{\mathrm{end},\rho }^{\dag T}\tau ^\mathrm{min}_{\rho }(\sigma ) + \epsilon ^\mathrm{T}(\sigma )W^{i}_\mathrm{end}(\sigma )\}\, \hbox {d} \sigma \le 0 \end{aligned}$$

(58)

so that from Eq. 55 we have $\int _{t-D}^{t} \dot{M_\mathrm{E}}(\sigma )\, \hbox {d} \sigma \le 0$.

From Eqs. 56 and 58, we can write:

$$\begin{aligned} \delta M_\mathrm{E}(t)&\le \int _{t-D}^{t} \{- \epsilon ^\mathrm{T}(\sigma )B^\mathrm{ini}_\mathrm{end}(\sigma )\epsilon (\sigma ) \nonumber \\&- \epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T} \tilde{K} J_{\mathrm{end},\rho }^{\dag })(\sigma )\delta (H^{d},H^\mathrm{r})(\sigma )\nonumber \\&-\epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T} \tilde{B} J_{\mathrm{end},\rho }^{\dag })(\sigma )\delta (V^{d},V^\mathrm{r})(\sigma ) \nonumber \\&- \epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T} \tilde{\tau })(\sigma ) \}\,\hbox {d}\sigma \end{aligned}$$

(59)

1.2 B.2 Metabolic cost function

The metabolic cost function is:

$$\begin{aligned} M_\mathrm{C}(t)=\frac{1}{2}\int _{t-D}^{t}\tilde{\Phi }^\mathrm{T}(\sigma )Q^{-1}\tilde{\Phi }(\sigma )\,\hbox {d}\sigma \end{aligned}$$

(60)

According to the definition of $\Phi (t)$ and $Q$, the following properties of $\hbox {vec}(\cdot )$, $\otimes$ and $\hbox {tr}(\cdot )$ operators:

$$\begin{aligned}&\hbox {vec}(\Omega YU)=(U^\mathrm{T} \otimes \Omega )\hbox {vec}(Y), \nonumber \\&\hbox {tr}(\Omega Y)=\hbox {vec}(\Omega ^\mathrm{T})^\mathrm{T}\hbox {vec}(Y), \ \ \hbox {tr}(\Omega Y)=\hbox {tr}(Y \Omega ) \end{aligned}$$

(61)

and using the symmetry of $Q_K^{-1}$, we obtain:

$$\begin{aligned}&\hbox {vec}(\tilde{K}^\mathrm{T})^\mathrm{T}(I \otimes Q_K)^{-1}\hbox {vec}(\tilde{K}^\mathrm{T})\nonumber \\&\quad =\hbox {vec}(\tilde{K}^\mathrm{T})^\mathrm{T}((Q_K^{-1})^\mathrm{T} \otimes I)\hbox {vec}(\tilde{K}^\mathrm{T})\nonumber \\&\quad =\hbox {vec}(\tilde{K}^\mathrm{T})^\mathrm{T}\hbox {vec}(\tilde{K}^\mathrm{T}Q_K^{-1})=\hbox {tr}\{\tilde{K}\tilde{K}^\mathrm{T}Q_K^{-1}\}\nonumber \\&\quad =\hbox {tr}\{\tilde{K}^\mathrm{T}Q_K^{-1}\tilde{K}\} \end{aligned}$$

(62)

In the same way, can be found the terms corresponding to $\tilde{B}$ and $\tilde{\tau }$.

For these reasons, we can define $\delta M_\mathrm{C}(t)= M_\mathrm{C}(t)-M_\mathrm{C}(t-D)$ as:

$$\begin{aligned}&\delta M_\mathrm{C}(t) = \frac{1}{2}\int _{t-D}^{t}\{\hbox {tr}\{[\tilde{K}^\mathrm{T}(\sigma )Q_K^{-1}\tilde{K}(\sigma )]\nonumber \\&\quad -[\tilde{K}^\mathrm{T}(\sigma -D)Q_K^{-1}\tilde{K}(\sigma -D)]\} + \hbox {tr}\{[\tilde{B}^\mathrm{T}(\sigma )Q_B^{-1}\tilde{B}(\sigma )]\nonumber \\&\quad -[\tilde{B}^\mathrm{T}(\sigma -D)Q_B^{-1}\tilde{B}(\sigma -D)]\}+ [\tilde{\tau }^\mathrm{T}(\sigma )Q_{\tau }^{-1}\tilde{\tau }(\sigma )]\nonumber \\&\quad - [\tilde{\tau }^\mathrm{T}(\sigma -D)Q_{\tau }^{-1}\tilde{\tau }(\sigma -D)]\}\,\hbox {d} \sigma \end{aligned}$$

(63)

From Eqs. 13, 14 and 16, we obtain:

$$\begin{aligned} \delta K&= Q_K\{J_{\mathrm{end},\rho }^{\dag } [\epsilon (t) \delta (H^{d},H^\mathrm{r})^\mathrm{T}] J_{\mathrm{end},\rho }^{\dag T} - \gamma (t) K_{\rho }^{l}(t)\}\nonumber \\ \delta B&= Q_B\{J_{\mathrm{end},\rho }^{\dag } [\epsilon (t) \delta (V^{d},V^\mathrm{r})^\mathrm{T}] J_{\mathrm{end},\rho }^{\dag T} - \gamma (t) B_{\rho }^{l}(t)\}\nonumber \\ \delta \tau&= Q_\tau \{J_{\mathrm{end},\rho }^{\dag } \epsilon (t) - \gamma (t) \tau ^{l}_{\rho }(t)\} \end{aligned}$$

(64)

Using the symmetry of $Q_K^{-1}$, $\tilde{K}(\sigma )-\tilde{K}(\sigma -D)=\delta K(\sigma )$ and Eq. 64, the first term in the integrand of Eq. 63 can be written as:

$$\begin{aligned}&\hbox {tr}\{[\tilde{K}^\mathrm{T}(\sigma )Q_K^{-1}\tilde{K}(\sigma )]-[\tilde{K}^\mathrm{T}(\sigma -D)Q_K^{-1}\tilde{K}(\sigma -D)]\} \nonumber \\&\quad = \hbox {tr}\{[\tilde{K}^\mathrm{T}(\sigma ) - \tilde{K}^\mathrm{T}(\sigma -D)]^\mathrm{T} Q_K^{-1}[2\tilde{K}^\mathrm{T}(\sigma )\nonumber \\&\qquad -\tilde{K}^\mathrm{T}(\sigma )+\tilde{K}^\mathrm{T}(\sigma -D)]\}\nonumber \\&\quad = \hbox {tr}\{\delta K^\mathrm{T}(\sigma ) Q_K^{-1} [2\tilde{K}(\sigma )-\delta K(\sigma )]\} \nonumber \\&\quad = -\hbox {tr}\{\delta K^\mathrm{T}(\sigma ) Q_K^{-1} \delta K(\sigma )\} + 2 \ \hbox {tr}\{\delta K (\sigma ) Q_K^{-1} \tilde{K}(\sigma )\}\nonumber \\&\quad = -\hbox {tr}\{\delta K^\mathrm{T}(\sigma ) Q_K^{-1}\delta K (\sigma )\}\nonumber \\&\qquad + 2\epsilon ^\mathrm{T}(\sigma )(J_{end,\rho }^{\dag T}\tilde{K}J_{\mathrm{end},\rho }^{\dag })(\sigma )\delta (H^{d},H^\mathrm{r})(\sigma ) \nonumber \\&\qquad - 2\gamma (\sigma )\hbox {tr}\{(K_{\rho }^{l})^\mathrm{T}(\sigma )\tilde{K}(\sigma )\} \end{aligned}$$

(65)

In the same way, can be found the second terms in the integrand of Eq. 63 as:

$$\begin{aligned}&\hbox {tr}\{\tilde{B}^\mathrm{T}(\sigma )Q_B^{-1}\tilde{B}(\sigma )-\tilde{B}^\mathrm{T}(\sigma -D)Q_B^{-1}\tilde{B}(\sigma -D)\}\nonumber \\&\quad = -\hbox {tr}\{\delta B^\mathrm{T}(\sigma ) Q_B^{-1}\delta B (\sigma )\} \nonumber \\&\qquad + 2\epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T}\tilde{B}J_{\mathrm{end},\rho }^{\dag })(\sigma )\delta (H^{d},H^\mathrm{r})(\sigma ) \nonumber \\&\qquad - 2\gamma (\sigma )\hbox {tr}\{(B_{\rho }^{l})^\mathrm{T}(\sigma )\tilde{B}(\sigma )\} \end{aligned}$$

(66)

and third terms in the integrand of Eq. 63 as:

$$\begin{aligned}&[\tilde{\tau }^\mathrm{T}(\sigma ) Q_{\tau }^{-1}\tilde{\tau }(\sigma )]-[\tilde{\tau }^\mathrm{T}(\sigma -D)Q_{\tau }^{-1}\tilde{\tau }(\sigma -D)]\nonumber \\&\quad = -[\delta \tau ^\mathrm{T}(\sigma ) Q_{\tau }^{-1} \delta \tau (\sigma )] \nonumber \\&\qquad + 2\epsilon ^\mathrm{T}(\sigma ) (J_{\mathrm{end},\rho }^{\dag T} \tilde{\tau })(\sigma ) - 2\gamma (\sigma )(\tau ^{l}_{\rho })^\mathrm{T}(\sigma ) \tilde{\tau }(\sigma ) \end{aligned}$$

(67)

Replacing Eqs. 65, 66 and 67 into 63, we obtain:

$$\begin{aligned} \delta M_\mathrm{C}(t)&= -\frac{1}{2}\int _{t-D}^{t} [\delta \tilde{\Phi }^\mathrm{T}(\sigma ) Q^{-1}\delta \tilde{\Phi }(\sigma )]\,\hbox {d}\sigma \nonumber \\&- \int _{t-D}^{t} [\gamma (\sigma ) \tilde{\Phi }^\mathrm{T}(\sigma ) \Phi (\sigma )]\,\hbox {d} \sigma \nonumber \\&+ \int _{t-D}^{t}\![\epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T}\tilde{K}J_{\mathrm{end},\rho }^{\dag })(\sigma )\delta (H^{d},H^\mathrm{r})(\sigma )\nonumber \\&+\epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T}\tilde{B}J_{\mathrm{end},\rho }^{\dag })(\sigma )\delta (V^{d},V^\mathrm{r})(\sigma )\nonumber \\&+\epsilon ^\mathrm{T}(\sigma )(J_{\mathrm{end},\rho }^{\dag T}\tilde{\tau })(\sigma )]\,\hbox {d} \sigma \end{aligned}$$

(68)

Combining Eqs. 59 and 68, we obtain the first difference of cost function:

$$\begin{aligned} \delta C(t)&= C(t) - C(t-D) = \delta M_\mathrm{E}(t) + \delta M_\mathrm{C}(t)\nonumber \\&\le -\frac{1}{2}\int _{t-D}^{t} [\delta \tilde{\Phi }^\mathrm{T}(\sigma ) Q^{-1}\delta \tilde{\Phi }(\sigma )]\,\hbox {d} \sigma \nonumber \\&- \int _{t-D}^{t} [\gamma (\sigma ) \tilde{\Phi }^\mathrm{T}(\sigma ) \tilde{\Phi }+ \gamma (\sigma ) \tilde{\Phi }^\mathrm{T}(\sigma ) \Phi ^d(\sigma ) \nonumber \\&+\epsilon ^\mathrm{T}(\sigma )B^\mathrm{ini}_\mathrm{end}(\sigma )\epsilon (\sigma )]\,\hbox {d}\sigma \end{aligned}$$

(69)

To obtain $\delta C(t) \le 0$, a sufficient condition is:

$$\begin{aligned}&\epsilon ^\mathrm{T} B^\mathrm{ini}_\mathrm{end} \epsilon + \gamma \tilde{\Phi }^\mathrm{T} \tilde{\Phi }+ \gamma \tilde{\Phi }^\mathrm{T} \Phi ^{d} \ge \lambda _{B}||\epsilon ||^2 \nonumber \\&\quad + \gamma ||\tilde{\Phi }||^2 - \gamma ||\tilde{\Phi }||||{\Phi }^{d}|| \ge 0 \end{aligned}$$

(70)

where $\lambda _{B}$ as the infimum of the smallest eigenvalue of $B^\mathrm{ini}_\mathrm{end}$.

Replacing $\gamma (t) = \frac{p}{1 + u ||{\epsilon (t)}||^2}$ into Eq. 70, we obtain:

$$\begin{aligned} \lambda _{B}u||\epsilon ||^4 + \lambda _{B}||\epsilon ||^2 + p||\tilde{\Phi }||^2 - p||\tilde{\Phi }||||{\Phi }^{d}|| \ge 0 \end{aligned}$$

(71)

To find the regions of points $(||\epsilon ||^2,||\tilde{\Phi }||)$ for each of which Eq. 71 holds, we need firstly to determine the set of points that satisfies:

$$\begin{aligned} \lambda _{B}u||\epsilon ||^4 + \lambda _{B}||\epsilon ||^2 + p||\tilde{\Phi }||^2 - p||\tilde{\Phi }||||{\Phi }^{d}|| = 0 \end{aligned}$$

(72)

Equation 72 is an ellipse passing through the points $(||\epsilon ||^2 = 0,||\tilde{\Phi }||=0)$ and $(||\epsilon ||^2 = 0,||\tilde{\Phi }||=||{\Phi }^d||)$.

To find the canonical equation of this ellipse, we need only to complete the squares and we obtain:

$$\begin{aligned} \frac{\lambda _{B}u(||\epsilon ||^2 + \frac{1}{2u})^2+p(||\tilde{\Phi }||-\frac{||{\Phi }^{d}||}{2})^2}{\frac{\lambda _{B}}{4u}+\frac{p||{\Phi }^{d}||^{2}}{4}} = 1 \end{aligned}$$

(73)

By Krasovskii–LaSalle principle, $||\epsilon ||^2$ and $||\tilde{\Phi }||$ will converge to an invariant set $\Omega _s \subseteq \Omega$ on which $\delta C(t)=0$, where $\Omega$ is the bounding set defined as:

$$\begin{aligned} \Omega \equiv \left\{ (||\epsilon ||^2,||\tilde{\Phi }||), \frac{\lambda _{B}u(||\epsilon ||^2 + \frac{1}{2u})^2+p(||\tilde{\Phi }||-||{\Phi }^{d}||/2)^2}{\frac{\lambda _{B}}{4u}+\frac{p||{\Phi }^{d}||^{2}}{4}} \le 1 \right\} \nonumber \\ \end{aligned}$$

(74)

If the parameter $\gamma$ is constant [24], the bounding set is:

$$\begin{aligned} \left\{ (||\epsilon ||^2,||\tilde{\Phi }||),\frac{4\lambda _{B}||\epsilon ||^2 +4\gamma (||\tilde{\Phi }||-||{\Phi }^{d}||/2)^2}{\gamma ||{\Phi }^{d}||^2} \le 1 \right\} \end{aligned}$$

(75)

$\gamma$ does not affect convergence, but the convergence speed and size of convergence set.

Appendix C: Minjerk

1.1 C.1 Formal definition

Using Eq. 20, we can write the inside term of the integral as:

$$\begin{aligned} P&=\left\| {\frac{\hbox {d}^3}{\hbox {d}t^3}\mathbf r (s)}\right\| ^2=\left\| {\frac{\hbox {d}^2}{\hbox {d}t^2}\mathbf r '(s)\dot{s}}\right\| ^2\nonumber \\&=\left\| {\frac{\hbox {d}}{\hbox {d}t}(\mathbf r ''(s)\dot{s}^2+\mathbf r '(s)\ddot{s})}\right\| ^2\nonumber \\&=\left\| \mathbf{r '''(s)\dot{s}^3+3\mathbf r ''(s)\dot{s}\ddot{s}+\mathbf r '(s)\dddot{s}}\right\| ^2 \end{aligned}$$

(76)

To explicit the invariance with respect to rotations and translations of the minimization problem in Eq. 76, we can define uniquely 3D curve [56] by its curvature $R(s)$ and its torsion $\eta (s)$.

The path $\mathbf r$ satisfies Frenet’s formulas:

$$\begin{aligned} \mathbf t =R\mathbf n \,\mathbf n '=\eta \mathbf b -R\mathbf t \,\mathbf b '=-\eta \mathbf n \end{aligned}$$

(77)

From geometry, we know that:

$$\begin{aligned} \mathbf r '=\mathbf t '&\mathbf r ''=R\mathbf n&\mathbf r '''=R'\mathbf n +R(\eta \mathbf b -R\mathbf t ) \end{aligned}$$

(78)

We replacing Eq. 78 in Eq. 76 and we obtain:

$$\begin{aligned} P=\left\| \mathbf{n (R'\dot{s}^3+3R\dot{s}\ddot{s})+\mathbf t (\dddot{s}-R^2\dot{s}^3)+\mathbf b (\dot{s}^3R\eta )}\right\| ^2 \end{aligned}$$

(79)

$\mathbf n$, $\mathbf t$ and $\mathbf b$ are orthogonal and thus we obtain:

$$\begin{aligned} P=(R'\dot{s}^3+3R\dot{s}\ddot{s})^2+(\dddot{s}-R^2\dot{s}^3)^2+(\dot{s}^3R\eta )^2 \end{aligned}$$

(80)

1.2 C.2 Relation to the 2/3 power law

We want to find the relation of Eq. 80 to 2/3 power law.

To obtain this, we define a function:

$$\begin{aligned} Z_s=\dot{s}^3R(s) \end{aligned}$$

(81)

$Z_s$ corresponds to the term multiplying the torsion $\eta$ in Eq. 80.

We derive Eq. 81 respect to time and we obtain:

$$\begin{aligned} \begin{aligned}&R'(s)\dot{s}^4+3\dot{s}^2\ddot{s}R(s)=Z_s'\dot{s} \\&R'(s)\dot{s}^3+3\dot{s}\ddot{s}R(s)=Z_s' \end{aligned} \end{aligned}$$

(82)

The term $R'(s)\dot{s}^3+3\dot{s}\ddot{s}R(s)$ is equal to the term multiplying $\mathbf n$ in Eq. 79. We now substitute Eq. 82 in Eq. 79:

$$\begin{aligned} P&= \left\| \mathbf{n (Z_s')+\mathbf t (\dddot{s}-Z_sR)+\mathbf b (Z_s\eta )}\right\| ^2\nonumber \\&= Z_s'^2+(\dddot{s}-Z_sR)^2+Z_s^2\eta ^2 \end{aligned}$$

(83)

From Eq. 81, we have:

$$\begin{aligned} \dot{s}(t) = Z_s^{\frac{1}{3}}R^{-\frac{1}{3}} \end{aligned}$$

(84)

In the $2/3$ power law $Z_s^{\frac{1}{3}}=\hbox {const}$ and $Z_s'=0$ and it is equivalent to setting the coefficient of n of the instantaneous jerk to zero, and the coefficient of b proportional to the coefficient of t. To demonstrate this, we analyze the 2D power law:

$$\begin{aligned}&(x'^2+y'^2)^{1/2}=\hbox {const}\left( \frac{\sqrt{(x'y''-y'x'')^2)}}{(x'^2+y'^2)^{3/2}}\right) \nonumber \\&\quad \Rightarrow x'y''-y'x''=\hbox {const} \end{aligned}$$

(85)

Taking derivatives, we obtain:

$$\begin{aligned} \frac{x'}{y'}=\frac{x'''}{y'''}, \ \ \mathbf r '=\mathbf r ''' \end{aligned}$$

(86)

The jerk vector points are orthogonal to n and aligned with t. Thus, the jerk along n is zero.