[1]Jonas Wittmann

1]Technical University of Munich, TUM School of Engineering and Design,
Department of Mechanical Engineering, Chair of Applied Mechanics; Munich
Institute of Robotics and Machine Intelligence (MIRMI); Boltzmannstr. 15,
Garching, 85748, Germany

# Energy-Aware Hierarchical Control of Joint Velocities

<EMAIL_ADDRESS>Daniel Hornung<EMAIL_ADDRESS>Korbinian
Griesbauer<EMAIL_ADDRESS>Daniel Rixen<EMAIL_ADDRESS>[

###### Abstract

Nowadays, robots are applied in dynamic environments. For a robust operation,
the motion planning module must consider other tasks besides reaching a
specified pose: (self) collision avoidance, joint limit avoidance, keeping an
advantageous configuration, etc. Each task demands different joint control
commands, which may counteract each other. We present a hierarchical control
that, depending on the robot and environment state, determines online a
suitable priority among those tasks. Thereby, the control command of a lower-
prioritized task never hinders the control command of a higher-prioritized
task. We ensure smooth control signals also during priority rearrangement. Our
hierarchical control computes reference joint velocities. However, the
underlying concepts of hierarchical control differ when using joint
accelerations or joint torques as control signals instead. So, as a further
contribution, we provide a comprehensive discussion on how joint velocity
control, joint acceleration control, and joint torque control differ in
hierarchical task control. We validate our formulation in an experiment on
hardware.

###### keywords:

Kinematics, Redundant Robots, Task Planning, Motion Control

##  1 Introduction

Robots in dynamic environments must consider several tasks simultaneously,
e.g. reaching a goal and avoiding obstacles. We define a task as follows:

######  Definition 1.

A task is a user-defined behavior of the robot for which a control target
$\mathbf{u}(\mathbf{q},\dot{\mathbf{q}})$ needs to be computed in a task space
of dimension $m$. This control depends on the current robot state, i.e. the
current joint configuration $\mathbf{q}$ and joint velocity
$\dot{\mathbf{q}}$, which are expressed in the robot’s configuration space
($\mathcal{C}$-space) of dimension $n$. The task Jacobian
$\mathbf{J}(\mathbf{q})\in\mathbb{R}^{m\times n}$ maps the control command
from $\mathcal{C}$-space to task space. A scalar metric
$\eta(\mathbf{q})\in[0,~{}1]$ expresses the current importance of the task.
Mathematically formulated:

$\textsc{task}:\mathbf{q},\mathbf{\dot{q}}\mapsto\mathbf{J},\mathbf{u},\eta$

For readability, we omit the dependency of $\mathbf{J}$, $\mathbf{u}$, and
$\eta$ on the robot state in the following. An example for a task is obstacle
avoidance, which may be formulated as a force that pushes the closest point on
the robot away from the obstacle. So $\mathbf{u}\in\mathbb{R}^{3}$ would be a
3D force vector whose direction is along the closest distance and whose norm
is determined by a user-defined law that relates the force $\mathbf{u}$ to
$\mathbf{q}$ and optionally $\dot{\mathbf{q}}$, e.g. with a PD control. The
task Jacobian $\mathbf{J}(\mathbf{q})\in\mathbb{R}^{3\times n}$ would be the
translational Jacobian of the closest point on the robot, and $\eta$ would
increase to one the closer the robot approaches the obstacle.
In the given example, $\mathbf{u}$ is a desired force $\mathbf{F}_{d}$ in task
space, but the task could also be formulated to compute a desired task space
velocity $\dot{\mathbf{x}}_{d}$ or acceleration $\ddot{\mathbf{x}}_{d}$. In
the following, subscript $\star_{d}$ represents desired values while values
without subscript indicate measured values. So according to Chiaverini et al.
(2008), we distinguish:

  1. 1.

In first-order differential kinematics control, denoted as velocity control in
the following, $\mathbf{u}$ is a desired task space velocity
$\dot{\mathbf{x}}_{d}$ that maps to a desired $\mathcal{C}$-space velocity
$\dot{\mathbf{q}}_{d}$ according to:

$\dot{\mathbf{q}}_{d}=\mathbf{J}^{\\#}_{W}\dot{\mathbf{x}}_{d}$ (1)

$\mathbf{J}^{\\#}_{W}$ is the $\mathbf{W}$-weighted pseudo-inverse of the task
Jacobian, and $\mathbf{W}$ is symmetric positive definite:

$\mathbf{J}^{\\#}_{W}=\mathbf{W}^{-1}\mathbf{J}^{T}\left(\mathbf{J}\mathbf{W}^{-1}\mathbf{J}^{T}\right)^{-1}$

  2. 2.

In second-order differential kinematics control, denoted as acceleration
control in the following, $\mathbf{u}$ is a desired task space acceleration
$\ddot{\mathbf{x}}_{d}$ that maps to a desired $\mathcal{C}$-space
acceleration $\ddot{\mathbf{q}}_{d}$ according to:

$\ddot{\mathbf{q}}_{d}=\mathbf{J}^{\\#}_{W}\left(\ddot{\mathbf{x}}_{d}-\dot{\mathbf{J}}\dot{\mathbf{q}}\right)$
(2)

  3. 3.

In torque control, $\mathbf{u}$ is a desired task space force $\mathbf{F}_{d}$
that maps to a desired $\mathcal{C}$-space torque $\boldsymbol{\tau}_{d}$
according to:

$\boldsymbol{\tau}_{d}=\mathbf{J}^{T}\mathbf{F}_{d}$ (3)

Concerning the terminology, we point out that, in torque control,
$\mathbf{F}_{d}$ could be an impedance controller that tracks a desired motion
and so, in the end, is used to track a desired task space position, velocity,
and acceleration. So, one can argue that in this case, $\boldsymbol{\tau}_{d}$
is used for velocity control, but we refer to it as torque control. The same
holds for velocity control, in which $\dot{\mathbf{x}}_{d}$ might be the
output of an admittance controller and so, in the end, is used to track a
desired task space force. So, one can argue that in this case,
$\dot{\mathbf{q}}_{d}$ is used for force control, but we refer to it as
velocity control.
Equations (1-3) map a task control to $\mathcal{C}$-space, and we refer to
this idea as task mapping in the following. These three mappings are linear.
This is not the case on position-level, where the mapping between a task space
variable $\mathbf{x}_{d}(\mathbf{q})$ and a corresponding $\mathcal{C}$-space
configuration $\mathbf{q}$ may be nonlinear, e.g. in motion tracking, where
$\mathbf{x}_{d}(\mathbf{q})$ is the desired Cartesian pose of the end
effector. In offline modules, such as position-level inverse kinematics for
path planning, the nonlinear mapping is solved iteratively, e.g. with a
Newton-based approach that exploits the linear mapping on velocity-level. As
we target online control, we will not consider the mapping on position-level
in the following.
Hierarchical control deals with several tasks that must be accomplished
simultaneously, defining priorities among those. It ensures that
$\mathcal{C}$-space control commands due to lower-prioritized tasks do not
counteract $\mathcal{C}$-space control commands originating from higher-
prioritized tasks. It is possible to execute several tasks simultaneously
because there are $n$ $\mathcal{C}$-space control variables, but one task may
have a task space of lower dimension $m<n$, and so there are $n-m$ remaining
degrees of freedom (DoF) to achieve lower-prioritized tasks. That is, the
robot is redundant w.r.t. the higher-prioritized task. Hierarchical control
uses projection techniques to avoid task conflicts: One can project the lower-
prioritized control into the nullspace of a higher-prioritized control. That
way, the control of the lower-prioritized task is not fully executed by the
robot, but only to such an extend that it has no effect in the higher-
prioritized task space. Therefore, the nullspace projection matrix
$\mathbf{N}$ is used that computes the nullspace of the higher-prioritized
task Jacobian, e.g. for velocity control:
$\mathbf{N}=\mathbf{I}-\mathbf{J}^{\\#}_{W}\mathbf{J}$. In the following, we
refer to the idea of projecting a lower-prioritized task into the nullspace of
a higher-prioritized task as nullspace projection. For torque control,
Dietrich et al. (2015) discusses the dynamical consistency of a nullspace
projection as a first important property, and we add the discussion for
velocity and acceleration control in Section 3.2. Further, Dietrich et al.
(2015) discusses the strictness of a nullspace projection as a second
important property that is relevant when there are more than two active tasks.
They distinguish successive and augmented projections. The former is non-
strict/soft, i.e. the third-level task, though not counteracting the second-
level task, may still impact the first-level task. The latter implements a
strict hierarchy with no task conflicts on any level. The difference is that
in the augmented approach, the task Jacobian of a lower-prioritized task
includes all the task Jacobians of higher-prioritized tasks. We use the
augmented approach in the following.
Some hierarchical control approaches define a static hierarchy in which safety
constraints are put at the first priority levels and activated only when
necessary. Other approaches define a dynamic hierarchy that evolves during
robot operation: The example for a collision avoidance task given above
increases the task priority the closer the robot to a collision. For these
approaches with dynamic hierarchy, there are solutions to avoid lower-
prioritized tasks counteract higher-prioritized tasks and to ensure that the
joint control commands are smooth during task priority rearrangment Liu et al.
(2015); Dehio and Steil (2019); Dietrich et al. (2015). Our contributions to
these existing solutions are:

  * •

The proposed solutions focus on only one of the three approaches: velocity
control, acceleration control, or torque control. But, a comprehensive
discussion on how they differ in terms of task mapping characteristics and
nullspace projection characteristics was, so far, not reported. We provide
this discussion in Section 3.

  * •

One further contribution consists in an enhanced version of the hierarchical
control approach in Liu et al. (2015); Dehio and Steil (2019): 1) Our approach
allows to consider the energy consumption of the robot; 2) Our approach
rearranges task priorities online based on importance metrics that consider a
robust robot operation. The existing approach defines hierarchy switching
points offline based on time, which is not useful for online applications. For
example, it is predefined that after 3.2 s, the least prioritized task will
become the most prioritized task.

  * •

Liu et al. (2015); Dehio and Steil (2019) apply the approach in simulations.
We apply the approach to hardware demonstrating its real-time capability.

##  2 Related Work

Siciliano and Slotine (1991) proposes a framework to handle multiple
hierarchies for velocity and acceleration control. However, it enforces a
strict hierarchy that leads to discontinuous control signals at priority
rearrangements, and it is only verified in simulation. Mansard and Chaumette
(2007) presents a velocity control that prioritizes tasks online. However,
they remove tasks based on expected task conflicts, whereas we consider task
importance. They validate their approach in experiments on a Gantry robot with
six DoF and achieve a control cycle of 25 Hz, whereas our robots demand a 1
kHz real-time control. Salini et al. (2011) introduces non-strict hierarchies
and, similar to our approach, uses an automated task reprioritization
according to relative importance. However, similar to Liu et al. (2015); Dehio
and Steil (2019), the change of the relative importance is based on time.
Further, they focus on torque control and only validate in simulation. Dehio
(2018) provides further details on Dehio and Steil (2019) and formulates the
different nullspace projectors for torque control and velocity control.
However, analysis and results are only provided for torque control. The
hierarchical control in Dietrich et al. (2012) ensures continuous control
signals during the activation and deactivation of tasks. Therefore, they shape
the nullspace projector based on a task activation variable, which is also our
approach. They provide experiments on a humanoid robot. However, their
presentation is limited to torque control. Kim et al. (2019) applies
hierarchical quadratic programming to implement continuous task transitions
and insertions. They compute continuous control commands when tasks are
inserted, removed, or rearranged, and, as they rely on nonlinear optimization,
they can handle differently formulated tasks, i.e. equality and inequality
tasks. They provide experiments on a seven DoF robot using kinematic control.
However, their approach formulates QP problems online, so they depend on
additional third-party software to solve those. They achieve an average
computation time of 3.7 ms for a three-level hierarchy. Based on Liu et al.
(2015); Dehio and Steil (2019), we provide a closed-form solution and achieve
an average computation time of less than 1.0 ms for a four-level hierarchy.
Tarbouriech et al. (2018) also presents a QP-based hierarchical velocity
control and evaluates the approach with two KUKA LWR4. The hierarchy contains
a primary motion task and a secondary joint limit avoidance task. Hu et al.
(2015) presents a velocity-based control approach for a dual-arm manufacturing
application with two KUKA LBR iiwa. A hierarchy of three tasks, including
trajectory tracking, joint limit avoidance, and respecting joint velocity
limits, is considered. However, the framework allows the adaption of the
hierarchy only by removing the lowest task from the Jacobian, which is
triggered by rank deficiency. That is, they do not provide a solution to adapt
priorities based on task importance. Further, they suffer from discontinuities
in the computed velocity control signals. In their conclusion, they outline
the importance of investigating an inertia-weighted pseudo-inverse, which we
present in our work. Our work builds upon Liu et al. (2015) and Dehio and
Steil (2019), and we will outline those methods and our modifications
throughout the paper.

##  3 Task Mapping, Nullspace Projection, and the Effect of the Weighting
Matrix

Task mapping and nullspace projection with their characteristics are well
discussed in literature. However, they are often not clearly separated. In
this section, we discuss how the selection of the weighting matrix
$\mathbf{W}$ in $\mathbf{J}^{\\#}_{W}$ affects the characteristics of task
mapping and nullspace projection, and thereby, we distinguish velocity,
acceleration, and torque control. We have not found this comprehensive
discussion in literature.

(a) Four DoF Manipulator in 2D (b) Effect of the choice of $\mathbf{W}$ on the
kinetic energy $\frac{1}{2}\dot{\mathbf{q}}^{T}\mathbf{M}\dot{\mathbf{q}}$ (c)
Effect of the choice of $\mathbf{W}$ on the dynamical consistency

Figure 1: A manipulator with four DoF tracks a 2D translational trajectory
($\mathbf{x}_{d}(t)$,$\dot{\mathbf{x}}_{d}(t)$,$\ddot{\mathbf{x}}_{d}(t)$).
Figure 1a shows that in the dynamic robot model, the links are approximated as
point masses at the center of mass of each link. For this scenario, Figure 1b
benchmarks Eqs. (1), (2) and (6) for the case that the only given task is to
track the trajectory. The curves corresponding to Eqs. (1) and (6), i.e. the
green and blue lines, are superposed. Figure 1c shows the dynamical
consistency condition DynCon in Eqs. (9) and (10) for the case that trajectory
tracking is the primary task and the identity joint command
$\mathbf{I}^{n\times 1}$ is used as a secondary task. The curves for
$\mathbf{W}=\mathbf{M}$ and for velocity and acceleration control are
superposed, i.e. all lines, except the dashed blue line, are superposed

###  3.1 Optimized Task Mapping for Velocity and Acceleration Control

First, we investigate task mapping, i.e. we map the task’s control from its
task space to $\mathcal{C}$-space according to Eqs. (1), (2), or (3). So in
this section, we consider only one task, and thus, there is no nullspace
projection of any other tasks.
Torque control of redundant robots defines a unique mapping from task space to
$\mathcal{C}$-space with Eq. (3). This is not the case in velocity and
acceleration control that have infinite solutions that differ in the selection
of $\mathbf{W}$ in $\mathbf{J}^{\\#}_{W}$. Thus, the optimized mapping from
task space to $\mathcal{C}$-space, i.e. the optimal choice of $\mathbf{W}$, is
only relevant for velocity and acceleration control.
$\mathbf{W}=\mathbf{I}^{n\times n}$ is a typical choice to minimize the local
$\text{L}^{2}$-norm of $\dot{\mathbf{q}}$ in velocity control or
$\ddot{\mathbf{q}}$ in acceleration control. This weighting is relevant to
keep the $\mathcal{C}$-space control feasible, i.e. within the joint velocity
and acceleration limits. For velocity control, it was proposed as Resolved
Motion Rate Control in Whitney (1969).
Using the $\mathcal{C}$-space mass matrix instead, i.e.
$\mathbf{W}=\mathbf{M}(\mathbf{q})$, is another typical choice, and it is
usually stated that it minimizes the instantaneous kinetic energy that the
robot requires to execute the task control. We drop the dependency of
$\mathbf{M}$ on $\mathbf{q}$ in the following for a shorter notation. This is
discussed in literature Dietrich et al. (2015); Chiaverini et al. (2008);
Khatib (1987), however, the mappings in Eqs. (1-3) are not clearly separated,
which is crucial. For velocity control, indeed, $\mathbf{W}=\mathbf{M}$
computes the joint velocities for a given task that minimize the instantaneous
kinetic energy. This directly results from the first-order optimality
conditions of the following optimization problem:

$\displaystyle\\!\min_{\dot{\mathbf{q}}}\quad$
$\displaystyle\frac{1}{2}\dot{\mathbf{q}}^{T}\mathbf{W}\dot{\mathbf{q}}$ (4)
s. t.: $\displaystyle\dot{\mathbf{x}}_{d}=\mathbf{J}\dot{\mathbf{q}}$ (5)

However, we want to emphasize that this does not hold for acceleration
control: Using $\mathbf{W}=\mathbf{M}$ in Eq. (2) tracks the desired task
space command but is not the solution that minimizes the kinetic energy.
Instead, the acceleration control for minimum instantaneous kinetic energy is
derived from Eq. (1):

$\ddot{\mathbf{q}}_{d}=\mathbf{J}^{\\#}_{W}\ddot{\mathbf{x}}_{d}+\dot{\left(\mathbf{J}^{\\#}_{W}\right)}\mathbf{J}\dot{\mathbf{q}}$
(6)

For $\dot{\left(\mathbf{J}^{\\#}_{W}\right)}$, often numerically approximated
gradients are used, e.g. in Nemec (1997). We use the analytical derivative
provided in Guo et al. (2014) and we use the chain rule for the required terms
$\dot{\mathbf{J}}=\frac{\text{d}\mathbf{J}}{\text{d}\mathbf{q}}\dot{\mathbf{q}}$
and
$\dot{\mathbf{W}}=\frac{\text{d}\mathbf{W}}{\text{d}\mathbf{q}}\dot{\mathbf{q}}$.
We use the algorithm in Haviland and Corke (2020) to compute
$\frac{\text{d}\mathbf{J}}{\text{d}q_{i}}$ and we use numeric differentiation
to compute $\frac{\text{d}\mathbf{W}}{\text{d}q_{i}}$ for
$\mathbf{W}=\mathbf{M}$.
Figure 1b shows the effect of the choice of $\mathbf{W}$ during task mapping
on the kinetic energy consumption. We implemented a manipulator with four
links in MATLAB (see Figure 1a). Its single task is to track a given task
space trajectory, which is a two-dimensional point-to-point (PTP) motion with
quintic spline interpolation that enforces zero task space velocities and
accelerations at start and goal. We compare the three approaches in Eqs. (1),
(2) and (6).
The following is obvious: $\mathbf{W}=\mathbf{M}$ minimizes the instantaneous
kinetic energy in velocity control. To achieve the same in acceleration
control, still $\mathbf{W}=\mathbf{M}$ is required, but Eq. (6) must be used
instead of Eq. (2).

###  3.2 Dynamically Consistent Nullspace Projection for Torque Control

Section 3.1 considered the execution of one task. This section considers the
simultaneous execution of several tasks denoted by subscript $k$, and it shows
the impact that $\mathbf{W}$ has on the so-called dynamical consistency
property of a nullspace projection. As outlined in Section 1, the
$\mathcal{C}$-space commands due to the lower-prioritized task $k+1$ are
projected into the nullspace of the higher-prioritized task $k$ with the
nullspace projector $\mathbf{N}_{k+1}$. The projector for torque control
$\mathbf{N}_{tor,k+1}$ is the transposed of the projector for velocity and
acceleration control $\mathbf{N}_{vel,k+1}=\mathbf{N}_{acc,k+1}$:

$\displaystyle\mathbf{N}_{vel,k+1}$ $\displaystyle=\mathbf{N}_{acc,k+1}$
$\displaystyle=\mathbf{I}-\mathbf{J}^{\\#}_{W,k}\mathbf{J}_{k}$ (7)
$\displaystyle\mathbf{N}_{tor,k+1}$ $\displaystyle=\mathbf{N}_{vel,k+1}^{T}$
$\displaystyle=\mathbf{I}-\mathbf{J}^{T}_{k}\left(\mathbf{J}^{\\#}_{W,k}\right)^{T}$
(8)

When choosing $\mathbf{W}=\mathbf{I}$, $\mathbf{N}_{vel}$ is symmetric and
thus $\mathbf{N}_{vel}=\mathbf{N}_{tor}$ holds. However, this is not the case
for $\mathbf{W}=\mathbf{M}$.
Dietrich et al. (2015) discusses the computation of dynamically consistent
nullspace projections for torque control. Dynamically consistent means that a
lower-prioritized task does not generate accelerations in a higher-prioritized
task space. Dietrich et al. (2015) formulates this condition, which we call
DynCon, for torque control, and we add the condition for velocity and
acceleration control:

$\displaystyle\textsc{DynCon}_{tor}:$
$\displaystyle\mathinner{\|\mathbf{J}_{k}\mathbf{M}^{-1}\mathbf{N}_{k+1}\boldsymbol{\tau}_{k+1}\|}_{2}$
$\displaystyle=0$ (9) $\displaystyle\textsc{DynCon}_{vel/acc}:$
$\displaystyle\mathinner{\|\mathbf{J}_{k}\mathbf{N}_{k+1}\mathbf{\ddot{q}}_{k+1}\|}_{2}$
$\displaystyle=0$ (10)

For torque control, Dietrich et al. (2015) proves that selecting
$\mathbf{W}=\mathbf{M}$ for the nullspace projector in Eq. (8) ensures
dynamical consistency while $\mathbf{W}=\mathbf{I}$ does not. Velocity and
acceleration control inherently implement dynamically consistent projections,
i.e. a lower-prioritized task never generates accelerations in a higher-
prioritized task independent from $\mathbf{W}$. The dynamical consistency
property is, therefore, only relevant for torque control. Note that the term
dynamical consistency is also used in velocity and acceleration control when
$\mathbf{W}=\mathbf{M}$ Dietrich et al. (2013). However, in these applications
$\mathbf{W}=\mathbf{I}$ also fulfills Eq. (10).
Figure 1c shows the left side of DynCon in Eqs. (9) and (10). The scenario is
the same as in Section 3.1. However, now, there is a certain secondary task.
Here, we simply assume that this secondary task requests unit commands, i.e.
$\dot{\mathbf{q}}_{d}=\mathbf{I}^{n\times 1}~{}\frac{\text{rad}}{\text{s}}$,
$\ddot{\mathbf{q}}_{d}=\mathbf{I}^{n\times
1}~{}\frac{\text{rad}}{\text{s}^{2}}$, or
$\boldsymbol{\tau}_{d}=\mathbf{I}^{n\times 1}~{}\text{Nm}$. For example, in
velocity control, the secondary task sets a target velocity of 1 rad/s in each
joint, or, in dynamic control, the secondary task sets a target torque of 1 Nm
in each joint. We want to investigate the dynamical consistency, i.e. if these
secondary task controls generate accelerations in the task space of the
primary task.
The following is obvious in Figure 1c: $\mathbf{W}=\mathbf{M}$ ensures a
dynamically consistent projection in torque control, and
$\mathbf{W}=\mathbf{I}$ does not. Projections in velocity and acceleration
control are dynamically consistent independent of $\mathbf{W}$.
Note that in Section 3.1, we defined the 2D translational trajectory
($\mathbf{x}_{d}(t)$,$\dot{\mathbf{x}}_{d}(t)$,$\ddot{\mathbf{x}}_{d}(t)$).
So, using (1), (2), and (6), we could compute the desired joint velocities and
the desired joint acceleration to track the trajectory. To track this
trajectory with dynamic control, i.e. with joint torques
$\boldsymbol{\tau}_{d}=\mathbf{J}^{T}\mathbf{F}_{d}$, we formulate the
dynamics in task space according to Siciliano et al. (2008):

$\displaystyle\overline{\mathbf{M}}$
$\displaystyle=\left(\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{T}\right)^{-1}$ (11)
$\displaystyle\overline{\mathbf{c}}$
$\displaystyle=\overline{\mathbf{M}}\mathbf{J}\mathbf{M}^{-1}\mathbf{C}\dot{\mathbf{q}}-\overline{\mathbf{M}}\dot{\mathbf{J}}\dot{\mathbf{q}}$
(12) $\displaystyle\overline{\mathbf{g}}$
$\displaystyle=\overline{\mathbf{M}}\mathbf{J}\mathbf{M}^{-1}\mathbf{g}$ (13)
$\displaystyle\mathbf{F}_{d}$
$\displaystyle=\overline{\mathbf{M}}\ddot{\mathbf{x}}_{d}+\overline{\mathbf{c}}+\overline{\mathbf{g}}$
(14)

$\mathbf{M}$, $\mathbf{C}$ and $\mathbf{g}$ are the mass matrix, Coriolis and
centrifugal terms, and gravity torques respectively formulated in
$\mathcal{C}$-space. $\overline{\mathbf{M}}$, $\overline{\mathbf{c}}$ and
$\overline{\mathbf{g}}$ are the corresponding terms formulated in task space.

###  3.3 Task Mapping and Nullspace Projection in Velocity Control

Table 1: Capabilities of Eqs. (15) and (20) to minimize kinetic energy consumption and error of secondary task Line | Approach | $\overline{E}_{kin}$ | $\overline{L}$ | $\overline{E}_{kin}+\overline{L}$
---|---|---|---|---
1: | Eq. (20) with $\mathbf{D}=\mathbf{M}$; $\mathbf{E}=\mathbf{I}$ | 104 | 267 | 371
2: | Eq. (20) with $\mathbf{D}=\mathbf{M}$; $\mathbf{E}=\mathbf{M}$ | 99 | 290 | 389
3: | Eq. (20) with $\mathbf{D}=\mathbf{I}$; $\mathbf{E}=\mathbf{I}$ | 187 | 252 | 439
4: | Eq. (20) with $\mathbf{D}=\mathbf{M}$; $\mathbf{E}=\mathbf{0}$ | 84 | 324 | 407
5: | Eq. (20) with $\mathbf{D}=\mathbf{0}$; $\mathbf{E}=\mathbf{I}$ | 177 | 236 | 413
6: | Eq. (25) with $\mathbf{W}=\mathbf{M}$; $\alpha=1.0$ | 116 | 280 | 396
7: | Eq. (25) with $\mathbf{W}=\mathbf{M}$; $\alpha=\frac{2}{3}$ | 99 | 290 | 389
8: | Eq. (25) with $\mathbf{W}=\mathbf{I}$; $\alpha=1.0$ | 177 | 236 | 413
9: | Eq. (25) with $\mathbf{W}=\mathbf{I}$; $\alpha=\frac{2}{3}$ | 186 | 252 | 439
10: | Eq. (25) with $\mathbf{W}_{\text{map}}=\mathbf{M}$; $\mathbf{W}_{\text{proj}}=\mathbf{I}$; $\alpha=1.0$ | 106 | 268 | 375

This section combines task mapping as discussed in Section 3.1 and nullspace
projection as discussed in Section 3.2, and it applies their combination to
velocity control. Our contribution consists in proposing a weighting approach
that computes a dynamically consistent projection, which trades off energy
consumption and tracking error of the lower-prioritized task.
For velocity control and in the context of task mapping, Section 3.1 states
that $\mathbf{W}=\mathbf{M}$ ensures minimum instantaneous kinetic energy. For
velocity control and its nullspace projection, Section 3.2 states that
projections are dynamically consistent independent of $\mathbf{W}$. So,
considering two tasks and combining task mapping and nullspace projection, the
final joint velocity control is:

$\displaystyle\dot{\mathbf{q}}=\underbrace{\mathbf{J}^{\\#}_{W_{\text{map},1}}\dot{\mathbf{x}}_{1}}_{\mathbf{\dot{q}}_{1}}+\left(\mathbf{I}-\mathbf{J}^{\\#}_{W_{\text{proj},1}}\mathbf{J}_{1}\right)\underbrace{\mathbf{J}^{\\#}_{W_{\text{map},2}}\dot{\mathbf{x}}_{2}}_{\mathbf{\dot{q}}_{2}}$
(15)
$\displaystyle\mathbf{W}_{\text{map},1}=\mathbf{W}_{\text{map},2}=\mathbf{M}$
(16) $\displaystyle\mathbf{W}_{\text{proj},1}=\text{arbitrary}$ (17)

That is, the weighting matrices for task mapping $\mathbf{W}_{\text{map}}$ and
nullspace projection $\mathbf{W}_{\text{proj}}$ may be chosen independently.
Equation (15) is a popular version for velocity control of redundant robots in
literature where $\mathbf{W}_{\text{map}}=\mathbf{W}_{\text{proj}}=\mathbf{W}$
is commonly used, i.e. task mapping and nullspace projection use the same
weighting.
For only one task, Eq. (1) minimizes the nonlinear optimization in Eqs. (4-5),
and as outlined, setting $\mathbf{W}=\mathbf{M}$ in Eqs. (1,4,5) minimizes
instantaneous kinetic energy. Similarly, for two tasks, we formulate the
minimization problem:

$\displaystyle\\!\min_{\dot{\mathbf{q}}}\quad$
$\displaystyle\frac{1}{2}\dot{\mathbf{q}}^{T}\mathbf{D}\dot{\mathbf{q}}+\left(\dot{\mathbf{q}}_{2}-\dot{\mathbf{q}}\right)^{T}\mathbf{E}\left(\dot{\mathbf{q}}_{2}-\dot{\mathbf{q}}\right)$
(18) s. t.:
$\displaystyle\dot{\mathbf{x}}_{1}-\mathbf{J}_{1}\dot{\mathbf{q}}=\mathbf{0}$
(19)

That is, we look for a compromise between the $\frac{\mathbf{D}}{2}$-weighted
squared norm of the joint velocities, corresponding to kinetic energy for
$\mathbf{D}=\mathbf{M}$, and the $\mathbf{E}$-weighted tracking error of the
secondary task command. The optimality conditions give:

$\displaystyle\dot{\mathbf{q}}=\mathbf{J}^{\\#}_{W_{1}}\dot{\mathbf{x}}_{1}+\left(\mathbf{I}-\mathbf{J}^{\\#}_{W_{1}}\mathbf{J}_{1}\right)\mathbf{W}_{1}^{-1}2\mathbf{E}\dot{\mathbf{q}}_{2}$
(20) $\displaystyle\mathbf{W}_{1}=\mathbf{D}+2\mathbf{E}$ (21)

In contrast to our solution in Eq. (20), one can show that Eq. (15) with
$\mathbf{W}_{\text{map},1}=\mathbf{W}_{\text{proj},1}=\mathbf{W}_{1}$ solves:

$\displaystyle\\!\min_{\dot{\mathbf{q}}}\quad$
$\displaystyle\left(\dot{\mathbf{q}}_{2}-\dot{\mathbf{q}}\right)^{T}\mathbf{W}_{1}\left(\dot{\mathbf{q}}_{2}-\dot{\mathbf{q}}\right)$
(22) s. t.:
$\displaystyle\dot{\mathbf{x}}_{1}-\mathbf{J}_{1}\dot{\mathbf{q}}=\mathbf{0}$
(23)

Thus, the commonly used Eq. (15) minimizes the $\mathbf{W}_{1}$-weighted
tracking error only and does not formulate a compromise between kinetic energy
and tracking error. However, our solution in Eq. (20) allows to formulate this
compromise.
Note that, in Eq. (20), if $\mathbf{D}$ is a multiple of $\mathbf{E}$, i.e.
$\mathbf{D}=\beta\mathbf{E}$ with $\beta\in\mathbb{R}$, the following holds:

$\dot{\mathbf{q}}=\mathbf{J}^{\\#}_{W_{1}}\dot{\mathbf{x}}_{1}+\frac{2}{\beta+2}\left(\mathbf{I}-\mathbf{J}^{\\#}_{W_{1}}\mathbf{J}_{1}\right)\dot{\mathbf{q}}_{2}$
(24)

This motivates that, in literature, the second term in Eq. (15), i.e. the
nullspace projection, is often applied with an additional scalar $\alpha$
Schuetz et al. (2014):

$\dot{\mathbf{q}}=\mathbf{J}^{\\#}_{W_{\text{map}}}\dot{\mathbf{x}}_{1}+\alpha\left(\mathbf{I}-\mathbf{J}^{\\#}_{W_{\text{proj}}}\mathbf{J}_{1}\right)\dot{\mathbf{q}}_{2}$
(25)

For the case that our solution in Eq. (20) uses $\mathbf{D}=\beta\mathbf{E}$
and $\alpha=\frac{2}{\beta+2}$ in Eq. (25), both approaches are equivalent.
Table 1 benchmarks Eqs. (20) and (25) w.r.t. their capability to minimize
kinetic energy and the error of the secondary task command. We use the robot
shown in Figure 1a. The high-priority task is to track a 2D PTP path in task
space that we interpolate with a quintic polynomial as in Section 3.1. The
low-priority task is to favor a comfort configuration $\mathbf{q}_{cmf}$. This
task is formulated with gradient descent, i.e. we minimize
$L=\kappa\frac{1}{2}\mathinner{\|\mathbf{q}_{cmf}-\mathbf{q}\|}_{2}^{2}$ with
$\kappa=0.1$ by applying the secondary control
$\dot{\mathbf{q}}_{2}=\mathbf{J}^{\\#}_{W_{map}}\left(-\frac{dL}{d\mathbf{q}}\right)=\mathbf{J}^{\\#}_{W_{map}}\kappa\left(\mathbf{q}_{cmf}-\mathbf{q}\right)$
with $\mathbf{J}_{2}=\mathbf{I}$. $\kappa$ scales $L$ w.r.t. $E_{kin}$. For
Eq. (25), except in line 10 of Table 1, we use
$\mathbf{W}_{\text{map}}=\mathbf{W}_{\text{proj}}=\mathbf{W}$. To get a
scenario-independent benchmark, we simulate 100 PTP motions that start at a
random joint configuration and reach a randomly sampled goal pose in the
reachable workspace. Eqs. (20) and (25) are applied to the same 100 simulation
scenarios. Table 1 shows the average kinetic energy $\overline{E}_{kin}$ and
the average secondary objective cost $\overline{L}$ over the 100 simulations.
$E_{kin}$ and $L$ of one simulated motion are computed as the sum of uniformly
distributed time samples over the trajectory. Note that both approaches
minimize the kinetic energy and tracking error only locally. However, for many
trajectory simulations, Table 1 supports our expectation that an approach that
is superior w.r.t. the local optimization capability, in the long run, is also
superior w.r.t. the global optimization capability.
From Table 1, the following is obvious: Eq. (20) with $\mathbf{D}=\mathbf{M}$
and $\mathbf{E}=\mathbf{I}$ minimizes $\overline{E}_{kin}+\overline{L}$ best.
As outlined, the approach in Eq. (25) with $\alpha=1.0$ only considers the
secondary objective, and thus for $\mathbf{W}=\mathbf{I}$ it minimizes
$\overline{L}$ best. This is equivalent to Eq. (20) with
$\mathbf{D}=\mathbf{0}$ and $\mathbf{E}=\mathbf{I}$ (line 5). As stated, Eq.
(20) with $\mathbf{D}=\beta\mathbf{E}$, e.g. $\beta=1.0$ in line 2, is
equivalent to Eq. (25) when $\alpha=\frac{2}{\beta+2}$, e.g.
$\alpha=\frac{2}{3}$ in line 7. With our solution in Eq. (20), one can scale
$\mathbf{D}$ or $\mathbf{E}$ to increase or decrease the importance of
$E_{kin}$ over $L$, and we show the two extreme cases that scale one of the
two to zero (lines 4 and 5). That way, we get the solution with minimum energy
and the solution with minimum tracking error. There is the suspicion that by
using different weighting matrices in Eq. (25) and setting
$\mathbf{W}_{\text{map}}=\mathbf{M}$ and
$\mathbf{W}_{\text{proj}}=\mathbf{I}$, one gets the solution that trade-offs
kinetic energy and tracking error. Table 1 shows that this gets close to the
solution of Eq. (20), but still is slightly worse.
We conclude the following:

  * •

The standard approach in Eq. (15) with
$\mathbf{W}_{\text{map}}=\mathbf{W}_{\text{proj}}=\mathbf{W}$ minimizes the
$\mathbf{W}$-weighted error norm of the secondary task command. Adding
$\alpha$ allows to additionally consider the $\mathbf{W}$-weighted velocity
norm.

  * •

The standard approach in Eq. (15) with
$\mathbf{W}_{\text{map}}\neq\mathbf{W}_{\text{proj}}$ implements a strict
hierarchy but does not allow a physical interpretation.

  * •

Our approach in Eq. (20), in contrast to Eq. (15), allows to consider kinetic
energy and secondary task with differently shaped matrices. For instance,
using $\mathbf{D}=\mathbf{M}$ and $\mathbf{E}=\mathbf{I}$ finds a compromise
between kinetic energy and secondary task tracking.

##  4 Hierarchical Velocity Control

Section 3.3 investigated the combination of task mapping and nullspace
projection for velocity control. This section incorporates both into a generic
formulation for the hierarchical control of joint velocities. Thereby, the
algorithm has to tackle the two challenges outlined in Section 1, i.e.
ensuring smooth control signals during task rearrangement, and avoiding that
lower-prioritized tasks counteract higher-prioritized tasks. Note that the
latter, i.e. a strict hierarchy, is only demanded in periods without task
rearrangement: During the continuous priority rearrangement, the control
computes only soft hierarchies Liu et al. (2015); Dehio and Steil (2019). We
aim for a priority rearrangement based on the importance metric $\eta_{k}$.
Based on the investigations in Section 3.3, we use Eq. (20) with
$\mathbf{D}=\mathbf{M}$ and $\mathbf{E}=\mathbf{I}$ and thus the projected
command of a task $k$ is defined as:

$\dot{\mathbf{q}}_{k}=\left(\mathbf{I}-\mathbf{J}^{\\#}_{M+2I,k}~{}\mathbf{J}_{k}\right)\left(\mathbf{M}+2\mathbf{I}\right)^{-1}2\mathbf{J}^{\\#}_{M+2I,k}\mathbf{u}_{k}$

Our work builds upon Liu et al. (2015) and Dehio and Steil (2019). Liu et al.
(2015) proposes Generalized Hierarchical Control (GHC) as a control algorithm
for multiple hierarchies. GHC solves the two challenges, i.e. smooth signals
and strict hierarchies, only for torque control and further, the derivation of
the projector does not consider $\mathbf{W}$. Thus, GHC can only be used if
$\mathbf{W}=\mathbf{I}$. Dehio and Steil (2019) enhances GHC and proposes
Dynamically Consistent GHC (DynGHC) for torque control. They achieve this by
considering $\mathbf{W}$ in the derivation of the nullspace projector.
Alg. 1 shows our hierarchical control that is based on Liu et al. (2015) and
Dehio and Steil (2019). We refer to them for details, but we briefly outline
each step and we show our modifications in the following.

Algorithm 1 Hierarchical Velocity Control of $N$ Tasks

1:$\mathbf{q},\dot{\mathbf{q}},\mathbf{D},\mathbf{E},\mathbf{W}=\mathbf{D}+2\mathbf{E}$

2:$\mathbf{\dot{q}}_{d}$

3:for $k\in{1,\dots,N}$ do

4:
$[\mathbf{J}_{k},\mathbf{u}_{k},\eta_{k}]=\text{computeTask}(\mathbf{q},\dot{\mathbf{q}})$

5:end for

6:$\mathbf{A}=\text{computeTaskPriorityMatrix}(\eta_{1},\dots,\eta_{N})$

7:$\mathbf{J}_{aug}=\text{computeAugJacobi}(\mathbf{J}_{1},\dots,\mathbf{J}_{N})$

8:for $k\in{1,\dots,N}$ do

9:
$\mathbf{J}_{aug,k}=\text{getTaskAugJacobi}(\mathbf{A}(k,:),\mathbf{J}_{aug})$

10:
$[\mathbf{L},\mathbf{Q},\mathbf{A}_{sr}]=\text{dynGHC}(\mathbf{A}(k,:),\mathbf{J}_{aug,k},\mathbf{W})$

11:
$\mathbf{N}_{kin,k}=\mathbf{L}\left(\mathbf{I}-\mathbf{Q}\mathbf{A}_{sr}^{T}\mathbf{Q}^{T}\right)\mathbf{L}^{-1}$

12:end for

13:for $k\in{1,\dots,N}$ do

14:
$\mathbf{K}=\eta_{k}\mathbf{I}+\left(1-\eta_{k}\right)\left(\mathbf{D}+2\mathbf{E}\right)^{-1}2\mathbf{E}$

15:
$\mathbf{\dot{q}}_{d}\leftarrow\mathbf{\dot{q}}_{d}+\mathbf{N}_{kin,k}\mathbf{K}\mathbf{J}^{\\#}_{W,k}\mathbf{u}_{k}$

16:end for

Table 2: Benchmark of GHC, DynGHC and our approach Line | Approach | $\overline{E}_{kin}$ | $\overline{L}$ | $\overline{E}_{kin}+\overline{L}$
---|---|---|---|---
1: | Ours with $\mathbf{D}=\mathbf{M}$; $\mathbf{E}=\mathbf{I}$ | 104 | 267 | 371
2: | Ours with $\mathbf{D}=\mathbf{M}$; $\mathbf{E}=\mathbf{0}$ | 84 | 324 | 407
3: | Ours with $\mathbf{D}=\mathbf{0}$; $\mathbf{E}=\mathbf{M}$ | 116 | 280 | 396
4: | Ours with $\mathbf{D}=\mathbf{0}$; $\mathbf{E}=\mathbf{I}$ | 177 | 236 | 413
5: |  DynGHC with $\mathbf{W}=\mathbf{M}$ | 116 | 280 | 396
6: |  DynGHC with $\mathbf{W}=\mathbf{I}$ | 177 | 236 | 413
7: |  GHC with $\mathbf{W}=\mathbf{I}$ | 177 | 236 | 413

Line 2 computes our defined tasks according to Definition 1. Line 4 computes
the task priority matrix $\mathbf{A}$ that, given the individual importance
metrics $\eta_{k}$, expresses the priorities among all tasks. This builds upon
Liu et al. (2015), however, they prioritize the tasks based on time, whereas
we prioritize based on importance metrics. Line 5, similar to Dehio and Steil
(2019), computes the augmented Jacobian $\mathbf{J}_{aug}$ according to
Dietrich et al. (2015) that stacks all task Jacobians $\mathbf{J}_{k}$. In
line 6 and the following, we compute the corresponding task projector for each
task. To do so, line 7 first rearranges the task Jacobians in
$\mathbf{J}_{aug}$ according to the hierarchy in $\mathbf{A}$. Line 8 computes
DynGHC as given in Dehio and Steil (2019). We refer to Dehio and Steil (2019)
for details specifying only here that: $\mathbf{L}$ is the lower triangular
matrix of $\mathbf{W}=\mathbf{D}+2\mathbf{E}$ (Dehio and Steil (2019) uses
$\mathbf{W}=\mathbf{M}$ instead) that stems from a Cholesky decomposition;
$\mathbf{Q}$ results from a QR decomposition of
$\mathbf{J}_{aug,k}\mathbf{L}$; and $\mathbf{A}_{sr}$ is a diagonal matrix
with the priorities of task $k$ among each task which are stored in the $k$-th
row of $\mathbf{A}$. Line 9 computes the transposed of the dynamically
consistent nullspace projector. We use the transposed version as, in contrast
to Dehio and Steil (2019), we use velocity control. Line 11 maps the task
control $\mathbf{u}_{k}$ to $\mathcal{C}$-space and projects this command into
the nullspace of the higher-prioritized tasks before adding it to the final
control $\dot{\mathbf{q}}_{d}$. However, in line 13 the factor
$(\mathbf{D}+2\mathbf{E})^{-1}2\mathbf{E}$ is not directly applied to the
control. Instead, line 12 defines a linear interpolation between this factor
and identity: Equation (20) was derived for a two-level hierarchy in which
only the command for the low priority task is scaled with
$(\mathbf{D}+2\mathbf{E})^{-1}2\mathbf{E}$. Therefore, we remove that scaling
for high-priority tasks for which $\eta_{k}=1.0$ holds. This also ensures
smooth control signals during task reprioritization.
Table 2 benchmarks our approach in Alg. 1 with GHC Liu et al. (2015) and
DynGHC Dehio and Steil (2019). As we use velocity control, we use the
transposed projectors in our formulations of GHC and DynGHC. We use the same
simulation scenario as for Table 1 with the same 100 random PTP motions.
Again, the motion task is of higher priority than keeping the comfort
configuration. Later, we will introduce dynamic hierarchies.
The following is obvious: Our approach in Alg. 1 with $\mathbf{D}=\mathbf{M}$
and $\mathbf{E}=\mathbf{I}$ minimizes $\overline{E}_{kin}+\overline{L}$ best.
Using $\mathbf{D}=\mathbf{0}$, it is equivalent to DynGHC. As expected, DynGHC
with $\mathbf{W}=\mathbf{I}$ is equivalent to GHC. Note that the results of
Table 2, computed with Alg. 1, correspond to the results of Table 1, computed
with the analytical formula of Section 3.3: For instance, line 1 of both
tables have the same settings and also the results are the same as we applied
the same 100 random motions to both. This proves that our implementation in
Alg. 1 is correct.

###  4.1 Stack of Tasks

So far, tracking a precomputed trajectory and keeping a comfort configuration
were used for demonstration in 2D. We use the following four tasks in our
experiments on real robots, that are implemented with potential fields as
introduced in Khatib (1986) for obstacle avoidance.

####  4.1.1 Motion

For hierarchical control, offline computed trajectories, such as the quintic
interpolation in Figure 1, are not suitable: Due to task rearrangements, the
trajectory can not be tracked at all times with the highest priority, and
thus, the robot motion deviates from the offline computed trajectory.
Therefore, we use an online trajectory generation approach that is based on
potential fields. At any time, the desired end effector velocity
$\dot{\mathbf{x}}_{motion}$ is heading towards the attracting goal pose. So,
the direction of $\dot{\mathbf{x}}_{motion}$ is
$\frac{\mathbf{x}_{goal}-\mathbf{x}}{\mathinner{\|\mathbf{x}_{goal}-\mathbf{x}\|}_{2}}$,
and its magnitude is the predefined maximum task space velocity
$\dot{x}_{max}$. In practice, we have to scale down $\dot{x}_{max}$ at the
beginning of the application to avoid a discontinuous velocity jump from the
rest position. Therefore, we introduce the scaling $\gamma(d)$. It scales
$\dot{x}_{max}$ to zero when the robot’s distance $d$ to the start or goal of
the trajectory is within the breaking distance $d_{break}$ (else
$\gamma=1.0$). That way, there are no discontinuities in the desired velocity
profile $\dot{\mathbf{x}}_{motion}$:

$\displaystyle\dot{\mathbf{x}}_{motion}$
$\displaystyle=\gamma(d)~{}\dot{x}_{max}\frac{\mathbf{x}_{goal}-\mathbf{x}}{\mathinner{\|\mathbf{x}_{goal}-\mathbf{x}\|}_{2}}$
(26) $\displaystyle\gamma(d)$
$\displaystyle=\frac{1}{2}\left(1-\text{cos}\left(\pi\frac{d}{d_{break}}\right)\right)$
(27)

At any time, we set $\eta_{motion}=1.0$ because reaching the goal is of high
importance independent of the robot state. $\mathbf{J}_{motion}$ is the end
effector Jacobian. Motion is a six-dimensional task.

####  4.1.2 Joint Limit Avoidance (Jla)

Jla pushes the joints away from their limits. The formulation of
$\dot{\mathbf{x}}_{Jla}\in\mathbb{R}^{n}$ is straightforward and given in
Wittmann et al. (2022a). This task gets activated for joints that are within
an activation threshold to their limits. $\mathbf{J}_{Jla}$ is a diagonal
matrix with 1.0 for joints within the threshold and 0.0 else. $\eta_{Jla}$
linearly increases from 0.0 to 1.0 within the threshold, i.e. $Jla$ gets more
important the closer to the limits.

####  4.1.3 Collision Avoidance (Coll)

Coll pushes the closest point on the robot to the environment/to the other
robot away from the approaching collision.
$\dot{\mathbf{x}}_{Coll}\in\mathbb{R}^{3}$ and $\mathbf{J}_{Coll}$ is the
translational Jacobian of the closest point on the robot. Wittmann et al.
(2022b) gives implementation details. Similar to Jla, Coll increases linearly
within an activation distance.

####  4.1.4 Manipulability (Mnp)

Mnp is based on the manipulability measure in Yoshikawa (1985) and maximizes
$l_{Mnp}=\sqrt{\text{det}(\mathbf{J}\mathbf{J}^{T})}$. We use
$\frac{\text{d}l_{Mnp}}{\text{d}\mathbf{q}}$, derived in Baur et al. (2012),
as the desired task space velocity $\dot{\mathbf{x}}_{Mnp}\in\mathbb{R}^{n}$
and we set $\mathbf{J}_{Mnp}=\mathbf{I}^{n\times n}$ to equally use all joints
for this task. Mnp defines an activation threshold $l_{Mnp}=0.08$ from which
$\eta_{Mnp}$ increases linearly to 1.0 at $l_{Mnp}=0.04$.

Similar to Motion, the resulting control commands of Jla, Coll and Mnp are
scaled according to Eq. (27) to achieve zero-clamped velocities at the start
and goal of a motion.

###  4.2 Metric-based Continuous Task Prioritization

During motion, depending on the current robot and environment state, the
priorities of tasks have to change for robust operation. For instance, when
the robot is in the broader area of obstacles but not too close, Coll is
active but still of lower priority than reaching a defined goal. The closer
the robot gets to obstacles, the higher the priority of Coll over Motion.
Similarly, when not too close to the joint limits, Jla might only be active in
the nullspace of Motion, but at some point, Jla becomes of higher priority.
Liu et al. (2015) formalizes this idea with the task prioritization matrix
$\mathbf{A}$ that is also used in Dehio and Steil (2019) and in our case
$\mathbf{A}\in\mathbb{R}^{4\times 4}$, as we define four tasks. The elements
in $\mathbf{A}$ take values in the range [0.0, 1.0] and define the priorities
among the tasks, e.g. $a_{ij}$ defines the priority of task $j$ over task $i$.
Liu et al. (2015) and Dehio and Steil (2019) change these priorities with
time, i.e. $a_{ij}(t)$, whereas we change the priorities depending on the
importance metrics, i.e. $a_{ij}(\eta)$:

$\mathbf{A}=\begin{pmatrix}0&1-\eta_{Coll}&1-\eta_{Coll}&1-\eta_{Coll}\\\
\eta_{Coll}&0&1-\eta_{Jla}&1-\eta_{Jla}\\\
\eta_{Coll}&\eta_{Jla}&0&1-\eta_{Motion}\\\
\eta_{Coll}&\eta_{Jla}&\eta_{Motion}&0\end{pmatrix}$ (28)

The diagonal entries are zero. Otherwise, a task would be projected into its
own nullspace, resulting in a deactivated task. Further, $a_{ij}=1-a_{ji}$
gives a proper priorization among two tasks.
We choose the following order of the rows/columns of $\mathbf{A}$: Coll in
first row; Jla in second row; Motion in third row; Mnp in fourth row. This
order of the rows already defines one aspect of the prioritization: Let us
assume that all $\eta$ are 1.0, i.e. all tasks are fully active. In this case,
the task in the first row (here Coll) will use the full available space
because its command will not be projected in any of the nullspace of the tasks
of lower priority. This information is stored in $a_{12},a_{13},a_{14}$.
Similarly, in this case, that all tasks are fully active, the second row of
$\mathbf{A}$ indicates that the task of the second row (here Jla) will be
fully projected in the nullspace of the first task (here Coll), which is
indicated by $a_{21}$. But it will not be projected into the nullspace of its
lower tasks (here Motion and Mnp), as indicated by $a_{23},a_{24}$. This
approach avoids meaningless prioritization, which is outlined in Dehio and
Steil (2019) as an open topic: In contrast to the approach in Liu et al.
(2015); Dehio and Steil (2019), Eq. (28) avoids meaningless prioritizations
like $Coll>Motion>Jla>Coll$.

##  5 Results

(a) Dual-Arm Test Scenario

(b) SSV Collision Model

Figure 2: Experimental validation scenario. In Figure 2b, the first robot is
on the left and the second robot is on the right. The blue lines show the
potential field-based PTP motion of the two end effectors if there were no
other tasks. The red line shows the closest distance between a robot and
another robot/the environment

We implemented our proposed approach in the C++ framework that controls our
two Franka Emika robots shown in Figure 2. The robots have seven DoF each. We
use the joint velocity control interface that expects continuous control
commands. In the evaluation scenario, both robots move from an initial
configuration to a Cartesian goal pose. The accompanying video shows the
experiment and is available at: https://youtu.be/uWFl824h1Po. During the
motion, the robots get close to each other such that Coll is activated, and
they get close to their joint limits such that Jla is activated. Mnp tries to
keep an advantageous configuration in motion segments of low manipulability.

Figure 3: Dotted and solid lines show the velocity control commands for joint
1 and 4, respectively. Green lines show the unfiltered commands, blue lines
show the filtered commands, and orange lines show the measured velocities. The
pink line shows the discontinuous control, expemplarily for joint 4, that
would result from an approach with discrete task prioritization

(a) Importance metrics $\eta$ (b) $a_{ij}$ for $i\equiv\text{{Motion}}$. Pink
and blue lines overlap (c) Computed Motion command according to Eq. (1) and
projected Motion command after nullspace projection according to line 13 in
Alg. 1 for joints 1 (solid) and 4 (dashed) (d) Resulting command and projected
task contributions for joint 1. The black line is the global command that
corresponds to the dotted line in Figure 3

Figure 4: Task (de)activation throughout the motion of the experiment
available at https://youtu.be/uWFl824h1Po

For the first robot, Figure 3 shows the resulting control command
$\dot{\mathbf{q}}_{d}$, exemplarily for joint 1 (dotted) and 4 (solid), i.e.
$\dot{\mathbf{q}}_{d}$ includes all projected tasks according to line 13 in
Alg. 1. Our computed control in green is continuous with close-to-zero
velocities at start and goal. The velocities are not exactly zero at start
because we allow a small velocity such that the robot starts moving. The
control interface of the robots expects continuous signals also on
acceleration level. Therefore, the computed $\mathcal{C}^{0}$-continuous
velocity commands are filtered with a PT1, however, Figure 3 shows that the
filtering changes the values only slightly, i.e. the filtered values in blue
are close to the unfiltered values in green. That is, our filter is not used
to smooth a discontinuous velocity signal. Such a discontinuous signal would
result from an approach that does not change the priorities based on
continuously evolving importance metrics but that changes the priorities
abruptly. The pink line in Figure 3 shows this for joint 4, with the same
application scenario as for the experiments. Note that the abrupt change of
priorities also leads to instabilities in task rearrangement, i.e. frequent
changes. The control interface of our robots does not accept such a
discontinuous signal, so in the accompanying video, we show this case only in
simulation but for the same application as in the experiments.
For the first robot, Figure 4 shows the effect of task (de)activation
throughout the robot motion in the experiment. Figure 4a shows that while the
robot approaches the other robot, Coll gets more important because the
distance decreases, i.e. $\eta_{Coll}$ increases. Also, for Jla and Mnp, the
priorities increase as the robot gets closer to the joint limits and
configurations of low manipulability. As explained earlier,
$\eta_{Motion}=1.0$ throughout the motion. Depending on the evolution of
$\eta_{k}$, the priorities in $\mathbf{A}$ change according to Eq. (28).
Figure 4b shows the row in $\mathbf{A}$ that corresponds to Motion, i.e. it
shows the priorities of other tasks over Motion. As outlined, the diagonal
terms of $\mathbf{A}$ are 0.0, thus, $a_{33}=0.0$ (blue line in Figure 4b). As
further outlined, the order of the rows of $\mathbf{A}$ already defines one
aspect of the prioritization: The priority of Coll and Jla over Motion change
according to their $\eta_{k}$. In contrast, Mnp is in a higher-indexed row in
$\mathbf{A}$ than Motion, and thus its priority over Motion is defined by
$\eta_{Motion}$ and not by $\eta_{Mnp}$ (see Section 4.2). $\eta_{Motion}$ is
set to 1.0 throughout the whole motion and thus the pink line is also at 0.0.
For joints 1 (solid) and 4 (dashed) of the first robot, Figure 4c shows that,
after other higher-prioritized tasks are activated, only a part of the desired
computed Motion command (orange line) gets propagated to the final joint
control command (blue line).
For joint 1 of the first robot, Figure 4d shows how much each task contributes
to the final joint velocity control command. When the robot is still far from
obstacles, Motion is the only active task: For this period, the final control
command (black line) overlaps with the Motion command (blue line). As soon as
Coll gets activated, Motion becomes less important, and the final control
command starts to deviate. Similar observations can be made for Jla and Mnp.
The experiment is a contribution on its own: The control of several tasks is
computationally expensive. Liu et al. (2015) and Dehio and Steil (2019) show
GHC and DynGHC only in simulation. However, both outline in their conclusions
that future work should include ”the reduction of the computational cost of
GHC to achieve real-time control of complex robots” Liu et al. (2015) and that
future work ”is necessary to evaluate the approach on real robots for useful
multi-task applications” Dehio and Steil (2019). We achieve experiments on
hardware though our four defined tasks include two computationally expensive
ones: Coll requires distance computations that often make up 90 % of the
computation in motion planning, and Mnp includes, as outlined, the elaborate
computation of the gradient according to Baur et al. (2012). Further, Alg. 1
itself is computationally expensive in real-time control, e.g. it includes the
high-dimensional augmented Jacobian $\mathbf{J}_{aug}$ that stacks all task
Jacobians. To achieve an efficient computation of Coll, we model the robot and
the environment with swept sphere volumes (SSV), i.e. simplified geometries
that provide fast distance computation (see Figure 2b). For this, we use the
open-source broccoli C++ library Seiwald and Sygulla (2022). With this
implementation, we can compute Alg. 1 at 1 kHz, which is the real-time control
of the robots’ control interface.

##  6 Conclusions

This work contributes to hierarchical control. First, Section 3 reviews
weighted pseudo-inverses of Jacobians. The discussion itself is not novel,
however, its comprehensiveness is. We clearly distinguish control methods
(velocity, acceleration, and torque control), task mapping, and nullspace
projection. The conclusion of the first part is: The standard approach in Eq.
(15) only minimizes the tracking error of the secondary task. It can be
enhanced with a scalar parameter to also consider velocity-squared norms.
However, in the latter case, both weighting matrices must be equally shaped.
Our approach allows to consider kinetic energy and task tracking error, i.e.
it supports differently shaped weighting matrices. Second, Section 4 presents
hierarchical velocity control. The algorithm for velocity-based control and
applying importance metrics for task rearrangement are novel. We demonstrate
the real-time capability of our formulation in experiments, which is also not
achieved in Liu et al. (2015) and Dehio and Steil (2019).
Our approach still suffers from local minima, which is a well-known problem in
potential field-based approaches. Proposed solutions are the definition of
potential fields with no local minima, e.g. the Brushfire Algorithm Choset et
al. (2005), or random walks to get out of local minima. Further, given several
task commands, our algorithm computes $\mathcal{C}^{0}$-continuous velocity
commands. However, this is only guaranteed if the individual tasks also
compute $\mathcal{C}^{0}$-continuous commands. For instance, if the closest
collision pair in Coll abruptly changes, the task computes a discontinuous
desired velocity that also propagates to the final command. Filtering
techniques may overcome this issue. Finally, in Section 3.3, we derive an
approach for a hierarchical control that allows to trade off energy
consumption and tracking error. We implement this approach into the generic
formulation of Alg. 1, and we prove the correct formulation by comparing the
simulation result of Alg. 1 with a simulation based on an analytical closed-
form solution. However, we do so only for two priority levels. In applications
with more than two active tasks, we do not have a reference to verify the
implementation.
In this work, we proposed our hierarchical control approach, benchmarked its
characteristics against other methods in 100 simulation scenarios, and
validated its real-time capability in an experiment.

## Statements and Declarations

Funding - The authors declare that no funds, grants, or other support were
received during the preparation of this manuscript.
Competing Interests - The authors have no relevant financial or non-financial
interests to disclose.
Author Contributions - Jonas Wittmann: Conceptualization, Methodology,
Software, Validation, Formal Analysis, Investigation, Data Curation, Writing -
Original Draft, Writing - Review and Editing, Visualization, Supervision.
Daniel Hornung: Conceptualization, Methodology, Software, Validation,
Investigation, Data Curation, Writing - Review and Editing. Korbinian
Griesbauer: Methodology, Writing - Review and Editing. Daniel Rixen:
Conceptualization, Resources, Writing - Review & Editing, Supervision, Project
Administration, Funding Acquisition.
Ethics approval - Not applicable.
Consent to participate - Not applicable.
Consent to publish - Not applicable.

## References

  *   * Chiaverini et al. (2008) Chiaverini, S., Oriolo, G., Walker, I.D.: Kinematically Redundant Manipulators, pp. 245–268. Springer, Berlin, Heidelberg (2008). https://doi.org/10.1007/978-3-540-30301-5_12 . https://doi.org/10.1007/978-3-540-30301-5_12
  * Dietrich et al. (2015) Dietrich, A., Ott, C., Albu-Schäffer, A.: An overview of null space projections for redundant, torque controlled robots. International Journal of Robotics Research 34(11), 1385–1400 (2015)
  * Liu et al. (2015) Liu, M., Tan, Y., Padois, V.: Generalized hierarchical control. Autonomous Robots 40(1), 17–31 (2015)
  * Dehio and Steil (2019) Dehio, N., Steil, J.J.: Dynamically-consistent generalized hierarchical control. In: 2019 International Conference on Robotics and Automation (ICRA), pp. 1141–1147 (2019). https://doi.org/%****␣main.bbl␣Line␣100␣****10.1109/ICRA.2019.8793553
  * Siciliano and Slotine (1991) Siciliano, B., Slotine, J.-J.E.: A general framework for managing multiple tasks in highly redundant robotic systems. In: Fifth International Conference on Advanced Robotics ’Robots in Unstructured Environments, pp. 1211–12162 (1991)
  * Mansard and Chaumette (2007) Mansard, N., Chaumette, F.: Task sequencing for high-level sensor-based control. IEEE Transactions on Robotics 23(1), 60–72 (2007) https://doi.org/10.1109/TRO.2006.889487
  * Salini et al. (2011) Salini, J., Padois, V., Bidaud, P.: Synthesis of complex humanoid whole-body behavior: A focus on sequencing and tasks transitions. In: 2011 IEEE International Conference on Robotics and Automation, pp. 1283–1290 (2011). https://doi.org/10.1109/ICRA.2011.5980202
  * Dehio (2018) Dehio, N.J.: Prioritized multi-objective robot control. PhD thesis, TU Braunschweig (Dec 2018). https://doi.org/10.24355/dbbs.084-201812051220-0
  * Dietrich et al. (2012) Dietrich, A., Albu-Schäffer, A., Hirzinger, G.: On continuous null space projections for torque-based, hierarchical, multi-objective manipulation. In: 2012 IEEE International Conference on Robotics and Automation, pp. 2978–2985 (2012). https://doi.org/10.1109/ICRA.2012.6224571
  * Kim et al. (2019) Kim, S., Jang, K., Park, S., Lee, Y., Lee, S.Y., Park, J.: Continuous task transition approach for robot controller based on hierarchical quadratic programming. IEEE Robotics and Automation Letters 4(2), 1603–1610 (2019)
  * Tarbouriech et al. (2018) Tarbouriech, S., Navarro, B., Fraisse, P., Crosnier, A., Cherubini, A., Sallé, D.: Dual-arm relative tasks performance using sparse kinematic control. In: 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 6003–6009 (2018). IEEE
  * Hu et al. (2015) Hu, Y., Huang, B., Yang, G.-Z.: Task-priority redundancy resolution for co-operative control under task conflicts and joint constraints. In: 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2398–2405 (2015). IEEE
  * Whitney (1969) Whitney, D.E.: Resolved motion rate control of manipulators and human prostheses. IEEE Transactions on Man-Machine Systems 10(2), 47–53 (1969) https://doi.org/10.1109/TMMS.1969.299896
  * Khatib (1987) Khatib, O.: A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE Journal on Robotics and Automation 3(1), 43–53 (1987) https://doi.org/10.1109/JRA.1987.1087068
  * Nemec (1997) Nemec, B.: Pseudoinverses and null space velocity controller. In: Proceedings of IEEE International Conference on Intelligent Engineering Systems, pp. 107–111 (1997). https://doi.org/10.1109/INES.1997.632401
  * Guo et al. (2014) Guo, D., Zhai, K., Xiao, Z., Tan, H., Zhang, Y.: Acceleration-level minimum kinetic energy (mke) scheme derived via ma equivalence for motion planning of redundant robot manipulators. In: 2014 Seventh International Symposium on Computational Intelligence and Design, vol. 1, pp. 26–30 (2014). https://doi.org/10.1109/ISCID.2014.65
  * Haviland and Corke (2020) Haviland, J., Corke, P.: A systematic approach to computing the manipulator jacobian and hessian using the elementary transform sequence. CoRR abs/2010.08696 (2020) 2010.08696
  * Dietrich et al. (2013) Dietrich, A., Ott, C., Albu-Schäffer, A.: Multi-objective compliance control of redundant manipulators: Hierarchy, control, and stability. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3043–3050 (2013). https://doi.org/10.1109/IROS.2013.6696787
  * Siciliano et al. (2008) Siciliano, B., Sciavicco, L., Villani, L., Oriolo, G.: Robotics: Modelling, Planning and Control, 1st edn. Springer, ??? (2008)
  * Schuetz et al. (2014) Schuetz, C., Buschmann, T., Baur, J., Pfaff, J., Ulbrich, H.: Predictive online inverse kinematics for redundant manipulators. In: 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 5056–5061 (2014). https://doi.org/10.1109/ICRA.2014.6907600
  * Khatib (1986) Khatib, O.: Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research 5(1), 90–98 (1986) https://doi.org/10.1177/027836498600500106 https://doi.org/10.1177/027836498600500106
  * Wittmann et al. (2022a) Wittmann, J., Kist, A., Rixen, D.J.: Real-time predictive kinematics control of redundancy: a benchmark of optimal control approaches. In: 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 11759–11766 (2022). https://doi.org/10.1109/IROS47612.2022.9981675
  * Wittmann et al. (2022b) Wittmann, J., Klein, C., Rixen, D.: Path quality improvement of sampling-based planners: An efficient optimization-based approach using analytical gradients. In: Kecskeméthy, A., Parenti-Castelli, V. (eds.) ROMANSY 24 - Robot Design, Dynamics and Control, pp. 182–190. Springer, Cham (2022)
  * Yoshikawa (1985) Yoshikawa, T.: Manipulability of robotic mechanisms. The International Journal of Robotics Research 4(2), 3–9 (1985) https://doi.org/10.1177/027836498500400201 https://doi.org/10.1177/027836498500400201
  * Baur et al. (2012) Baur, J., Pfaff, J., Ulbrich, H., Villgrattner, T.: Design and development of a redundant modular multipurpose agricultural manipulator. In: 2012 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), pp. 823–830 (2012). https://doi.org/10.1109/AIM.2012.6265928
  * Seiwald and Sygulla (2022) Seiwald, P., Sygulla, F.: broccoli: Beautiful Robot C++ Code Library (2022). https://doi.org/10.14459/2022mp1686390 . https://gitlab.lrz.de/AM/broccoli
  * Choset et al. (2005) Choset, H., Lynch, K., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L., Thrun, S.: Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, ??? (2005)


# Learning Symbolic Representations for Reinforcement Learning of Non-
Markovian Behavior

Phillip J.K. Christoffersen, Andrew C. Li, Rodrigo Toro Icarte, Sheila A.
McIlraith
University of Toronto & Vector Institute for Artificial Intelligence, Toronto,
Canada
<EMAIL_ADDRESS>
###### Abstract

Many real-world reinforcement learning (RL) problems necessitate learning
complex, temporally extended behavior that may only receive reward signal when
the behavior is completed. If the reward-worthy behavior is known, it can be
specified in terms of a non-Markovian reward function—a function that depends
on aspects of the state-action history, rather than just the current state and
action. Such reward functions yield sparse rewards, necessitating an
inordinate number of experiences to find a policy that captures the reward-
worthy pattern of behavior. Recent work has leveraged Knowledge Representation
(KR) to provide a symbolic abstraction of aspects of the state that summarize
reward-relevant properties of the state-action history and support learning a
Markovian decomposition of the problem in terms of an automaton over the KR.
Providing such a decomposition has been shown to vastly improve learning
rates, especially when coupled with algorithms that exploit automaton
structure. Nevertheless, such techniques rely on _a priori_ knowledge of the
KR. In this work, we explore how to automatically _discover_ useful state
abstractions that support learning automata over the state-action history. The
result is an end-to-end algorithm that can learn optimal policies with
significantly fewer environment samples than state-of-the-art RL on simple
non-Markovian domains.

##  1 Introduction

Deep RL has shown promise at learning complex behavior in many settings,
including game-playing [1], robotics [2], and control systems [3]. These
algorithms typically take advantage of a Markov assumption — that it is enough
to consider only the current state when deciding which action to take.
However, many real-world tasks are inherently temporally extended. The pattern
of behavior the RL agent must learn depends not only upon the current state
but also on past states and actions. For example, an agent that needs to get
through a locked door to get high reward must have previously located and
acquired the key. Unfortunately, learning such temporally extended behavior
can be incredibly challenging since the agent must learn to discern relevant
features from its state-action history; these can be arbitrarily far removed
from the present state, and may depend on this history in complex ways and
without intermediate reward signal to aid learning. The standard deep RL
solution to learning such temporally extended behavior is to use a recurrent
neural network (RNN), which learns an abstract hidden state in order to
summarize environment histories, but RNNs require much data to train, and are
difficult to tune.

By contrast, in recent work, an algorithm for learning temporally extended
behavior is proposed, where the RNN hidden states are replaced with an
augmentation of the current state in terms of hand-designed propositional
symbols, which incorporate domain knowledge and point the agent towards
potentially reward-relevant properties of the state-action history (e.g. [4,
5, 6, 7]). In the previous example, one can augment the agent with the
propositional symbol have_keys, indicating whether the agent has acquired the
keys to the door in the past. Markovian policies on this state space now
become vastly more expressive: if one can additionally condition on the truth
state of have_keys when deciding an action, we can perform the following
temporally extended behavior: go towards the keys when have_keys is false,
then go towards the door when have_keys is true. But how did we know to
augment the agent with have_keys in the first place? While this example seems
simple, this is only because we contrived the reward: it is not in general
clear which propositional symbols to augment an agent with, in order to
achieve high performance. To address this, we propose the use of automata
learning within the RL framework to _automatically_ yield such propositional
symbols, rather than relying on domain knowledge. We demonstrate that the
trained automata dramatically accelerate policy learning, with our end-to-end
approach outperforming a state-of-the-art RL algorithm (Recurrent-PPO) on
several non-Markovian reward domains.

##  2 Related Work

Recently, the idea of specifying non-Markovian reward functions in RL via
formal languages such as Linear Temporal Logic (LTL) (e.g., ( [8, 9, 10, 11,
12, 13, 5, 14]) or automata (e.g., [7, 4, 15, 16]) has garnered significant
attention. While these approaches rely on domain knowledge and a domain-
specific vocabulary for specification of the reward function, we consider a
black-box non-Markovian reward and present an automated approach to uncover
the reward structure. In this approach, we train automata offline using a
reward-prediction heuristic, and augment the environment states with the
states of the learned automaton, as opposed to hand-designed features. An
alternate black-box approach to ours is to first train an RNN, with a standard
deep (recurrent) RL algorithm, and then "quantize" the hidden state of the
RNN, but this learned transition model is not a direct function of the state-
action history [17].

Previous work by Toro Icarte et al. [4] and Xu et al. [18] share many of the
motivations of our work but perform poorly in noisy environments. The work
most similar to ours is by Gaon & Brafman [6], which we build on in several
key ways. First, the (off-the-shelf) automata-learning approaches they employ
are sensitive to noisy data and often learn large, sample-sensitive automata
even when the reward structure is simple. We make use of recent advances in
automata-learning which are robust to noise and regularize the size of the
automaton, which we demonstrate in Section 5. Furthermore, [6] lacked
experimental comparisons against state-of-the-art RL. In our experiments with
non-Markovian goals, we outperform state-of-the-art RL based on RNNs.

##  3 Preliminaries

An MDP is a tuple $\mathcal{M}=(S,A,P,R,\gamma)$, where $S$ is a set of
states, $A$ a set of actions, $P:S\times A\times S\to[0,1]$ the state-action
transition function, $R:S\to\mathbb{R}$ the reward, and $0\leq\gamma\leq 1$
the discounting factor [19]. In such a setup, the reward $R$ is considered
Markovian, due to its dependence only on the most recent state. We will
consider the following extension of the MDP: an NMRDP [20] (non-Markovian
Reward Decision Process) $\mathcal{N}=(S,A,P,R,\gamma)$ is as before, but
where the reward $R:\mathcal{H}\to\mathbb{R}$, where $\mathcal{H}=(S\times
A)^{*}$ is the set of finite histories with states $S$ and actions $A$: in
other words, the agent can be rewarded for behavior which is arbitrarily far
removed from its current experience. Further, we define a proposition as a
function $P:\mathcal{H}\to\\{\mathrm{True},\mathrm{False}\\}$, in an NMRDP.
Intuitively, propositions correspond to facts about the state-action history
in a given episode, such as "the agent has at some point reached the top right
corner" or "within the 3 most recent timesteps, the agent took action x".
While the number of possible propositions grows double-exponentially in the
length of the episode, domain knowledge is often used to specify a relevant
set of such propositions, under which the reward is Markovian. The RL domains
we consider have non-Markovian goals, i.e. there exists
$\mathcal{G}\subset\mathcal{H}$ where $R(g)=1$ for $g\in\mathcal{G}$, and
$R(h)=0$ for $h\in\mathcal{H}-\mathcal{G}$. Intuitively, we want to create a
policy which makes the agent attain a goal history as soon as possible.

##  4 Algorithm

We use the algorithm described in Algorithm 1 named AutRL. Following each
period of Markovian learning, the sampled traces are used to train (offline) a
deterministic finite automaton (DFA) $M$ to predict whether a given sequence
achieves reward $0$ or $1$. We leverage the DFA-learning approach from [21]
due to its efficiency, its propensity to learn small DFAs with few
transitions, and its robustness to noise. Tabular $Q$-learning is used for
markov_learn. Intuitively, a DFA with state space $Q$ that accurately
discriminates reward $1$ traces from reward $0$ traces (which we define as
_consistent_) must model all parts of the state-action history relevant to the
goal, and therefore the augmented state space $S\times Q$ must make the
problem Markovian. We remark that the resultant DFAs are functions
$\mathcal{H}\rightarrow Q$ and are learned end-to-end without domain
knowledge. Examples of this can be seen in Section 5. We also provide a
convergence guarantee for this algorithm in Appendix A.

1 dfa $\leftarrow$ empty_automata;

2 $\pi$ $\leftarrow$ uniform_random_policy;

3 traces $\leftarrow\emptyset$ ;

4 while  _true_ do

5 sample_traces $\leftarrow$ sample($\pi$, N) ;

6 append traces with sample_traces;

7 if  __sample_traces_ inconsistent with _dfa__ then

8 dfa $\leftarrow$ aut_learn(traces);

9

10 end if

11 $\pi$ $\leftarrow$ markov_learn(sample_traces $\times$ dfa);

12

13 end while

Algorithm 1 AutRL

We note that our implementation using $Q$-learning converges to an optimal
policy as the number of environment samples approaches infinity (assuming
$\mathcal{G}$ is regular, as above) due to the optimal convergence guarantees
of $Q$-learning on MDPs [19]). Note that while we search for consistent DFAs,
this condition is not necessary to make the learning problem Markovian. For
this reason, we relax the inconsistency condition analyzed above, replacing
the DFA only under weak performance (i.e. low average reward) at the end of a
given epoch of markov_learn.

##  5 Experimental Results

The purpose of our experiments was to evaluate our AutRL algorithm, which
leverages a learned symbolic representation, relative to a state-of-the-art
RNN-based deep RL algorithm. The two metrics were the quality of the policies
in terms of maximizing reward, and their sample efficiency. We tested on four
non-Markovian domains, similar to those used in the experiments of [6] as
follows.

Multi-Armed Bandit: A single-state environment with two actions
($\mathrm{left},\mathrm{right}$) and episodes of length $6$. A reward of 1 is
obtained only if the 6 actions performed are precisely
$\mathrm{left},\mathrm{right},\mathrm{right},\mathrm{left},\mathrm{right},\mathrm{left}$
in that order.

Hallway: A $1\times 10$ grid, aligned left-to-right, with actions
$\mathrm{left},\mathrm{right}$, and with episodes of length 30. The agent
spawns at a random location on the left half of the grid and must first reach
the right-most square, and then reach the left-most square to obtain a reward
of 1. Before reaching the right-most square, the optimal action in every state
is $\mathrm{right}$, but after reaching the right-most square, the optimal
action becomes $\mathrm{left}$ in every state.

Gridworld: A $5\times 5$ gridworld (with $x,y$-coordinates from $0$ to $4$)
with an episode length of 20 and actions
$\mathrm{up},\mathrm{down},\mathrm{left},\mathrm{right}$. The agent must first
reach $(4,0)$ and then the opposite corner $(0,4)$ to collect a reward of $1$.
We also tested a noisy version of this environment where the reward was
randomly withheld in $10\%$ of successful traces, and actions had a random
outcome $10\%$ of the time.

We compared AutRL against Recurrent-PPO ([22]) (with an LSTM policy) using the
OpenAI Baselines implementation [23]. As shown in all of the below
environments, AutRL converges to high reward policies in fewer environment
samples than Recurrent-PPO does. Notice that AutRL, despite being automata-
based, outperforms Recurrent-PPO even on the Stochastic Gridworld, showing
that AutRL is robust to noisy environments. In Figure 2 are two such examples
of learned automata on two different runs of the Hallway environment.

Legend: |  PPO + LSTM |  AutRL + Q-Learning (ours)
---|---|---

Figure 1: The results from the conducted experiments. The error bars are 95%
confidence intervals over 30 runs. AutRL learns a superior policy to the LSTM-
based PPO in a way that is, at most, over order of magnitude more sample-
efficient. Learning is far more stable across runs with AutRL: the error bars
for AutRL are far tighter than those for PPO, indicating very little variation
in the learning trajectory across runs.

$q_{0}$$q_{2}$$q_{1}$o/wo/walwaysRight sideLeft side

$q_{0}$$q_{2}$$q_{1}$o/wo/walwaysRight side4th square from left

Figure 2: Two distinct DFAs learned from the Hallway environment. Both are
enough to make the domain Markovian: however, the one on the right is not a
perfect classifier of $\mathcal{G}$ for this domain. This shows that the
condition in the statement of AutRL above is strictly stronger than necessary:
there are automata which do not perfectly decide $\mathcal{G}$, but which
discern enough relevant information to learn optimal, temporally extended
behavior in non-Markovian settings.

##  6 Concluding Remarks

In this work, we show how to learn a KR that provides a useful symbolic
abstraction in support of realizing temporally extended behavior in standard
RL agents which rely on a Markov assumption. We provide an end-to-end RL
algorithm which, in deterministic and stochastic domains, is demonstrably more
sample-efficient than the state of the art. We further provide theoretical
guarantees of optimal convergence for our approach (under conditions outlined
in the theorem of Appendix A), along with insight into the structure of DFAs
which are learned by this method.

This work reveals several promising directions for future research. While we
considered environments where the reward function is non-Markovian, learning
in the more general setting of POMDPs is an important problem related to this
work, to which the DFA methods leveraged here do not apply. Further, AutRL
leverages DFAs, which can only represent regular languages, thus learning
efficiently when $\mathcal{G}$ is not a regular language in $S\times A$, as
well as learning KR in high-dimensional and continuous state spaces, remain
major open challenges to this methodology.

## Acknowledgments and Disclosure of Funding

We gratefully acknowledge funding from the Natural Sciences and Engineering
Research Council of Canada (NSERC), the Canada CIFAR AI Chairs Program, and
Microsoft Research. The third author also acknowledges funding from ANID
(Becas Chile). Resources used in preparing this research were provided, in
part, by the Province of Ontario, the Government of Canada through CIFAR, and
companies sponsoring the Vector Institute for Artificial Intelligence
(www.vectorinstitute.ai/partners). Finally, we thank the Schwartz Reisman
Institute for Technology and Society for providing a rich multi-disciplinary
research environment.

## References

  * [1] David Silver, Thomas Hubert, Julian Schrittwieser, Ioannis Antonoglou, Matthew Lai, Arthur Guez, Marc Lanctot, Laurent Sifre, Dharshan Kumaran, Thore Graepel, et al.  A general reinforcement learning algorithm that masters chess, shogi, and go through self-play.  Science, 362(6419):1140–1144, 2018.
  * [2] Jan Peters, Sethu Vijayakumar, and Stefan Schaal.  Reinforcement learning for humanoid robotics.  In Proceedings of the Third IEEE-RAS International Conference On Humanoid Robots, pages 1–20, 2003.
  * [3] Robert H Crites and Andrew G Barto.  Improving elevator performance using reinforcement learning.  In Proceedings of the 9th Conference on Advances in Neural Information Processing Systems (NIPS), pages 1017–1023, 1996.
  * [4] Rodrigo Toro Icarte, Ethan Waldie, Toryn Klassen, Rick Valenzano, Margarita Castro, and Sheila McIlraith.  Learning reward machines for partially observable reinforcement learning.  In Proceedings of the 32nd Conference on Advances in Neural Information Processing Systems (NeurIPS), pages 15523–15534, 2019\.
  * [5] Alberto Camacho, Rodrigo Toro Icarte, Toryn Q Klassen, Richard Anthony Valenzano, and Sheila A McIlraith.  LTL and beyond: Formal languages for reward function specification in reinforcement learning.  In Proceedings of the 28th International Joint Conference on Artificial Intelligence (IJCAI), volume 19, pages 6065–6073, 2019.
  * [6] Maor Gaon and Ronen Brafman.  Reinforcement learning with non-Markovian rewards.  In Proceedings of the 34th AAAI Conference on Artificial Intelligence (AAAI), volume 34, pages 3980–3987, 2020.
  * [7] Rodrigo Toro Icarte, Toryn Q. Klassen, Richard Valenzano, and Sheila A. McIlraith.  Using reward machines for high-level task specification and decomposition in reinforcement learning.  In Proceedings of the 35th International Conference on Machine Learning (ICML), pages 2112–2121, 2018.
  * [8] Bruno Lacerda, David Parker, and Nick Hawes.  Optimal and dynamic planning for Markov decision processes with co-safe ltl specifications.  In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), volume 13, pages 1511––1516, 2014.
  * [9] Michael L. Littman, Ufuk Topcu, Jie Fu, Charles Isbell, Min Wen, and James MacGlashan.  Environment-independent task specifications via GLTL.  arXiv preprint arXiv:1704.04341, 2017.
  * [10] Alberto Camacho, Oscar Chen, Scott Sanner, and Sheila McIlraith.  Decision-making with non-Markovian rewards: From LTL to automata-based reward shaping.  In Proceedings of the Tenth International Symposium on Combinatorial Search (SoCS 2017), pages 279––283, 2017.
  * [11] Xiao Li, Yao Ma, and Calin Belta.  A policy search method for temporal logic specified reinforcement learning tasks.  In 2018 Annual American Control Conference (ACC), pages 240–245, 2018.
  * [12] Rodrigo Toro Icarte, Toryn Q Klassen, Richard Valenzano, and Sheila A McIlraith.  Teaching multiple tasks to an RL agent using LTL.  In Proceedings of the 17th International Conference on Autonomous Agents and Multiagent Systems (AAMAS), pages 452–461, 2018.
  * [13] Ronen Brafman, Giuseppe De Giacomo, and Fabio Patrizi.  LTLf/LDLf non-Markovian rewards.  In Proceedings of the 32nd AAAI Conference on Artificial Intelligence (AAAI), pages 1771–1778, 2018.
  * [14] Mohammadhosein Hasanbeig, Yiannis Kantaros, Alessandro Abate, Daniel Kroening, George J Pappas, and Insup Lee.  Reinforcement learning for temporal logic control synthesis with probabilistic satisfaction guarantees.  In 2019 IEEE 58th Conference on Decision and Control (CDC), pages 5338–5343. IEEE, 2019.
  * [15] Ronen I Brafman and Giuseppe De Giacomo.  Regular decision processes: A model for non-Markovian domains.  In Proceedings of the 28th International Joint Conference on Artificial Intelligence (IJCAI), pages 5516–5522, 2019.
  * [16] Mohammadhosein Hasanbeig, Alessandro Abate, and Daniel Kroening.  Logically-constrained reinforcement learning.  arXiv preprint arXiv:1801.08099v8, 2019.
  * [17] Anurag Koul, Sam Greydanus, and Alan Fern.  Learning finite state representations of recurrent policy networks.  International Conference on Learning Representations, 2019.
  * [18] Zhe Xu, Ivan Gavran, Yousef Ahmad, Rupak Majumdar, Daniel Neider, Ufuk Topcu, and Bo Wu.  Joint inference of reward machines and policies for reinforcement learning.  In Proceedings of the International Conference on Automated Planning and Scheduling, volume 30, pages 590–598, 2020.
  * [19] Richard S Sutton and Andrew G Barto.  Reinforcement learning: An introduction.  MIT press, 2018.
  * [20] Faheim Bacchus, Craig Boutilier, and Adam Grove.  Rewarding behaviors.  In Proceedings of the Thirteenth National Conference on Artificial Intelligence (AAAI-96), volume 13, pages 1160–1161, 1996.
  * [21] Maayan Shvo, Andrew C Li, Rodrigo Toro Icarte, and Sheila A McIlraith.  Interpretable sequence classification via discrete optimization.  arXiv preprint arXiv:2010.02819, 2020.
  * [22] John Schulman, Filip Wolski, Prafulla Dhariwal, Alec Radford, and Oleg Klimov.  Proximal policy optimization algorithms.  arXiv preprint arXiv:1707.06347, 2017.
  * [23] Prafulla Dhariwal, Christopher Hesse, Oleg Klimov, Alex Nichol, Matthias Plappert, Alec Radford, John Schulman, Szymon Sidor, Yuhuai Wu, and Peter Zhokhov.  Openai baselines.  https://github.com/openai/baselines, 2017.

## Appendix A: Convergence Guarantee + Proof

######  Theorem 1.

Let $\mathcal{N}$ be an NMRDP. Under the following (realistic) conditions,
_AutRL_ converges to an optimal policy in the sample limit.

  1. 1.

_sample_traces_ visits every reachable history within $\mathcal{N}$ i.o.a.s.
(infinitely often, almost surely) (exploration)

  2. 2.

For any regular language $\mathcal{L}$, when sampling traces using
_sample_traces_ , that _aut_learn_ eventually returns an automaton perfectly
deciding $\mathcal{L}$ w.p.1. (consistency of _aut_learn_)

  3. 3.

_markov_learn_ converges to the optimal policy in the sample limit for an MDP
(consistency of _markov_learn_)

  4. 4.

$\\{h:R(h)=1\\}=\mathcal{G}\subset\mathcal{H}$ forms a regular language with
alphabet $\Sigma=S\times A$ (regularity)

###### Proof.

Fix an NMRDP $\mathcal{N}=(S,A,P,R,\gamma)$, and assume all conditions are met
in the theorem statement. Then, since $\mathcal{G}$ is a regular language, we
have that the variable dfa is, with probability 1, eventually set to an
automaton, with state space $Q$, which perfectly decides $\mathcal{G}$. At
such a point, then we have that the states of the automaton perfectly predict
the reward $R$: thus, the problem $\mathcal{M}=(S\times Q,A,P,R,\gamma)$,
simply defined as $\mathcal{N}$ but with a state space augmented with the
automaton states of dfa, is actually an MDP. Further, dfa is never thereafter
reset, since no inconsistent trace can possibly be sampled from the
environment subsequently. Thus, $\mathcal{M}$ is simply an MDP being trained
with Markovian learning algorithm markov_learn, and thus the appropriate
convergence conditions apply. ∎

## Appendix B: Baseline Hyperparameter Tuning

For the tabular $Q$-learning for our approach (AutRL), we used a learning rate
of $0.1$ on the deterministic environment, $0.001$ on the stochastic
environment. Further, an exploration parameter of $0.01$ was used on the
deterministic environments, and $0.05$ with a decaying exponential schedule
(with factor $0.99$) was used for the stochastic environments. The automaton
learning method was set at a maximum state threshold of $5$ for all
environments but the multi-armed bandit (which had $14$), a loop penalty of
$0.01$ in all environments, and a transition penalty of $0.01$ for the Multi-
Armed Bandit, $0.3$ for both gridworlds, and $0.6$ for the hallway. Further,
the timeout was set to $250$.

For the Recurrent-PPO baseline, we used the OpenAI Baselines [23]
implementation with an LSTM policy network, an entropy coefficient of $0.001$,
a learning rate of $0.0003$, a discounting factor of $0.99$, a batch size of
$16384$ (with $8$ minibatches per update), and all other hyperparameters set
to default. We followed standard practice in tuning these hyperparameters and
found that these settings consistently performed best across our domains.