Composing Diverse Policies for Temporally Extended Tasks

TL;DR

This paper presents a novel method for composing diverse control policies for complex, temporally extended tasks, leveraging local dynamics models and expert demonstrations to improve planning and robustness.

Contribution

It introduces a global goal scoring estimator that sequences policies using local dynamics models, enabling efficient planning without explicit pre- and post-conditions.

Findings

Successfully applied to an MDP benchmark demonstrating robustness

Effectively solved a complex gear assembly task on a PR2 robot

Discovered optimal controller sequences in diverse scenarios

Abstract

Robot control policies for temporally extended and sequenced tasks are often characterized by discontinuous switches between different local dynamics. These change-points are often exploited in hierarchical motion planning to build approximate models and to facilitate the design of local, region-specific controllers. However, it becomes combinatorially challenging to implement such a pipeline for complex temporally extended tasks, especially when the sub-controllers work on different information streams, time scales and action spaces. In this paper, we introduce a method that can compose diverse policies comprising motion planning trajectories, dynamic motion primitives and neural network controllers. We introduce a global goal scoring estimator that uses local, per-motion primitive dynamics models and corresponding activation state-space sets to sequence diverse policies in a locally optimal fashion. We use expert demonstrations to convert what is typically viewed as a gradient-based learning process into a planning process without explicitly specifying pre- and post-conditions. We first illustrate the proposed framework using an MDP benchmark to showcase robustness to action and model dynamics mismatch, and then with a particularly complex physical gear assembly task, solved on a PR2 robot. We show that the proposed approach successfully discovers the optimal sequence of controllers and solves both tasks efficiently.

Tables

Table 1. TABLE I : Table of successful trials for different policies. MP - Motion Planning, DMP - Dynamic Motion Primitive, CNN - Convolutional Neural Network. The CNN policy has a maximum of 50 steps to reach the goal. The symbol * indicates policies terminated early due to safety concerns.

Control Method	Pick Up	Gear Move	Gear Insert	Full Task
MP	10/10	10/10	1/10	1/10
DMP	10/10	10/10	1/10	1/10
CNN	*	10/10	10/10	*
MP & CNN (Our)	10/10	10/10	10/10	10/10

Equations

$$s_{t+1}={\mathcal{D}}_{\omega }(s_t,a_t)={\mathcal{D}}_{\omega }(s_t,{\pi }_{\omega }(\widehat{s_t})).$$

$$s_{t+n}={\mathcal{D}}_{\omega }\circ {\mathcal{D}}_{\omega }\circ \cdots \circ {\mathcal{D}}_{\omega }(s_t)={\mathcal{D}}_{\omega }^n(s_t).$$

$${\pi }_{\Omega }({\omega }_t|s_t)=\underset{\omega }{\mathrm{argmax}}\left(\mathbb{E}\left[1_{{\mathcal{I}}_{\omega }}(s_t)\cdot \mathcal{G}\circ {\mathcal{D}}_{\omega }^n(s_t)\right]\right)$$

Full text

I Introduction

For robots to work in the wild, they need to be able to perform a variety of consecutive tasks that might require vastly different skills. Each individual skill could be partitioned and optimized outside of this complex system, and is potentially constructed using a number of diverse methods or control strategies, such as motion planning approaches for reaching, contact aware grasping, picking and placing, or through the use of end-to-end neural network based controllers.

In many practical applications, we wish to combine a diversity of such controllers to solve complex tasks. This typically requires that controllers share a common domain representation and a notion of progress to sequence these. For instance, the problem of assembly, as shown in Figure 1, can be partitioned by first picking up a mechanical part, then using motion planning and trajectory control to move this in close proximity to an assembly, before the subsequent use of a variety of wiggle policies to fit the parts together, as shown by [1]. Alternatively, the policy could be trained in an end-to-end fashion with a neural network, but one may find this difficult for extended tasks with sparse rewards, such as in Figure 1. In the interest of sample efficiency and tractability, such end-to-end learning could be warm-started by using samples from a motion planner, which provides information on how to bring the two pieces together and concentrates effort on learning an alignment policy, as in [2]. Additionally, the completion of these independent sub-tasks can be viewed as a global metric of progress.

We propose a hybrid hierarchical control strategy that allows for the use of diverse sets of sub-controllers, consisting of commonly used goal-directed motion planning techniques, other strategies such as wiggle, slide and push-against [3] that are so elegantly used in human manipulation, as well as deep neural network based policies that are represented very differently from their sampling-based motion planning counterparts.

Thus, we tackle a key challenge associated with existing motion primitive scheduling approaches, which typically assume that a common representation is used by all sub-controllers. We make use of the fact that controllers tend to have a dynamic model of the active part of their state space - either an analytical or a learned model, and further estimate how close each state is to completing the overall task using a novel goal scoring estimator. This allows the hierarchical controller to model the outcome of using any of the available sub-controllers and then determine which of these would bring the world state closest to achieving the desired solution – in the spirit of model predictive control.

As in the work of [4] on sequencing funnels and [5] on LQR-Trees, the scheduled controllers for sub-regions of the state space can be optimized in our framework, allowing for compositional task completion, but importantly, also for additional diversity of the controller set.

Value function approximation techniques used in the reinforcement learning community [6] can be considered similar to the proposed progress estimator, but only model the expected reward and require the actions to be in the same state space. We attempt to remedy this oversight, by allowing for a diversity of action and state spaces, and by modelling global progress at a local controller level.

This paper makes the following contributions:

• •

We use a Goal Score estimator to sequence a set of policies to solve a task. This estimator is trained using expert demonstrations to evaluate the current and future state of the plan and helps to transform the hierarchical learning problem into a planning problem.

• •

We provide a method for composing diverse policies that work with different input information, or decompose the action in either joint or end-effector space and work at different operational frequencies to solve a high level task.

We first evaluate the use of the controller dynamics and the goal metric to compose policies in a hybrid controller on an MDP benchmark problem to evaluate robustness to action and model dynamics noise. Next, we apply this approach to a physical gear assembly task performed by the PR2 robot, making use of both motion planning and visual neural network policies (Figure. 1).

II Related Work

Robotics: Compositionality is a key paradigm for robot control, which methods of composing controllers of a single type like [4, 5, 7] aim to exploit. These techniques rely on partitioning a state space into smaller overlapping operating regions and tuning sub-controllers (feedback or LQR) for operation in these regions. Unfortunately, these methods often fail to consider the fact that different tasks may require different controller sequences, and the scheduling of control laws in work on compositionality is often underemphasized. Inspired by this capability and the funnels framework [8]111Regions of robustness arising from the dynamics and control applied in a sub-region of the control space. this work provides a Model Predictive Control (MPC) [9] framework for compositional sequencing where controllers can be of different types and operate using different state spaces.

The ability to act on different state-spaces and action sets is particularly important, as the sub-policies required to complete a temporally extended task can be highly variable. For example, sub-problems such as grasping and pushing have been addressed and investigated at least since the 1980s, and these could be encapsulated into operation as motion primitives [3]. Using a diverse set of policies allows for the selection of controllers that best fit the working domain - for example [10] highlights that compliance may be needed when movement and sensing reaches the perception noise boundary, [11] advocate using non-prehensile grasps for manipulation of objects and [12] explore manipulation strategies that allow for caging of objects, such that these can be re-grasped stably in a subsequent stage. Alternatively these motion planning strategies can be formulated using stable nonlinear attractor systems as in DMPs [13, 14] or as DeepDMPs [15]. We aim to create a hybrid control framework that allows the use of these diverse motion planning controllers, alongside neural network policies to solve long-sequence tasks.

Learning from Demonstration: To expedite the learning process, it is common to provide demonstrated example solution trajectories to a problem. Methods like Behaviour Cloning (BC) allow for simple visuomotor policies to be learned end-to-end [16], or to be extended to learn safe policies [17], extract preferences [18] or to learn mappings for the perception and kinematic differences [19]. Alternatively, they can be used to calculate the relative value of each state through inverse reinforcement learning and to create a hierarchical formulation for control [20]. As explained in [21], there are limitations to BC in terms of number of demonstrations, generalization, and the challenge of modeling complex scenarios. However, we use these full task demonstrations as a means for estimating the distance to a desired goal state, which is arguably a simpler task than learning an entire policy. Additionally, by allowing different controller representations, we do not need to re-represent one control law in alternative approximate forms.

Reinforcement Learning: In the reinforcement learning (RL) literature the concept of options has parallels to our work, as each policy can be viewed as a controller with the initiation set as its domain. Our method lies between learning policies over options as in [22], and computing solutions using learning from demonstration by inverse reinforcement learning [23].

The options framework [24, 25] provides a formal means to work with hierarchically structured sequences of decisions made by a set of RL controllers. Temporal abstractions have been extensively investigated [26, 27, 28, 29, 24], and it is clear that hierarchical structure helps to simplify control, allows an observer to disambiguate the different states of the agent, and encapsulates a control policy and termination of the policy within a subset of the state space of the problem. This split in the state space allows us to verify the individual controller within the domain of operation [30, 31], deliberate about the cost of an option and increases interpretability [32]. Our work can be viewed as using a planner as a hierarchical policy in the options framework, which is made possible through the incorporation of a goal-scoring progress function learned from demonstration.

In a similar manner, [33] showed how planning can be incorporated into action selection when future states can be evaluated. Our method borrows this view of temporally abstracting trajectories and extends it by applying a dynamics model for each of the options, allowing an agent to assess its states and incorporate foresight [34] in its actions.

The work of [35] highlights that including a dense reward indeed increases the overall performance of the agent. Instead of using a predetermined dense function, we learn a Goal Scoring estimator from the demonstrations. As shown in [2] naively tuning and shaping a reward function may result in sub-optimal solutions using base actions. Furthermore, our planner selects an already learned controller and thus avoids converging to sub-optimal behaviours.

As highlighted by Sunderhauf [36], there are limits of the use of RL in robotics. By leveraging strategies from both RL and control communities, this work aims to increase the scope of problems that can be tackled in robotics.

III Method

Our framework defines a hierarchical controller over the set of pre-existing controllers. Each policy uses its dynamic model to propagate the current state to a future state conditioned on its control law. The Goal Scoring Estimator, learned over expert demonstrations, evaluates those future states and selects a controller that brings the system closest to the desired configuration.

Formally, assume the existence of a learned set of controllers $\mathcal{C}=\{c_{1},c_{2},...,c_{N}\}$ including those learnt from experience in previously solved problems. Using notation similar to the RL options framework [24], each controller $c_{\omega}$ is independently defined by a control law $\pi_{\omega}(s)\rightarrow a$, $s\in\mathcal{S}_{\omega}$, action $a\in\mathcal{A}_{\omega}$, a working domain $\mathcal{I}_{\omega},\mathcal{I}_{\omega}\subseteq\mathcal{S}_{\omega}$ where the controller can be started, and a termination criterion $\beta_{\omega}$. We rely on a forward dynamics model $s_{t+1}\sim\mathcal{D}_{\omega}(s_{t},a_{t})$, which is a stochastic mapping, and a Goal Scoring metric $g\sim\mathcal{G}_{K_{j}}(s_{t}),~{}0\leq g\leq 1$, that estimates the progress of the state $s_{t}$ with respect to a desired world configuration. We assume $\mathcal{G}_{K_{j}}$ to change monotonically through the demonstrated trajectories. The different controllers can work on different state spaces $\mathcal{S}=\{\mathcal{S}_{1},\mathcal{S}_{2},..,\mathcal{S}_{N}\}$ as long as there exists a space $\mathcal{S}^{*}$, such that $\mathcal{S}_{i}\subseteq\mathcal{S}^{*}$. This means there exists a higher or equal order state space, which maps the controller space of operation to regions of $\mathcal{S}^{*}$.

This work constructs a hybrid hierarchical controller $\pi_{\Omega}(\omega_{t}|s_{t})$ that can choose the next controller $c_{\omega_{t}}$ that needs to be executed to bring a learned latent state $s_{t}$ to some desired $s_{final}$. It uses the forward dynamics model $\mathcal{D}_{\omega}$ in an $n$-step Model Predictive Control (MPC) look-ahead, using a Goal Scoring metric $\mathcal{G}_{K}$ that evaluates how close $s_{t+n}$ is to $s_{final}$.

As shown in Fig. 2, in this work, we use a variational autoencoder to learn a latent state $s_{t}$ from image observations. We assume that each controller in the library has an associated forward dynamics model, trained to predict the next latent state, $s_{t+1}$. This provides us with an implicit mapping between states, and allows us to render an image of an expected scene for each controller that is applied. This scene prediction is then used by the goal score metric to evaluate the effect of choosing each controller and to select the most appropriate controller to be used at a given time step. In effect, this means that controllers act on the appropriate state components, but the underlying state representation used for controller selection is conditioned on image observations. Conditioning on images is feasible, as the robot head camera provides an overhead view of the entire workspace. While it may be possible to learn a shared state representation or mapping between states, this can be challenging (e.g. mapping from joint angles to images is extremely hard), while learning to predict the next latent state is a much easier task. Each of the framework components is described below.

III-A Goal Score Evaluation

The key component of the proposed framework is the ability to evaluate how well a particular state $s$ maps to parts of a demonstrated expert trajectory. This allows us to estimate the temporal distance of that state to the end of the demonstration (see Figure 2). In a similar manner to [37], who use adjacency of frames as positive and negative examples, we leverage the temporal sequence of the demonstration as a measure of task completeness.

We capture demonstrations of the global task (in its entirety) to use as a weak supervision for learning a goal scoring network that allows us to map a state to a progress estimation value $g\sim\mathcal{G}(s_{t})$ for a given task. To build the Goal Scoring models, we use a convolutional network head with a Mixture Density Network (MDN) tail to encode the different goal representations based on image observations. The network predicts a distribution over the proximity of the current state to the desired goal state.

The first observation of a demonstration can be viewed as score [math] – far away from the goal state, whereas the final observation as score $1.0$ – a target representation of the world. Even though there may not be a one-to-one mapping between the values within several demonstrations, we rely on the variability in their lengths being encoded within the different modes of the MDN of the Goal Scoring Model.

III-B Controller Selection

At a particular point at state $s_{t},s_{t}\in S^{*}$ when $c_{\omega}$ is active, we can compute the goodness of following the current controller given these conditions up to a particular time horizon. The action given by the policy is $a_{t}=\pi_{\omega}(\hat{s_{t}}),\hat{s}\in S_{\omega}$, and following the dynamics model we can write that:

[TABLE]

As the dynamics model is conditioned on the controller $c_{\omega}$, we can simplify to $s_{t+1}=\mathcal{D}_{\omega}(s_{t})$. Chaining this for $n$ steps into the future we obtain

[TABLE]

We can evaluate this future state as $g_{t+n}=\mathcal{G}\circ\mathcal{D}^{n}_{\omega}(s_{t})$. Thus, the hierarchical controller over controllers can be sequentially optimized,

[TABLE]

This chooses the controller that is within the operation domain for the current state and delivers the largest goal score estimate after $n$ steps. After choosing and evaluating the optimal $\pi_{\Omega}$ with respect to the above criterion, another controller can be selected at the next time step, with repetition until the goal is reached.

III-C Controller Dynamics Modelling

The dynamics of each controller is modelled individually only within its operational domain. This simplifies the complexity the dynamics model has to learn and thus requires less data. Here, we learn a neural dynamics model for each controller that predicts the latent state configuration $s_{t+1}$ from $s_{t}$, as in [38]. The architecture shown in Fig.  2 is based on a VAE encoding, but includes an additional dynamics network, which predicts the next latent state if a given controller were applied. The same decoder is used to force the two representations not to diverge.

A diverse dynamics network can be used as a prior for each controller [39] and the execution of the controllers themselves can be used to build an individual model using the image state space if it is not provided internally.

IV Experimental Setup

We perform two sets of experiments to investigate the efficacy of the structured hierarchical policy by performing MPC future predictions at each step on a simulated MDP problem and on a much more complex physical gear assembly task on the PR2 robot.

IV-A Simulated MDP

In the first experiment, we use the standard 19-state random walk task as defined in [40] and shown in Figure 3 to illustrate concepts in a simple sequential decision making task. The goal of the agent is to reach past the $19^{th}$ state and obtain the $+1$ reward. The action space of the agent is to go “left” or “right”, moving the agent to an increasing or decreasing state. There also exist 5 controllers defined as in Section III, with the following policies: (1-3) policies that go “right” with a different termination probabilities $\beta=\{0.9,0.5,0.2\}$; (4) random action; (5) policy with action to go “’left” with $\beta=0.5$. We assume that there exists a noisy dynamics model $\mathcal{D}_{\omega}$ and the goal evaluation model $\mathcal{G}_{MDP}$, which has the probability of falsely predicting the current state or its value of $0.2$.

Further, we expand the MDP to be of size 100 and evaluate how sensitive the performance of the model is in regards to noise in the Goal Scoring evaluator and in each of the dynamics models.

IV-B Gear Assembly

In this task the PR2 robot needs to assemble the first part of the Siemens Challenge222The challenge is at https://new.siemens.com/us/en/company/fairs-event, which involves grasping a compound gear from a table, and placing it on a peg module held in the other hand of the robot. We record expert demonstrations of the task being performed, and assume access to a set of controllers that (1) picks up the gear from the table; (2) moves the left PR2 arm in proximity to the other arm; (3) inserts the gear on the peg module. Policy (1, 2) rely exclusively on scripted path planning techniques and work using discrete time steps, while (3) is learned entirely with a neural network. Controllers (1, 2) share a common state space of the robot’s joint angles, whereas (3) works directly on the visual pixel input from the robot’s head camera.

The visual neural policy, shown in Figure 2, performs imitation learning by using behaviour cloning of the 50 tele-operated demonstrations. This is trained until convergence or 100 epochs using different encoder heads - small convolutional network, ResNet-50, -101. The expert-illustrated trajectories were performed using a HTC Vive controller teleoperating the PR2 robot and the process took less than 1h wall time. The action generation part of the network is an MDN that predicts a distribution of the next time step joint angles $\theta$, which are set as the internal PID targets for the robot 7-DOF arm.

The dynamics model for each controller is learned independently and is represented with a Forward Dynamics MDN, learned from forward rollouts of the policy network. The Goal Score estimator is learned on an additional 5 rollouts of the full gear assembly task and operates on the latent space of the particular policy. Throughout all of the experiments we use the Adam optimizer with a weight decay rate of $1e^{-}6$, batch size of $120$, train for $200$ epoch and the MDN uses $24$ Gaussian mixtures. We show the performance of this model with several video streams from different cameras on the robot (head, left and right forearm cameras).

Additionally, we compare the performance of the scripted Motion Planning method (using RRT Connect [41]), Dynamic Motion Primitives (learned from the MPs) and the Visual Neural Policy on each subtask, as well as using the full sequence under the different controllers as a baseline.

V Experimental Results

We demonstrate the viability of composing diverse policies by using the controller dynamics as a method for choosing a satisfactory policy. The dynamics can be learned independently of the task, and can be used to solve a downstream task.

Simulated MDP   This problem illustrates the feasibility of using our architecture as a planning method. Figure 4 shows that the agent reaches the optimal state in just 4 planning steps, where each planning step is a rollout of a controller. The predicted state under the specified time horizon is illustrated at each step for the different controller options. This naturally suggests the use of the policy $\pi_{1}$ that outperforms the alternatives ($\pi_{1}$ reaches state 6, $\pi_{2}$ - state 4, $\pi_{2}$ - state 3, $\pi_{3}$ - state 1, $\pi_{4}$ - state 1, $\pi_{5}$ - state 0). Even though the predicted state differs from the true rollout of the policy, it allows the hierarchical controller to use the controller that would progress the state the furthest. The execution of some controllers (i.e. $c_{5}$ in planning steps 1, 2, 3) reverts the state of the world to a less desirable one. By using the forward dynamics, we can avoid sampling these undesirable controllers.

In order to investigate the robustness and convergence properties of our method, we introduce noise within the system, while expanding the MDP to be of size 100 and maintaining the same 5 controllers as above. We can see in Figure. 5 how the number of controllers required to reach the target location varies at different noise levels. When we observe low amounts of noise, the performance remains stable and requires activating any of these controllers a total of less than 20 times (top-left part of the heatmap). The expected optimal number of controller activations based on policy 1 is 12 (black region of the heatmap). As the noise in both the dynamics model and the Goal Score Estimation increases, we observe a degradation and the selection of more sub-optimal controllers. The model is more sensitive to noise in the Goal Score Estimator than when the dynamics of the controllers make errors in their predictions. Despite this, the method converges to the optimal state.

It is interesting to note that the method uses close to or optimal number of controller activations in cases where multiple policies would drive the world in a progressive state, highlighting that the goal score metric is capable of choosing longer horizon controllers due to the MPC look ahead.

Gear Assembly   We build the library of controllers for the task - picking up a gear (Figure  6), moving it close to the base of the assembly and inserting the gear on the base plate (Figure. 7). A motion planning control method was used to perform different tasks. Those demonstrations were used to build the DMP model, using the ROS-DMP module, which is based on [14]. The Convolutional Neural Network policy was trained using 50 tele-operated demonstrations covering a wide variety of initialization cases for each specific task. We did not observe any task performance changes between the small Convolutional or the ResNet-50,-101 head and therefore relied on the simple architecture. Other tasks may benefit from deeper or more complex models (such as [42, 43, 44]), but integration within the method would remain the same.

Table. I shows the performance of the different controllers on different tasks. The MP and DMP models exhibit stable performance in contact based tasks, but fail where the initial conditions differ – in Figure 6 we can see the variability that the pickup controller exhibits in terms of the location of the grasp on the gear, which leads to failures in attempting to insert this onto the base assembly. The issue comes from the tolerances of the fit as using an MP and a sequence of trajectory points does not compensate for any inaccuracies incurred during the previous stages of the process or manual positioning. Precise insertion is known to fail outside of a very small convergence basin when using MP controllers - we obtain similar (bad) performance similar to [2, 45].

As a baseline, we compare against optimally sequencing the MP and DMP control strategies, which can be seen under the “Full Task” performance. Due to the low performance on a part of the task, the overall success rate is limited.

In contrast, the natural variability of the grasp is part of the training set of the CNN model and successfully inserts the gear even with a high variance of initial locations (Figure. 7). As the visual CNN policy is not dependant on the absolute position of either the grasped location or the position of the base assembly, it performs corrective/feedback actions for the policy to succeed. However, the CNN performance on the pickup task could not be evaluated, as the prescribed controller actions were jerky and violated safety constraints (pre-defined velocity and position limits).

This illustrates that the combination of MP for picking up the gear and moving it closer to the assembly and CNN to insert the gear, selected using our method allows for the full task to be successfully solved optimally 10 out of the 10 attempts. This shows the advantage of using a diverse set of controllers, allowing each one to be tuned to the domain of operation.

The Goal Score Model is trained on only 5 full task demonstrations. We empirically choose $n=10$ for the $n$ step MPC look ahead as our planning horizon. In the interests of reproducibility, more information about the sub-controllers and training routines is available on the website 333https://sites.google.com/view/composingdiverse. Figure 8 illustrates the Goal Score estimation for a previously unseen demonstration from camera streams with different viewpoints. The score for the different controllers can clearly be used to sequence the policies. This is shown by the fact that the score follows a monotonically increasing value with regards to the average score for the individual controller domain.

VI Conclusion

We introduce a method for composing diverse policies with varied representations, including Motion Planning, Dynamic Motion Primitives and Convolutional Neural Networks. This allows for the solution of combinatorially complex and temporally extended tasks requiring multiple steps, without needing to predefine controller sequences or design high level state machines. We sequence tasks by using a Goal Scoring Model trained by expert demonstrations providing a weak supervisory signal. The goal scoring model provides a controller invariant prediction of progress towards a goal, which can be used with shared latent space across sub-controllers. This work has also introduced different methods that allow for a model-based or a model-free way to create a dynamics model, which can be used to analytically plan the next best option within a model predictive control framework.