{'fraction_non_alphanumeric': 0.03502803200920025, 'fraction_numerical': 0.010781542000095836, 'mean_word_length': 4.4067357512953365, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 2, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 12, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Enforcing Constraints over Learned Policies via Nonlinear MPC: Application to the Pendubot

In recent years Reinforcement Learning (RL) has achieved remarkable results. Nonetheless RL algorithms prove to be unsuccessful in robotics applications where constraints satisfaction is involved, e.g. for safety. In this work we propose a control algorithm that allows to enforce constraints over a learned control policy. Hence we combine Nonlinear Model Predictive Control (NMPC) with control-state trajectories generated from the learned policy at each time step. We prove the effectiveness of our method on the Pendubot, a challenging underactuated robot.


INTRODUCTION
In recent years Reinforcement Learning (RL) has proven to be particularly successful in extremely complex application scenarios. From beating professional gamers Vinyals et al., 2019) to dexterous in-hand manipulation for solving a Rubik's cube (OpenAI et al., 2018), the RL methods are at the forefront of the artificial intelligence research. These results are possible because in RL the learning process generally does not require any analytical model of the controlled system. The final policy, which can be highly nonlinear, is the result of the optimization of the expected sum of the reward signals gathered by the agent while interacting with the environment, as shown by Sutton and Barto (1998).
Nonetheless, real robotics applications pose many challenges which greatly diverge from the classical RL assumptions (Kober et al., 2013): continuous control and state spaces, need to be data efficient if the learning procedure is performed directly on the robot, and the necessity of state and control constraints which can guarantee, for example, safety conditions.
In the last two decades many efforts have been devoted to improve the effectiveness of RL in robotics scenarios. Policy Search (PS) methods (Deisenroth et al., 2013) represent a first attempt to overcome the issue of directly estimating the optimal cost-to-go for each state, a problem that becomes rapidly intractable for robotics applications. PS approaches optimize directly the expected return of a parametrized policy. More recently, other solutions have been proposed to deal with continuous control-state space such as Deep Deterministic Policy Gradient (DDPG) . Moreover, in the literature it has been shown that PS approaches can effectively cope with the data efficiency issue if a model of the controlled system is learned in combination with a control policy (Deisenroth and Rasmussen, 2011). Chatzilygeroudis et al. (2017)  present a state-of-the-art model-based PS method which combines a black-box optimization with a gaussian process surrogate model.
Fewer works consider the problem of constraints satisfaction. In RL this issue is usually addressed by extending the reward with a penalty term. This approach has however several drawbacks and feasibility is not always assured. Achiam et al. (2017) propose an actor-critic approach based on a constrained Markov decision process formulation, which is solved by approximations. Similarly as in Cheng et al. (2019), a penalization term is added to limit constraint violations, thus increasing the safety of the learned policy. Another recent work (Heim et al., 2019) introduces an algorithm to learn the viability kernel directly in the control-state space. Even though it provides an estimation of a safety measure while learning, this procedure is conservative by construction and might converge slowly in a large control-state space.
On the other hand, Model Predictive Control (MPC) is an optimization-based control technique applied to dynamical systems. MPC optimizes an objective function based on a predicted evolution of the system state (Rawlings and Mayne, 2009;Borrelli et al., 2017). The possibility of including input and state constraints in the optimization problem, together with its inherent robustness, make MPC one of the most successful strategies. Even though a large number of algorithms have been developed for efficiently computing a solution, in general, for nonlinear control problems, the use of MPC in real time is typically not straightforward. Usually a simplified prediction model is convenient and, when the final state is not reachable in a small time frame, the knowledge of a reference trajectory for the entire task is required.
Many works aim at achieving better performance through the combination of an MPC with data driven approaches.
One of the first attempt to combine RL and MPC has been presented by Zhong et al. (2013), where an offline estimation of the value function is used as a final cost in an unconstrained iterative linear quadratic regulator. Tamar et al. (2017) propose iterative learning of a term in the objective function to incorporate long-term reasoning into the MPC. Other works have applied data driven techniques to learn a predictive model for MPC (Nagabandi et al., 2018;Kamthe and Deisenroth, 2018). More recent contributions towards the combination of MPC and RL are due to Mansard et al. (2018) and Farshidian et al. (2019).
Here we introduce an algorithm for combining MPC and RL that tries to overcome the aforementioned issues. We propose an optimal controller based on state-of-theart Nonlinear MPC (NMPC) solvers that allows for constraints satisfaction and can be used in real-time on challenging robotic systems. We explore how the continuous interaction between the control policy and the MPC can take care of constraints satisfaction in a real application scenario. The performance of our method is tested both in simulation and experiments on a challenging underactuated robotic system, the Pendubot (see Fig. 1).
This paper is organized as follows. Sec. 2 describes the control architecture, the RL algorithm and the NMPC formulation. The Pendubot case study is developed in Sec. 3, which also presents the simulation and experimental results. Future developments are briefly discussed in Sec. 4.

THE CONTROL ALGORITHM
The proposed method consists of two different phases. In the first phase any RL approach can be used to learn an offline policy that solves the desired task. In our case we use Deep Deterministic Policy Gradient (DDPG) from Lillicrap et al. (2016). In the second phase, once a control policy is learned, we can employ this information online to provide a guidance for an MPC. Here we define an NMPC problem and provide an online solution using the Real-Time Iteration scheme (RTI) (Diehl et al., 2005), which is a Sequential Quadratic Programming (SQP) variant. At each time step, starting from the current state, the learned policy is applied to a simulated robot model over a fixed prediction horizon. The resulting control-state trajectory is passed as a reference to the cost function of the NMPC, which allows to transfer the policy learned offline into an optimal control problem to enforce the constraints satisfaction. Afterwards, the resulting optimal control action is applied to the physical robot, which in turn reaches a new state where the procedure is then restarted. A block scheme of the proposed algorithm is shown in Fig. 2.
Our method is capable to find a successful solution under the assumption that the control policy has learned a sequence of actions to accomplish the given task at least in a subset of the feasible region. In practice, an extensive training phase and a large exploration of the controlstate space can ensure the validity of the aforementioned hypothesis.

Offline Policy Learning
Let us consider X ⊆ R nx and U ⊆ R nu the state and control space of our system. Moreover, we denote with π(x) : X − → U the control policy, and with u t ∈ U , x t ∈ X respectively the control action and the system state defined at time t.
DDPG is an actor-critic algorithm where the control policy π(x|θ π ) and the Q-Value function Q(x, π(x|θ Q )) : X × U − → U are both approximated with two neural networks, where θ π ∈ R nπ and θ Q ∈ R n Q are their parameters. The actor π, given the actual state x t , outputs a control action u t , while the critic Q provides an estimate of the Q-Value function for the pair (u t , x t ). During the offline training phase, at each time step, the agent applies an action u t = π(x t ) to the simulated robot and collects a reward r t from the environment. The generated experience represented by (x t , u t , r t , x t+1 ) is stacked inside the Replay Buffer (RB), an array that contains a fixed number of elements I. To stabilize the learning procedure, two additional copies of the actor, π (x|θ π ) and the critic Q (x, π(x)|θ Q ) are used in the algorithm.
The DDPG alternates between updating the Q-Value function and the control policy. The learning process is performed at a fixed frequency during the exploration phase of the agent. The update of the parameters θ = (θ Q , θ π , θ Q , θ π ) follows the subsequent steps: first, we randomly retrieve n samples from the RB, where n represents the chosen batch size. For each experience i only the new state x i+1 is passed to the actor copy network π which returns the action u i+1 . The pair (u i+1 , x i+1 ) is used as input to Q that estimates the associated Q-Value. The obtained result is then combined with the sampled experience reward r i to compute the temporal difference, which is defined as where γ is the discount factor. The final loss L, calculated by subtracting to y i the Q function computed over the sampled values (u i , x i ) as is then used to retrieve the gradient for the critic Q.
Finally the actor gradient π can be approximated as Each gradient is then back-propagated to update the values of the parameters θ.
At the end of the training phase, the control policy π is obtained and will be quickly used to generate the reference trajectory for the NMPC. During the offline policy learning phase constraints cannot be explicitly enforced, but their violation can only be penalized trough the reward function.

NMPC for Online Constraints Satisfaction
NMPC is an advanced control method that uses a nonlinear dynamic model of a system to define a finite-time constrained Optimal Control Problem (OCP) that is solved numerically in an iterative fashion. We define with N the prediction horizon length. Let T (x t ) = (u r t|k , x r t|k ), k = 0, . . . , N represents the control-state reference trajectory where x r t|k and u r t|k = π(x r t|k ) are the state and control input at t + k generated by forward integrating the simulation robot model with π(·) from the initial condition x t ∈ X. Given x t|k ∈ X, u t|k ∈ U and s t|k ∈ R nc which denote, respectively, states, controls and slack variables for hard constraints relaxation, the resulting NMPC problem is min u t|0 ,...,u t|N −1 s t|0 ,...,s t|N where the nonlinear function f (·) represents the discrete model dynamics, and h(·), h N (·) ∈ R nc the vector of path and terminal constraints. Finally, the positive-definite matrices W , W N > 0 and the scalar ρ > 0 represent the weights terms.
It is important to notice that each time the system reaches a new state x t+1 , before solving the NMPC problem, an update of the reference control-state trajectory is required. Hence, to generate the new trajectory, the policy π learned offline is applied to the simulated robot. The resulting control-state trajectory T (x t+1 ) is used as a reference in the cost function of the NMPC problem. Updating the reference trajectory plays a central role in the proposed algorithm. Thus, in the event that the current reference trajectory violates the constraints, the NMPC will drive the system away from the current reference trajectory, which will become obsolete.
In real-time applications, the NMPC needs to be solved at every sampling instant under tight runtime requirements. Here we employed the real-time iteration (RTI) scheme. The main idea consists in warm-starting the problem with the previous solution shifted by one sample and performing only one SQP iteration with Gauss-Newton Hessian approximation per each time step. A complete overview of the algorithm can be found in (Diehl et al., 2005).

CASE STUDY
We tested the proposed controller on the Pendubot (Fig. 1), an underactuated, planar, robotic system composed by two links and two rotary joints used in education and in research as a benchmark for nonlinear control schemes. The Pendubot moves in a plane. The base link is directly connected (no reduction gears) to a motor while the second link can only be passively driven by the existing dynamic coupling with the first link.
We consider the problem of swinging up and balancing the robot around the up-up equilibrium starting from the stable down-down equilibrium, while satisfying the imposed velocity constraints. We define a coordinate system where the desired final state is x g = (q 1 q 2q1q2 ) T = (0 0 0 0) T , with q i andq i respectively the position and the velocity of the i-th joint (Fig. 3). The joint position q 1 is measured with respect to the upwards vertical axis, while the angle q 2 is defined relatively to the first joint.
In the following we will describe the mathematical model of the system, and the setup of the learning and control q 2 q 1 Fig. 3. Schematic of the Pendubot coordinate system.
phases. Moreover, we will show the behaviour of the Pendubot under two different constraint settings, either in simulation and on the real platform. Finally, we will show that the continuous update of the reference trajectory is critical to successfully achieve the desired task.

The Simulated Robot Model
Given the dynamics formulation of the system B(q)q + c(q,q) + g(q) = u, where q = (q 1 q 2 ) T andq = (q 1q2 ) T , the Pendubot model can be written as where g represents the gravity and τ the torque input. The A i coefficients can be written as A 1 = I 1 + I 2 + m 1 a 2 1 + m 2 (l 2 1 + a 2 2 ), A 2 = I 2 + m 2 a 2 2 , A 3 = m 2 l 1 a 2 , A 4 = m 1 a 1 + m 2 l 1 , A 5 = m 2 a 2 , where I i and m i are, respectively, the moment of inertia and the mass of the i-th link, l i represents the length of i-th link and a i is the distance of the center of mass of the i-th link from the center of the i-th joint. Moreover, the physical parameter values for the Pendubot are listed in Table 1. Table 1. Pendubot physical parameters.

Offline Learning Phase
The learning of the control policy π is performed offline using the simulated robot model described in Sec. 3.1. In order to train π, a reward function must be defined. The function used here penalizes the distance between the current state and the desired one x g = (0 0 0 0) T . Thus, we define the reward r(x) as where α v , α τ are weighting parameters, and r 1 (q) = |q 1 | + |q 2 |. Moreover, as it is desired to have an upper limit on the joint velocities, both in experiments and in simulations, the quantity r 2 (q) = |q 1 | + |q 2 | is introduced in eq. (2). Finally, the term −α τ |τ | is used to minimize the control effort.

Online Control Phase
We show that our approach can enforce constraints over an offline learned control policy while still successfully performing the task. The Pendubot system is characterized by an highly nonlinear and fast dynamics. Therefore an high frequency controller is necessary to reach the desired goal. The proposed framework runs in real-time with a control frequency of 500 Hz. To solve the NMPC for both the simulations and the experiments we used the ACADO Toolkit (Houska et al., 2011), with a fixed prediction horizon of N = 10. The simulated robot model defined in Sec. 3.1 is used both within the NMPC optimization problem and for generating the desired reference trajectory with the policy π at each time step.
The swing-up of the Pendubot can be performed either rapidly or with an energy pumping maneuver, reaching the desired state x g through an oscillatory motion. In both cases, when the state is close to x g , a local controller, usually based on a linearization of the system around the final equilibrium point, can be used for the balancing phase. To this end we implemented a Linear Quadratic Regulator (LQR) for the experiment on the real platform.
Since the robot is only equipped with encoders, angular velocities are numerically derived from position measures.

Simulation and Experimental Results
In this section we show the results obtained in simulation and on the real robot, under two different constraint settings. The control policy, learned as explained in Sect. 2.1, is able to solve the swing-up including the balancing phase. The maximum velocities reached in simulation during the motion are 9.2 rad/sec and 9.8 rad/sec for the first and second joint, respectively. For clarity, in Fig. 4, Fig. 5 and Fig. 6, we represent with blue lines the solution obtained with the NMPC, and with the red dashed lines the state trajectory q π i obtained using only the control policy π. In the first setting, we impose a symmetric velocity constraintq 1,b = 7.2 rad/sec on the first joint and a maximum admissible torque τ 1,b = 0.4 N · m. We compare our approach with an NMPC where the reference trajectory obtained by the control policy is never updated. In the first row of Fig. 4 we show the results obtained with our approach. Due to the imposed constraints, the proposed controller steers the system to new feasible states, forcing the policy to recalculate a different trajectory, while successfully reaching the up-up configuration. On the other hand, if an NMPC with a fixed reference trajectory is used, the robot does not reach the desired final configuration as shown in Fig. 4 (bottom). It is therefore clear from these results, that recomputing the control-state trajectory at each time step is crucial to successfully complete the desired task.
In the next simulation, we define a second constraints setting, withq 1,b = 7.8 rad/sec, τ 1,b = 0.4 N · m and Fig. 4. Simulation results obtained with the proposed approach (top) and with an NMPC that uses a fixed reference trajectory (bottom) by imposing the constraintq 1,b = 7.2 rad/sec. Fig. 5. Simulation result obtained with the constraintsq 1,b = 7.8 rad/sec,q 2,b = 5 rad/sec. q 2,b = 5 rad/sec for the angular velocity of the second joint. As shown in Fig. 5, our controller is still able to perform the desired task. The maximum admissible velocity for the second joint is quickly reached, and the final motion of the system has a larger change with respect to the solution obtained with the first constraints setting.
Finally, we validated our approach on the real Pendubot system using the second constraints setting. Controlling the real robot involves additional challenges due to model uncertainties. Fig. 6 shows that our method is able to successfully perform the swing-up maneuver despite the velocity constraints. In this experiment a final LQR is used to stabilize the system around the equilibrium point, and the use of soft constraints in the NMPC formulation becomes necessary to avoid infeasibility due to model inaccuracies. Indeed, the introduction of slack variables determines small constraint violations of 0.1 rad/s and 1 rad/s, respectively on the first and second joint velocities (see Fig. 6). Thus, the oscillatory behaviour, that is visible on the velocity of the second joint (Fig. 6), depends on the effect of the cost minimization associated to the constraints violation. Model and parameters uncertainties, due to mechanical wear and time varying dynamics (like the dynamic friction induced by the sliding contacts for the encoder of link 2), explain the difference between the results obtained in simulation and on the real robot.
A video with the experimental results is available at https://youtu.be/5KATfbDwKlI.

CONCLUSIONS AND FUTURE WORK
In this work we present an approach for imposing constraints to a learned control policy. We test the proposed controller both in simulation and on a real Pendubot robot, showing that the continuous interplay between the NMPC and the learned policy is at the base of the constraints enforcement.
In the future, we plan to extensively study the NMPC steering capabilities. Not every constraint can be arbitrarily imposed in the optimization problem, since no assumption is made over the policy capability π to generate a satisfactory trajectory in the feasible region. A possible solution will be to automatically retrieve an index that measures the ability of the policy to accomplish the given task from any state, relaxing the constraints when it is needed using the slack variables. A second extension will be the improvement of the robustness over model uncertainties. Multiple policies can be learned in simulation with different dynamical parameters, and the choice of the appropriate π can be recomputed online within the NMPC formulation. Fig. 6. Experimental result obtained with the constraintsq 1,b = 7.8 rad/sec,q 2,b = 5 rad/sec.