Learning potential functions from human demonstrations with encapsulated dynamic and compliant behaviors

Abstract

We consider the problem of devising a unified control policy capable of regulating both the robot motion and its physical interaction with the environment. We formulate this control policy by a non-parametric potential function and a dissipative field, which both can be learned from human demonstrations. We show that the robot motion and its stiffness behaviors can be encapsulated by the potential function’s gradient and curvature, respectively. The dissipative field can also be used to model desired damping behavior throughout the motion, hence generating motions that follows the same velocity profile as the demonstrations. The proposed controller can be realized as a unification approach between “realtime motion generation” and “variable impedance control”, with the advantages that it has guaranteed stability as well as does not rely on following a reference trajectory. Our approach, called unified motion and variable impedance control (UMIC), is completely time-invariant and can be learned from a few demonstrations via solving two (convex) constrained quadratic optimization problems. We validate UMIC on a library of 30 human handwriting motions and on a set of experiments on 7-DoF KUKA light weight robot.

Appendix

1.1 Coefficients of the optimization function

As described in Sect. 4, the optimization problem given by Eq. (13) can be transformed into the well-known form:

$$\begin{aligned}&\min _{\varvec{\varTheta }} J(\varvec{\varTheta }) = \frac{1}{2} \varvec{\varTheta }^T \varvec{H} \varvec{\varTheta } + \varvec{h}^T \varvec{\varTheta } + h_0\nonumber \\&{\text {subject to}} \end{aligned}$$

(20a)

$$\begin{aligned}&\varvec{\mathcal {C}} \varvec{\varTheta } \le \varvec{0} \end{aligned}$$

(20b)

$$\begin{aligned}&\varvec{\mathcal {G}} \varvec{\varTheta } = \varvec{g} \end{aligned}$$

(20c)

where $\varvec{H} \in {\mathbb {R}}^{{\mathcal {T}} \times {\mathcal {T}}}$ is a symmetric positive semi-definite matrix,⁵ $\varvec{{\mathcal {C}}} \in {\mathbb {R}}^{{\mathcal {T}} \times {\mathcal {T}}}$ and $\varvec{{\mathcal {G}}} \in {\mathbb {R}}^{d \times {\mathcal {T}}}$ are full rank matrices, $\varvec{h} \in {\mathbb {R}}^{{\mathcal {T}}}$, $\varvec{g} \in {\mathbb {R}}^{d}$, and $h_0$ is a scalar independent of $\varvec{\varTheta }$.

In this appendix, we provide the formulation to compute the coefficients of this quadratic optimization function as given by Eq. (20). The first step to reach our goal is to transform the term $- \nabla \varPhi (\varvec{\xi }^i;\varvec{\varTheta })$ into the form $\varvec{A}^i \varvec{\varTheta } + \varvec{b}^i$, where $\varvec{A}^i \in {\mathbb {R}}^{d \times {\mathcal {T}}}$ and $\varvec{b}^i \in {\mathbb {R}}^{d}$. Let us define:

$$\begin{aligned}&\varvec{\eta }^{ik} = \frac{\tilde{\omega }^{k}(\varvec{\xi }^{i})}{(\sigma ^{k})^2}\left( \varvec{\xi }^{i} - \varvec{\xi }^{k}\right) \end{aligned}$$

(21a)

$$\begin{aligned}&\varvec{\rho }^{i} = \sum _{k=1}^{{\mathcal {T}}} \varvec{\eta }^{ik}\end{aligned}$$

(21b)

$$\begin{aligned}&\upsilon ^{ik} = \frac{1}{2}\left( \varvec{\xi }^i - \varvec{\xi }^k\right) ^T \varvec{S}^k \left( \varvec{\xi }^i - \varvec{\xi }^k\right) \end{aligned}$$

(21c)

then we have:

$$\begin{aligned}&\varvec{A}^i = \left[ \varvec{\eta }^{i1} - \tilde{\omega }^{1}(\varvec{\xi }^{i}) \varvec{\rho }^{i} \ldots \varvec{\eta }^{i {\mathcal {T}}} - \tilde{\omega }^{{\mathcal {T}}}(\varvec{\xi }^{i}) \varvec{\rho }^{i} \right] \end{aligned}$$

(22a)

$$\begin{aligned}&\varvec{b}^i = \sum _{k=1}^{{\mathcal {T}}} \upsilon ^{ik} \varvec{\eta }^{ik} - \tilde{\omega }^{k}(\varvec{\xi }^{i}) \Big ( \upsilon ^{ik} \varvec{\rho }^{i} + \varvec{S}^k\left( \varvec{\xi }^{i} - \varvec{\xi }^{k}\right) \Big ) \end{aligned}$$

(22b)

The desired gradient for the i-th data point is also given by $\varvec{\gamma }^i$ (see Sect. 4). To account for all the points in the data set, we concatenate the matrices $\varvec{A}^i$, vectors $\varvec{b}^i$ and $\varvec{\gamma }^i$ into a bigger matrix $\varvec{A}\in {\mathbb {R}}^{{\mathcal {T}}d \times {\mathcal {T}}}$ and vectors $\varvec{b} \in {\mathbb {R}}^{{\mathcal {T}}d}$ and $\varvec{\gamma } \in {\mathbb {R}}^{{\mathcal {T}}d}$:

$$\begin{aligned}&\varvec{A}= \left[ (\varvec{A}^1)^T \ldots (\varvec{A}^{{\mathcal {T}}})^T \right] ^{T}\end{aligned}$$

(23a)

$$\begin{aligned}&\varvec{b} = \left[ \varvec{b}^1 \ldots \varvec{b}^{{\mathcal {T}}} \right] ^{T}\end{aligned}$$

(23b)

$$\begin{aligned}&\varvec{\gamma } = \left[ \varvec{\gamma }^1 \cdots \varvec{\gamma }^{{\mathcal {T}}} \right] ^{T} \end{aligned}$$

(23c)

Then we could obtain the equivalent of Eq. (13a) in the quadratic form $\frac{1}{2} \varvec{\varTheta }^T \varvec{H} \varvec{\varTheta } + \varvec{h}^T \varvec{\varTheta } + h_0$:

$$\begin{aligned}&\varvec{H} = 2 \varvec{A}^T \varvec{A}\end{aligned}$$

(24a)

$$\begin{aligned}&\varvec{h} = 2 \varvec{A}^T (\varvec{b} - \varvec{\gamma }) \end{aligned}$$

(24b)

$$\begin{aligned}&h_0 = (\varvec{b} - \varvec{\gamma })^T (\varvec{b} - \varvec{\gamma }) \end{aligned}$$

(24c)

The matrix $\varvec{{\mathcal {C}}}$ in Eq. (20b) can be computed by:

$$\begin{aligned} \varvec{{\mathcal {C}}}^{ij} = {\left\{ \begin{array}{ll} -1 &{}\quad i=j \\ 1 &{}\quad i=j-1,~ i \notin \varOmega \\ 0 &{}\quad otherwise \end{array}\right. } \end{aligned}$$

(25)

where $\varvec{{\mathcal {C}}}^{ij}$ refers to the component in the i-th row and j-th column of $\varvec{{\mathcal {C}}}$, and $\varOmega$ is the set of indices that corresponds to the last point of each demonstration trajectory.

The matrix $\varvec{{\mathcal {G}}}$ and vector $\varvec{g}$ in Eq. (20c) are in fact equal to $\varvec{A}^{{\mathcal {T}}}$ and $-\varvec{b}^{{\mathcal {T}}}$, respectively. This is due to the fact that the last point of each demonstration trajectory is in fact the target point, hence $\varvec{\xi }^{{\mathcal {T}}} = \varvec{\xi }^*$. Note that as we have N trajectories, we could use any of the N available $\varvec{A}^i$ and $\varvec{b}^i$, $i \in \varOmega$ to obtain $\varvec{{\mathcal {G}}}$ and vector $\varvec{g}$. Here for simplicity we use the last point of the last trajectory which by construction has the index ${\mathcal {T}}$.

1.2 Stability proof

For a manipulator with d generalized degrees of freedom $\varvec{\xi }\in {\mathbb {R}}^{d}$, the robot dynamics (whether in operational or joint space) can be represented by (Khatib 1995; Ott 2008; Siciliano et al. 2009):

$$\begin{aligned} \varvec{M}(\varvec{\xi }) \ddot{\varvec{\xi }}+ \varvec{C}\left( \varvec{\xi },\dot{\varvec{\xi }}\right) \dot{\varvec{\xi }}+ \varvec{g}(\varvec{\xi }) = \varvec{\tau } + \varvec{\tau }_{ext} \end{aligned}$$

(26)

where $\varvec{M}(\varvec{\xi }) \in {\mathbb {R}}^{d \times d}$ is the inertia matrix, $\varvec{C}(\varvec{\xi },\dot{\varvec{\xi }}) \in {\mathbb {R}}^{d \times d}$ is the Coriolis/centrifugal matrix, $\varvec{g}(\varvec{\xi })$ is the gravitational force, $\varvec{\tau }$ represents the actuators generalized force, and $\varvec{\tau }_{ext}$ is the external generalized force applied to the robot by the environment.

Similarly to an impedance controller, the actuators generalized force in our control setting is composed of two terms: the gravitational term $\varvec{g}(\varvec{\xi })$ to compensate for the weight of the robot and the controller term $\varvec{\tau }_{c}$ to perform the task:

$$\begin{aligned} \varvec{\tau } = \varvec{\tau }_{c} + \varvec{g}(\varvec{\xi }) \end{aligned}$$

(27)

To verify stability of UMIC, we use the following definition of passivity, taken from (Slotine and Li 1991):

Definition 1

A system with input effort v and output flow y is passive if it satisfies:

$$\begin{aligned} \dot{V} = v^Ty-n \end{aligned}$$

(28)

for some lower bounded scalar functions V and $n \ge 0$.

In our setting, the effort is $\varvec{\tau }_{ext}$ and the flow is $\dot{\varvec{\xi }}$. To ensure passivity/stability of our controller, we define the following candidate Lyapunov function:

$$\begin{aligned} V(\varvec{\xi },\dot{\varvec{\xi }}) = \varPhi (\varvec{\xi }) + \frac{1}{2} \dot{\varvec{\xi }}^T \varvec{M}(\varvec{\xi }) \dot{\varvec{\xi }}\end{aligned}$$

(29)

Taking the time-derivative of $V(\varvec{\xi },\dot{\varvec{\xi }})$ yields:

$$\begin{aligned} \dot{V}(\varvec{\xi },\dot{\varvec{\xi }}) = \dot{\varvec{\xi }}^T \nabla \varPhi (\varvec{\xi }) + \dot{\varvec{\xi }}^T \varvec{M}(\varvec{\xi }) \ddot{\varvec{\xi }}+ \frac{1}{2}\dot{\varvec{\xi }}^T \dot{\varvec{M}}\left( \varvec{\xi },\dot{\varvec{\xi }}\right) \dot{\varvec{\xi }}\end{aligned}$$

(30)

The term $\varvec{M}(\varvec{\xi }) \ddot{\varvec{\xi }}$ can be obtained by rearranging Eq. (26):

$$\begin{aligned} \varvec{M}(\varvec{\xi }) \ddot{\varvec{\xi }}= \varvec{\tau } + \varvec{\tau }_{ext} - \varvec{C}\left( \varvec{\xi },\dot{\varvec{\xi }}\right) \dot{\varvec{\xi }}- \varvec{g}(\varvec{\xi }) \end{aligned}$$

(31)

Furthermore from Eqs. (1) and (27), we have $\varvec{\tau } = \varvec{\tau }_{c} + \varvec{g}(\varvec{\xi }) = -\nabla \varPhi (\varvec{\xi }) - \varvec{\varPsi }(\varvec{\xi },\dot{\varvec{\xi }}) + \varvec{g}(\varvec{\xi })$. Considering this and the skew-symmetric property $\dot{\varvec{M}}(\varvec{\xi },\dot{\varvec{\xi }}) - 2\varvec{C}(\varvec{\xi },\dot{\varvec{\xi }})$, Eq. (31) can be simplified to:

$$\begin{aligned} \dot{V}(\varvec{\xi },\dot{\varvec{\xi }})&= - \dot{\varvec{\xi }}^T \varvec{\varPsi }(\varvec{\xi },\dot{\varvec{\xi }}) - \dot{\varvec{\xi }}^T \varvec{\tau }_{ext} \nonumber \\&\quad -\, \underbrace{\dot{\varvec{\xi }}^T \Big ( \sum _{i} \tilde{\omega }^i(\varvec{\xi }) \varvec{D}^i \Big ) \dot{\varvec{\xi }}}_{\varvec{n}(\varvec{\xi },\dot{\varvec{\xi }}) \ge 0} + \dot{\varvec{\xi }}^T \varvec{\tau }_{ext} \nonumber \\&= - \varvec{n}(\varvec{\xi },\dot{\varvec{\xi }}) + \dot{\varvec{\xi }}^T \varvec{\tau }_{ext} \end{aligned}$$

(32)

Note that $\varvec{n}(\varvec{\xi },\dot{\varvec{\xi }}) \ge 0$ because $D^i$ are positive definite matrices and by construction $\tilde{\omega }^i(\varvec{\xi })>0$. Hence, following Definition 1, UMIC yields a passive map from the external generalized force to the velocity of the manipulator.