Adaptation of manipulation skills in physical contact with the environment to reference force profiles

Abstract

We propose a new methodology for learning and adaption of manipulation skills that involve physical contact with the environment. Pure position control is unsuitable for such tasks because even small errors in the desired trajectory can cause significant deviations from the desired forces and torques. The proposed algorithm takes a reference Cartesian trajectory and force/torque profile as input and adapts the movement so that the resulting forces and torques match the reference profiles. The learning algorithm is based on dynamic movement primitives and quaternion representation of orientation, which provide a mathematical machinery for efficient and stable adaptation. Experimentally we show that the robot’s performance can be significantly improved within a few iteration steps, compensating for vision and other errors that might arise during the execution of the task. We also show that our methodology is suitable both for robots with admittance and for robots with impedance control.

Appendix: Impedance control

In general, the dynamics of a robot arm interacting with the environment is described by

$$\begin{aligned} \varvec{\rho }=\mathbf{H}\ddot{\pmb {\theta }}+\mathbf{h}+\mathbf{J}^\mathrm {T}\left[\mathbf{F}^\mathrm {T}, \mathbf{M}^\mathrm {T} \right]^\mathrm {T} \end{aligned}$$

(48)

where $\varvec{\rho }$ is a vector of joint torques, $\mathbf{H}\in \mathbb {R}^{n\times n}$ is a symmetric, positive definite inertia matrix, $\mathbf{h}\in \mathbb {R}^n$ contains nonlinear terms due to the centrifugal, Coriolis, friction and gravity forces, $\mathbf{J}\in \mathbb {R}^{6\times n}$ is the robot Jacobian, and $\mathbf{F},\, \mathbf{M}\in \mathbb {R}^3$ are the vectors of environment contact forces and torques acting on the robot’s end-effector. $\pmb {\theta } \in \mathbb {R}^n$ denotes the joint angles. The following relationship holds between joint space and Cartesian space accelerations (Hsu et al. 1989; Nakanishi et al. 2008)

$$\begin{aligned} \ddot{\pmb {\theta }} = \mathbf{J}_\mathbf{H}^+\left( \left[ \begin{array}{c} \ddot{\mathbf{p}} \\ \dot{\pmb {\omega }} \end{array} \right] - \dot{\mathbf{J}}\dot{\pmb {\theta }}\right) + \mathbf{N}\pmb {\xi }, \end{aligned}$$

(49)

where $\mathbf{J}_\mathbf{H}^+=\mathbf{H}^{-1}\mathbf{J}^\mathrm {T}(\mathbf{J}\mathbf{H}^{-1}\mathbf{J}^\mathrm {T})^{+} = \mathbf{H}^{-1/2}(\mathbf{J}\mathbf{H}^{-1/2})^+$ denotes the inertia weighted pseudo-inverse of $\mathbf{J}$ (Nakamura 1991; Whitney 1969). $\pmb {\xi }$ is any vector from $\mathbb {R}^n$ and defines the null space motion. By inserting (49) into (48) we obtain a general form control law for a redundant robot (Nemec et al. 2007)

$$\begin{aligned} \varvec{\rho }=\mathbf{H}\mathbf{J}_\mathbf{H}^+\left( \left[ \begin{array}{c} \ddot{\mathbf{p}} \\ \dot{\pmb {\omega }} \end{array} \right] -\dot{\mathbf{J}}\dot{\pmb {\theta }}\right) +\mathbf{H}\mathbf{N}\pmb {\xi } + \mathbf{h}+ \mathbf{J}^\mathrm {T}\left[\begin{array}{c} \mathbf{F}\\ \mathbf{M}\end{array}\right], \end{aligned}$$

(50)

where $\mathbf{N}$ is the projection matrix onto the null space of inertia weighted Jacobian. It is defined as $\mathbf{N}=\mathbf{I}-\mathbf{J}_\mathbf{H}^+\mathbf{J}$. The parameters $\ddot{\mathbf {p}}$, $\dot{\pmb {\omega }}$, and $\pmb {\xi }$ can be used as control inputs that should be set so that the task space tracking errors are minimized. The first term in Eq. (50) is the task controller, the second term is the null space controller and the third and the fourth term compensate for the non-linear robot dynamics and external forces, respectively. Lets choose the task command inputs $\ddot{\mathbf{p}}_c,$ $\dot{\pmb {\omega }}_c$ as follows

$$\begin{aligned} \ddot{\mathbf{p}}_c= & {} \ddot{\mathbf{p}}_r+\mathbf{K}^d_p(\dot{\mathbf{p}}_r-\dot{\mathbf{p}})+\mathbf{K}^p_p(\mathbf{p}_r - \mathbf{p}) + \mathbf{K}_{s1}\mathbf{e}_p, \end{aligned}$$

(51)

$$\begin{aligned} \dot{\pmb {\omega }}_c= & {} \dot{\pmb {\omega }}_r+\mathbf{K}^d_q(\pmb {\omega }_r- \pmb {\omega })+\mathbf{K}^p_q\log \left( \mathbf{q}_r*\overline{\mathbf{q}}\right) + \mathbf{K}_{s2}\mathbf{e}_q, \end{aligned}$$

(52)

where subscript ${}_r$ denotes the reference values and variables without the substrict the current values as received from the robot. The force/torque errors $\mathbf{e}_p$ and $\mathbf{e}_q$ are calculated using (28) and (29), respectively. $\mathbf{K}^d_p$, $\mathbf{K}^d_q$, $\mathbf{K}^p_p$, $\mathbf{K}^p_q$, $\mathbf{K}_{s1}$ and $\mathbf{K}_{s2}$ are positive definite, diagonal positional and rotational damping matrices, positional and rotational stiffness matrices, and force and torque feedback matrices, respectively. The reference position and orientation are calculated by integrating (20), (37), (8), (38), and (32), followed by displacement (28) – (28). The reference velocities and accelerations are calculated as $\dot{\mathbf{p}}_r=\Delta \mathbf{q}_{w}*\dot{\mathbf{p}}_{ DMP}* \bar{\Delta \mathbf{q}} _w$ and $\ddot{\mathbf{p}}_r=\Delta \mathbf{q}_w*\ddot{\mathbf{p}}_{ DMP}* \bar{\Delta \mathbf{q}}_w$, respectively, while the reference angular velocities and accelerations are calculated as $\omega _r=\Delta \mathbf{q}_w*\eta _{ DMP}/\tau * \bar{\Delta \mathbf{q}}_w$ and $\dot{\omega }_r=\Delta \mathbf{q}_w*\dot{\eta }_{ DMP}/\tau *\bar{\Delta \mathbf{q}}_w$, respectively.

With the above choice of command accelerations we obtain the well known impedance control law (Hogan 1985). By choosing also the null space command input $\pmb {\xi }_c$, one can control the null space motion of the robot. Simply setting $\pmb {\xi }_c$ to $0$ results in non-conservative motion, i. e. the robot will constantly move in null space minimizing the kinetic energy. An appropriate solution to this problem is to set the desired null space velocities to zero, which results in an energy dissipation controller (Khatib 1987). The command null space motion vector is thus calculated as

$$\begin{aligned} \pmb {\xi }_c = -\mathbf{K}_n\dot{\pmb {\theta }}. \end{aligned}$$

(53)

The commanded torque $\varvec{\rho }_c$ is finally calculated by inserting $\ddot{\pmb {p}}_c$, $\dot{\pmb {\omega }}_c$, $\pmb {\xi }_c$ into (50), whereas the current values are used for all other variables in (50).