An optimal control approach to the design of periodic orbits for mechanical systems with impacts

TL;DR

This paper presents an optimal control method for designing stable periodic orbits in hybrid mechanical systems with impacts, combining trajectory optimization and root finding for improved stability and performance.

Contribution

It introduces a novel optimal control framework that efficiently designs periodic orbits for underactuated hybrid systems with impacts, ensuring numerical stability and boundary constraint satisfaction.

Findings

Successfully designed periodic orbits for a compass biped model.

Demonstrated numerical stability and effectiveness of the proposed method.

Achieved optimized trajectories satisfying boundary constraints.

Abstract

In this paper we study the problem of designing periodic orbits for a special class of hybrid systems, namely mechanical systems with underactuated continuous dynamics and impulse events. We approach the problem by means of optimal control. Specifically, we design an optimal control based strategy that combines trajectory optimization, dynamics embedding, optimal control relaxation and root finding techniques. The proposed strategy allows us to design, in a numerically stable manner, trajectories that optimize a desired cost and satisfy boundary state constraints consistent with a periodic orbit. To show the effectiveness of the proposed strategy, we perform numerical computations on a compass biped model with torso.

Equations

$$M(q)\ddot{q}+C(q,\dot{q})+G(q)=Y_u(q)u,$$

$$\dot{x}(t)=f(x(t))+g(x(t))u(t),$$

$$x^{+}(t)=\Delta (x^{-}(t)),$$

$$\Sigma{\color[rgb]{0,0,0}:}=\left\{\begin{array}[]{cc}\dot{x}(t)=f(x(t))+g(x(t))u(t),&\;x^{-}(t)\notin S\\ x^{+}(t)=\Delta(x^{-}(t)),&\;x^{-}(t)\in S.\end{array}\right.$$

$$\begin{split}\min_{x(\cdot),u(\cdot)}&\;\frac{1}{2}\int_{0}^{T}\!\!(\|x(\tau)-x_{d}(\tau)\|_{Q}^{2}+\|u(\tau)-u_{d}(\tau)\|_{R}^{2})\;d\tau\\ \!\!\!{\color[rgb]{0,0,0}\text{subj. to}}&\;\dot{x}(t)=f(x(t))+g(x(t))\;u(t)\\ &\;x(0)=x_{0}\\ &\;x(T)=x_{f},\end{split}$$

$$M(q)\ddot{q}+C(q,\dot{q})+G(q)=Y(q)u^e,$$

$$\dot{x}(t)=f(x(t))+g(x(t))u(t)+g_{emb}(x(t))u_{emb}(t),$$

$$\begin{split}\min_{x(\cdot),u(\cdot)}&\;\frac{1}{2}\int_{0}^{T}(\|x(\tau)-x_{d}(\tau)\|_{Q}^{2}+\|u(\tau)-u_{d}(\tau)\|_{R}^{2}\\ &\;\quad+\rho_{emb}^{2}\|u_{emb}(\tau)\|^{2})\;d\tau+\frac{1}{2}\rho_{f}^{2}\|x(T)-x_{T}\|^{2}\\ {\color[rgb]{0,0,0}\text{subj. to}}&\;\dot{x}(t)=f(x(t))+g(x(t))u(t)+g_{emb}(x(t))u_{emb}(t)\\ &\;x(0)=x_{0},\end{split}$$

$$u_d^e=Y(q_d{)}^{-1}[M(q_d){\ddot{q}}_d+C(q_d,\dot{q_d})+G(q_d)].$$

$$\begin{split}\min_{x(\cdot),u(\cdot)}&\;\frac{1}{2}\int_{0}^{T}\!\!\!(\|x(\tau)-x_{d}(\tau)\|_{Q}^{2}+\|u(\tau)-u_{d}(\tau)\|_{R}^{2})\;d\tau\\ &+\frac{1}{2}\rho_{f}^{2}\|x(T)-x_{T}\|^{2}\\ \!\!{\color[rgb]{0,0,0}\text{subj. to}}&\;\dot{x}(t)=f(x(t))+g(x(t))u(t)\\ &\;x(0)=x_{0}.\end{split}$$

$$\int_{0}^{T}\left[\begin{array}[]{c}z(\tau)\\ v(\tau)\\ \end{array}\right]^{T}\left[\begin{array}[]{cc}H_{xx}(\tau)&H_{xu}(\tau)\\ H_{xu}^{T}(\tau)&H_{uu}(\tau)\\ \end{array}\right]\left[\begin{array}[]{c}z(\tau)\\ v(\tau)\\ \end{array}\right]d\tau\;>0,\quad\forall z\in{\mathbb{R}}^{n},\forall v\in{\mathbb{R}}^{m},$$

$$M(\theta )\ddot{\theta }+C(\theta ,\dot{\theta })+G(\theta )=U$$

$$\Delta([\theta^{-T}\;\dot{\theta}^{-T}])=\left[\begin{array}[]{cc}R&0_{3\times 3}\\ 0_{3\times 3}&A(\theta^{-})\\ \end{array}\right]\left[\begin{array}[]{c}\theta^{-}\\ \dot{\theta}^{-}\\ \end{array}\right]$$

$$R=\left[\begin{array}[]{ccc}0&1&0\\ 1&0&0\\ 0&0&1\\ \end{array}\right].$$

$$S:=\{x\in {\mathbb{R}}^6|{\theta }_1={\theta }_1^{jmp}\}$$

$$M(\theta )\ddot{\theta }+C(\theta ,\dot{\theta })+G(\theta )=U^e,$$

$$Y=\left[\begin{array}[]{ccc}-1&0&0\\ 0&-1&0\\ 1&1&1\\ \end{array}\right],$$

$$u_d^e(t)=Y^{-1}(M({\theta }_d(t)){\ddot{\theta }}_d(t)+C({\theta }_d(t),{\dot{\theta }}_d(t))+G({\theta }_d(t))),$$

$$\begin{array}{rl}& \min {\int }_0^{T}l(x(\tau ),u(\tau ))d\tau +m(x(T))\\ & \text{subj. to}\dot{x}(t)=f(x(t),u(t)),x(0)=x_0,\end{array}$$

$$\begin{array}{rl}\dot{x}(t)=& f(x(t),u(t)),x(0)=x_0,\\ u(t)=& \mu (t)+K(t)(\alpha (t)-x(t)),\end{array}$$

$$\mathcal{P}:\xi \mapsto \eta ,$$

$$M(\theta)=\left[\begin{array}[]{ccc}(\frac{5}{4}m+M_{H}+M_{T})r^{2}&-\frac{1}{2}mr^{2}c_{12}&M_{T}rlc_{13}\\ -\frac{1}{2}mr^{2}c_{12}&\frac{1}{4}mr^{2}&0\\ M_{T}rlc_{13}&0&M_{T}l^{2}\end{array}\right]$$

$$C(\theta,\dot{\theta})=\left[\begin{array}[]{c}-\frac{1}{2}mr^{2}s_{12}\dot{\theta_{2}}^{2}+M_{T}rls_{13}\dot{\theta_{3}}^{2}\\ \frac{1}{2}mr^{2}s_{12}\dot{\theta_{1}}^{2}\\ -M_{T}rls_{13}\dot{\theta_{1}}^{2}\end{array}\right]$$

$$G(\theta)=\left[\begin{array}[]{c}-\frac{1}{2}g(2M_{H}+3m+2M_{T})r\sin\theta_{1}\\ \frac{1}{2}gmr\sin\theta_{2}\\ -gM_{T}l\sin\theta_{3}\end{array}\right]$$

$$U=\left[\begin{array}[]{c}-u_{1}\\ -u_{2}\\ u_{1}+u_{2}\end{array}\right]$$

$$\begin{array}[]{c}c_{12}:=\cos(\theta_{1}-\theta_{2})\quad c_{13}:=\cos(\theta_{1}-\theta_{3})\\ s_{12}:=\sin(\theta_{1}-\theta_{2})\quad s_{13}:=\sin(\theta_{1}-\theta_{3}),\end{array}$$

$$A_{11}=\frac{1}{\text{den}}[2M_T\cos (-{\theta }_1^{-}-{\theta }_2^{-}+2{\theta }_3^{-})+$$

$$-(2m+4M_H+2M_T)\cos ({\theta }_1^{-}-{\theta }_2^{-}),$$

$$A_{12}=\frac{1}{\text{den}}m,A_{13}=0,$$

$$A_{21}=\frac{1}{\text{den}}[m-(4m+4M_H+2M_T)\cos (2{\theta }_1^{-}-2{\theta }_2^{-})+$$

Full text

††journal: Nonlinear Analysis: Hybrid Systemsecrecrfootnotetext: This result is part of a project that has received funding from the European Research Council (ERC) under the European Union’s Horizon 2020 research and innovation programme (grant agreement No 638992 - OPT4SMART).

1 Introduction

Hybrid systems, involving both continuous and discrete dynamics, arise naturally in a number of engineering applications. In particular, many robotics tasks, such as legged locomotion, (multi-finger) manipulation and load transportation with aerial vehicles, can be modeled as mechanical systems with impacts, a particular class of hybrid systems. These classes of systems experience continuous dynamics until an interaction with the surrounding environment (i.e., an impact) occurs, thus causing a state discontinuity due to impulsive forces and (possibly) a switch between different dynamics.

In this paper we concentrate on a particular aspect that has important implications both for modeling and control of robots, namely trajectory design.

Motivations

The trajectory design task has gained a lot of attention both as a preliminary task for control and as a tool to explore and understand the capabilities of the system [1],[2] ( see [3] for an earlier reference). Optimization techniques allow us to design reference trajectories for robot controllers by minimizing a given cost function. Typical cost functions are (i) the distance from a desired state-input curve (which does not satisfy the dynamics) and (ii) the energy injected into the system. For example, for humanoid robot design, the distance from a desired (but unfeasible) human-like walking pattern ([4], [5]) is often considered. Additional challenges arise when the trajectory generation problem is addressed for (underactuated) mechanical systems with impacts. The impact events complicate the trajectory optimization problem since discontinuous changes in the state occur. For some systems, the underactuation and the instability of the continuous dynamics render the problem even more challenging. The optimal control theory offers powerfull tools to deal with trajectory generation problems for hybrid dynamical systems.

Literature review

The literature on optimal control of hybrid systems is quite vast, thus we report only two sets of contributions relevant for our work.

Fist, a general overview on recent advances for optimal control of hybrid systems is presented. A recent survey, focusing in particular on switched systems, is [6]. In [7] the authors propose optimal control algorithms for discrete-time linear hybrid systems which “combine a dynamic programming exploration strategy with multiparametric linear programming and basic polyhedral manipulation”. In [8] a set of necessary conditions is formulated and optimization algorithms are presented for optimal control of hybrid systems with continuous nonlinear dynamics and with autonomous and controlled switchings. The algorithm described in [9] (for hybrid systems with partitioned state space and autonomous switching) is based on a version of the minimum principle for hybrid systems providing optimality conditions for intersections and corners of switching manifolds, and thus avoiding the combinatorial complexity of other algorithms (e.g., [8]). Furthermore, the work in [10] deals with optimal mode-scheduling via a gradient descent algorithm for the particular class of autonomous switched-mode hybrid dynamical systems. In [11] and [12] the design of switching laws for switched systems with linear dynamics, based on the optimization of a quadratic criterion, is addressed.

Second, we focus on trajectory optimization for the particular class of mechanical systems with impacts. Besides results regarding systems with impulsive controls, e.g. [13], [14], we focus on the trajectory design for systems controlled by inputs of the continuous dynamics. The majority of works adopt parametric optimization methods, as, e.g., [15], [16], [17]. This means that trajectories are approximated as, for example, classical, trigonometric or Bézier polynomials and the optimization is performed with polynomial coefficients as decision variables. In other works, e.g. [18], the optimization problem is addressed via dynamics equation discretization and the optimal periodic trajectory is computed by means of an approximated cost function. Available software toolboxes are used to solve trajectory generation problems addressed in [15], [16], [17] and [18]. Recently, the optimization over jump times and/or mode sequence, as e.g., in [19] and [20], has been considered. In [19] the instants of jump are optimized, but the sequence of continuous dynamics modes is fixed. In [20] the mode sequence is also optimized by sequential quadratic programming.

Focusing in particular on the trajectory generation problem for legged robots (the major robotic application regarding mechanical systems with impacts), challenges arising when dealing with underactuated robots are addressed in [21] and [22] by considering an additional “virtual” input acting on the unactuated degrees of freedom. Then, an optimal approximated trajectory of the underactuated dynamics is computed by dynamics inversion. Furthermore, online trajectory optimization is addressed in [23] through a method based on iterative LQG and in [24] trajectories are designed by using an auxiliary system of differential equations and then stabilizing the generated curves through a feedback on the target system.

Contributions

As main contribution of this paper, we develop an optimal control based strategy to design periodic orbits for a class of hybrid dynamical systems with impacts. First, we formulate an optimal control problem with ad-hoc boundary conditions. These ones are provided by studying how the initial conditions and the jump conditions are related in a periodic orbit with one jump per period. Second, instead of using available softwares, we develop an ad-hoc strategy based on the combination of trajectory functional optimization with three main tools: dynamics embedding, constraint relaxation and zero finding techniques. These tools enable us to (i) deal with the undeartuated nature of dynamics, (ii) consider highly non trivial constraints and (iii) avoid the tedious search for a suitable “initial (guess) trajectory” to initialize the optimization algorithm. Furthermore, optimal control problems involved in our strategy are solved by combining the penalty function approach [25] with the Projection Operator Newton method [26]. In contrast with many strategies reported in the literature, we do not resort to approximations such as considering discrete sets of motion primitives (thus ending up with the only optimization of their parameters), and/or discrete time. In fact we consider system states and inputs as optimization variables and a second-order approximation of the optimization problem is directly constructed in continuous time. In detail, the proposed strategy is based on the following steps. By adding a fictitious input, we embed the system into a completely controllable (fully actuated) one. On this system we set up, by constraint relaxation, an unconstrained optimal control problem to find a trajectory of the system (a curve satisfying the dynamics) that minimizes a weighted $L_{2}$ distance from a desired curve. The desired curve, together with the weights of the cost functional become important parameters in the designer’s hand to explore the system dynamics, that is different periodic orbits that the system can execute. We solve the optimal control problem to generate a trajectory of the fully actuated system with almost null fictitious input. Differently from [21] and [22], we use this trajectory to initialize the last step of our strategy, where the optimal control problem is set up on the underactuated dynamics. A Newton update rule on the final penalty of the weighted cost functional is adopted in order to hit the desired final state. It relies on the solution of an optimal state transfer problem presented in [27], where the advantages of the method are highlighted with respect to the classic ones.

We provide a set of numerical computations showing the effectiveness of the proposed strategy. In particular, we generate a periodic gait for a three-link biped robot (compass model with torso). We perform two computations by choosing two different sets of weights. In the first scenario we try to generate a trajectory that is as close as possible to the guessed state curve. Vice-versa, in the second one, we compute a trajectory that minimizes the input effort (i.e., some sort of minimum-energy trajectory). It is worth noting that, although the three links model is relatively simple, its underactuation represents a significant challenge. Also, it is well known that for many applications, even such a reduced model of the biped dynamics is instrumental for analyzing and controlling the actual system, see, e.g., [24].

Paper organization

The paper is organized as follows. In Section 2 we introduce the model of the particular class of hybrid system we study in this paper and we present the problem formulation for the generation of periodic orbits. In Section 3 we describe the proposed strategy and in Section 4 we provide numerical computations (on a biped walking model) showing its effectiveness.

2 Problem formulation for the generation of periodic orbits

In this section we provide the optimal control formulation for solving the problem of periodic orbit generation.

2.1 Hybrid system model: underactuated systems with impacts

In this subsection we introduce the particular class of hybrid systems studied in the paper. The hybrid model consists of a continuous dynamics phase and a discrete (impulsive) jump event, occurred when the system state reaches a jump set.

The mechanical system dynamics is modeled by the ordinary differential equation

[TABLE]

where $q$, $M(q)$, $C(q,\dot{q})$, $G(q)$, $u$, are respectively the generalized coordinate vector, the mass matrix, the Coriolis vector, the gravity vector, and the input vector. The dynamics (1) can be written in the state space form as the continuous and control affine dynamics

[TABLE]

with state $x\in{\mathbb{R}}^{n}$ defined as $x:=[q^{T}\;\dot{q}^{T}]^{T}$ and input $u\in{\mathbb{R}}^{m}$. In particular, we assume that (i) the system is underactuated, i.e., we let $m<n$, and (ii) functions $f:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{n}$ and $g:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{n\times m}$ are (at least) $C^{3}$.

When the system state, evolving as modeled in (2), reaches a jump set $S\subset{\mathbb{R}}^{n}$, a discrete impulsive event occurs, causing a discontinuity in the system state evolution. The discrete event is modeled by the jump map $\Delta:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{n}$, which we assume to be invertible, such that

[TABLE]

where $x^{-}(t):=\lim_{\tau\rightarrow t^{-}}x(\tau)$ and $x^{+}(t):=\lim_{\tau\rightarrow t^{+}}x(\tau)$ denote the left and right limits of system trajectories satisfying (2). Finally, the overall hybrid model is:

[TABLE]

2.2 Optimal control problem set-up

We aim at designing periodic trajectories, minimizing a weighted $L_{2}$ distance from a desired curve, whose period $T$ is fixed. We approach the task by formulating an optimal control problem over the continuous dynamics (2). We impose a boundary constraint $x(T)=x_{f}$, where $x_{f}$ is a final state that allows the system to jump exactly to the initial condition $x_{0}$, i.e., $x_{0}=\Delta(x_{f})$. In particular, we choose the initial state $x_{0}$ and we compute the final state by inverting the jump map, i.e., $x_{f}=\Delta^{-1}(x_{0})$.

Let a desired curve $(x_{d}(\cdot),u_{d}(\cdot))$, satisfying $x_{d}(0)=x_{0}$, $x_{d}(T)=x_{f}$ and thus $x_{d}(T)=\Delta^{-1}(x_{d}(0))$, be given. Furthermore, let us assume that the desired curve does not reach the jump set $S$ in the open time interval $(0,T)$. We are ready now to present the optimal control problem we aim to solve for the generation of periodic orbits,

[TABLE]

where $x(\cdot)$ is an absolutely continuous state trajectory, $u(\cdot)$ is a bounded (measurable) input, $Q$ and $R$ are positive definite weighting matrices and for some vector $z\in{\mathbb{R}}^{k}$ and matrix $W\in{\mathbb{R}}^{k\times k}$ we denote $\|z\|_{W}^{2}=z^{T}Wz$.

The time-horizon $T$ is a design parameter together with the desired curve, the initial (or final) conditions and the weighting matrices $Q$ and $R$. We stress the fact that the desired curve is not a trajectory of the system, i.e., even if it satisfies the boundary conditions, it does not satisfy the dynamics. Thus, the desired curves can be seen as “design tools” to parameterize actual trajectories of the system. Furthermore, by choosing the desired trajectory $(x_{d}(\cdot),u_{d}(\cdot))$ to satisfy the jump condition only at time $T$, we expect to compute an optimal (state) trajectory satisfying the same property. Otherwise, this constraint can be (implicitly) enforced by properly choosing the weight matrices $Q$ and $R$.

Remark** (Final state constraint and controllability).**

It is worth noticing that for problem (5) to be feasible the state $x_{f}$ must belong to the reachable space of the system. $\square$

3 Optimal control based strategy for periodic orbit design

As highlighted in the literature review, several software toolboxes are available to solve nonlinear constrained optimal control problems. Nevertheless, optimal control problems are optimization problems in the infinite dimensional space of state-input curves subject to highly non trivial constraints as the dynamics and the fixed final state constraint. This means that the solution of an algorithm is highly influenced by the desired curve and by the “initial curve” (possibly a trajectory) that initializes the algorithm. For this reason, instead of attacking the problem directly by simply applying one of the solvers available in the literature, we develop a strategy based on suitable embedding and relaxation ideas that gives a systematic method to compute periodic trajectories.

We divide our strategy into three steps that will be presented in detail in the next subsections. Here we provide an informal idea. First, we add a fictitious input to the underactuated dynamics, thus getting a fully actuated system. Second, we consider an unconstrained relaxation of the optimal control problem (5) by substituting the final state constraint with a penalty in the cost function. We solve the relaxed problem by using also the fictitious input, but with a high penalty. Third and final, we get rid of the fictitious input and enforce the final state constraint exactly by means of a suitable iterative method (i.e., a zero finding Newton iteration on the final state penalty).

3.1 Dynamics embedding

The first part of the strategy is called dynamics embedding and is inspired by [28]. It consists of embedding the underactuated system into a fully actuated one by adding a fictitious input $u_{emb}\in{\mathbb{R}}^{p}$, with $p=n-m$. Thus, embedding the mechanical system (1) into a fully actuated one, we get

[TABLE]

where $u^{e}:=[u^{T}\;u_{emb}^{T}]^{T}$ and $Y(q):=[Y_{u}(q)\;Y_{emb}(q)]$ is assumed to be invertible $\forall q$. Accordingly, the state space equations of the (embedding) fully-actuated dynamics are

[TABLE]

where $g_{emb}$ is a $\mathcal{C}^{3}$ vector field taking into account the action of the embedding input. The role of this additional input is to allow for the tracking of any (sufficiently regular) desired curve. This provides a useful tool to initialize the optimal control problem solved in the next part of the strategy.

3.2 Optimal control problem relaxation and continuation with respect to parameters

Next, we design an optimal control relaxation of problem (5). The relaxation involves two aspects. First, according to the dynamics embedding introduced in the previous step we consider the fully actuated version of the system. We add a penalty in the cost function for the embedding input. Second, we relax the final state constraint by adding a penalty in the cost functional. The relaxed problem is

[TABLE]

where $\rho_{emb}$ and $\rho_{f}$ in ${\mathbb{R}}_{>0}$ are respectively the weights on the embedding input and the final state error. The target state $x_{T}$ is a strategy parameter that is set to $x_{d}(T)$ at beginning of the strategy, but will be modified during the strategy evolution according to the root finding procedure described in the next step. The idea is to weight the embedding input and the final state error much more than the other inputs and the states. In order to avoid a conflict between the two objectives we propose a continuation procedure on the two penalties.

Problem (8) is an unconstrained optimal control problem that could be solved by means of several available tools in the literature. We use the PRojection Operator Newton method for Trajectory Optimization (PRONTO) introduced in [26] and described in A. We adopt the PRONTO tool because it shows two main appealing features for our trajectory design task. First, it allows one to handle unstable dynamics in a numerically stable manner and with a “reasonable” computational effort. Second, it guarantees recursive feasibility during the algorithm evolution. That is, at each step a system trajectory is available.

We are now ready to present the second step of the strategy. First, given a desired state curve $x_{d}(\cdot)$ such that $x_{d}=[q_{d}^{T}\;\dot{q}_{d}^{T}]^{T}$ , we can easily compute input trajectories of the fully actuated system such that the desired state curve is a system trajectory. By inverting the fully actuated dynamics model (6), we compute $u_{d}^{e}(t)$, $\forall t\in[0,T]$ as

[TABLE]

Note that, $u_{d}^{e}(\cdot)$ depends on the choice of $Y_{emb}(\cdot)$ but once $Y_{emb}(\cdot)$ is defined, the input $u_{d}^{e}(\cdot)$ is unique.

Thus, the second step of the strategy can be informally described as follows. We denote by $\xi^{e}=(x(\cdot),[u^{T}(\cdot)\;u_{emb}^{T}(\cdot)]^{T})$ a generic state-input curve for the fully actuated system. We set the desired curve to $\xi_{d}^{e}=(x_{d}(\cdot),[u_{d}^{T}(\cdot)\;0^{T}(\cdot)]^{T})$ and let $\xi^{e}_{0}=(x_{d}(\cdot),u_{d}^{e}(\cdot))$ be the initial trajectory (of the fully actuated system) for the Projection Operator Newton method (the optimal control solver). Then, following an integral penalty function approach [25], we iteratively solve problem (8) increasing the weight $\rho_{emb}$ at each step, by means of a suitable heuristic. When the norm of the embedding input is sufficiently small (i.e., when $\|u_{emb}(\cdot)\|<\epsilon_{emb}$, where $\epsilon_{emb}$ is a given tolerance) we stop the procedure, thus obtaining an approximated trajectory of the underactuated system.

A pseudo-code description of the strategy is given in the next table (Algorithm 1). We denote $\mathcal{P}^{e}$ the projection operator (17) acting on the fully actuated system, so that $\eta^{e}=\mathcal{P}^{e}(\xi^{e})$ is a trajectory of the fully-actuated system. For given $\rho_{emb}$ and $\rho_{f}$, we denote $\texttt{PRONTO}^{e}$ a routine such that $\xi^{e}_{opt^{e}}=\texttt{PRONTO}^{e}(\xi^{e}_{0},\xi^{e}_{d};\rho_{emb},\rho_{f})$, i.e., it takes as inputs an initial trajectory $\xi^{e}_{0}$ and a desired (state-control) curve $\xi^{e}_{d}$, and computes an embedding optimal trajectory, which is denoted as $\xi^{e}_{opt^{e}}=(x_{opt^{e}}(\cdot),[u_{opt^{e}}^{T}(\cdot)\;u_{opt^{e},emb}^{T}(\cdot)]^{T})$, solving the nonlinear optimal control relaxation in (8).

Remark**.**

(Convergence) Provided that a solution to (8) with $u_{emb}(\cdot)=0$ exists, the existence of the solution is also guaranteed, by continuity, when $u_{emb}$ is in a neighbourhood of the origin (with suitable radius $r_{emb}\geq\epsilon_{emb}$). Thus, a solution to (8) with $||u_{emb}(\cdot)||<\epsilon_{emb}$ exists and the convergence to that solution follows since we are using an integral penalty function approach (see, e.g. [25]). Nevertheless, dealing with a non-convex problem, only a local convergence is ensured, provided that the algorithm is initialized with a trajectory lying inside the basin of attraction.

3.3 Enforcing the final state constraint

We are ready to present the third and final step of the strategy. This part relies on the idea, proposed in [27], of enforcing the final state constraint of problem (5) by combining two actions: (i) increase the penalty $\rho_{f}$ on the final state error, and (ii) vary the target state $x_{T}$ until the candidate optimal trajectory “practically” meets the final state constraint.

The following optimal control problem is the relaxation of problem (5) subject to the original system, i.e., without dynamics embedding

[TABLE]

Before presenting the theorem that establishes a connection between the original problem (5) and its relaxed version (9), let us state the second order sufficient condition for optimality satisfied by a local minimum of the original problem (5).

Definition 1**.**

(Second order sufficient condition [27]) The second order sufficient condition for a local minimum is

[TABLE]

where $H$ is the control Hamiltonian and $H_{uu}=\frac{\partial^{2}H}{\partial u^{2}}$, $H_{xx}=\frac{\partial^{2}H}{\partial x^{2}}$, $H_{xu}=\frac{\partial^{2}H}{\partial x\partial u}$.

The following result is at the basis of the third step of the proposed strategy.

Theorem 1**.**

(Equivalence of constrained and relaxed minimizers [27]) Suppose that $\xi^{*}=(x^{*}(\cdot),u^{*}(\cdot))$ is a local minimum of (5) satisfying the second order sufficiency condition for optimality. Then for each $\rho_{f}>0$, there is a target state $x_{T}=x_{T}(\rho_{f})$ such that $\xi^{*}$ is a local minimizer of problem (9) satisfying the corresponding second order sufficiency condition. $\square$

Thus, the third step of the strategy can be informally described as follows. Given an embedding optimal trajectory $\xi^{e}_{opt^{e}}$ from the previous step, we obtain the initial trajectory $\xi_{0}=\mathcal{P}((x_{opt^{e}}(\cdot),u_{opt^{e}}(\cdot))$ of the original system, where $\mathcal{P}$ is the projection operator acting on the original system. Furthermore, we reduce the embedding desired curve $\xi_{d}^{e}$, in order to get the desired curve for the original system as $\xi_{d}=(x_{d}(\cdot),u_{d}(\cdot))$. Then, adopting a penalty function approach, we iteratively solve problem (9) increasing the weight $\rho_{f}$ at each step. The use of a large $\rho_{f}$ can easily result in a poorly conditioned problem where the terminal cost overwhelms the remaining one. Thus, we start with a reasonably small $\rho_{f}$ and we gradually increase it by means of a suitable heuristic. Note that our penalty function approach generates an optimal trajectory which approximately satisfies the final state constraint. For this reason, when the final state $x_{opt,i}(T)$ of the temporary optimal trajectory is “sufficiently close” to the desired final state (i.e., $\|x_{opt,i}(T)-x_{d}(T)\|\leq\delta^{f}_{tol}$, where $\delta^{f}_{tol}$ is tolerance guaranteeing the root finding convergence), we apply a Newton method for root finding on $x_{T}$ to meet exactly the final state constraint. The update rule we use on $x_{T}$ is the one proposed in [27]. According to the Implicit Function Theorem, (i) there exists a mapping $\beta:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{n}$ such that $x_{opt,i}(T)=\beta(x_{T,i})$, where $\xi_{opt,i}(\cdot)=(x_{opt,i}(\cdot),u_{opt,i}(\cdot))$ is the solution to (9) with target state $x_{T,i}$ and (ii) the first Fréchet differential of $\beta(\cdot)$ at $x_{T,i}$, i.e., $z_{f}\mapsto D\beta(x_{T,i})\cdot z_{f}$, exists. Furthermore, provided that the linearization of $\dot{x}(t)=f(x(t))+g(x(t))u(t)$ about $\xi_{opt,i}$ is controllable on $[0,T]$, the mapping $\beta(\cdot)$ is invertible. Thus, a Newton method for root finding is applied on the final state constraint equation $\beta(x_{T})-x_{f}=0$ in order to find the value of $x_{T}$ such that the solution to (9) is equivalent to the solution to (5). In particular, at the iteration $i+1$, we update the value of $x_{T}$ according to the following rule: $x_{T,i+1}=x_{T,i}+D\beta(x_{T,i})^{-1}(x_{f}-\beta(x_{T,i}))$. The final state constraint is met when $\|x_{opt,i}(T)-x_{d}(T)\|<\epsilon^{f}_{tol}$, where $\epsilon_{tol}^{f}$ is the desired tolerance on the final state error.

A pseudo code description of the third step is given in the following table (Algorithm 2). We use the following notation. For a given $\rho_{f}$ and $x_{T}$, we denote PRONTO the Projection Operator Newton method for the original system, so that we have $\xi_{opt}=\texttt{PRONTO}(\xi_{0},\xi_{d};\rho_{f},x_{T})$.

Note that, as for Algorithm 1, convergence of Algorithm 2 directly follows since it is a properly formulated penalty function approach.

4 An illustrative example: three link planar biped robot

As an example, we apply the proposed technique to a planar biped walking model. The model adopted in this paper was introduced in [29].

4.1 Biped model

The three rigid links biped with four lumped masses consists of a torso and two legs of equal length connected at the hip. The hybrid walking model consists of a continuous swing phase model and a discrete jump event model.

The swing phase model describes the motion of the swing leg to develop a walking step. Let $\theta_{1},\;\theta_{2},\;\theta_{3}$ denote the orientation of the stance leg, the swing leg and the torso, respectively. Furthermore, let $u_{1}$ be the torque applied between the stance leg and the torso while the torque $u_{2}$ acts between the swing leg and the torso. The swing phase model via Euler-Lagrange equations is given by

[TABLE]

where $\theta=[\theta_{1}\;\theta_{2}\;\theta_{3}]^{T}$ and $M(\theta)$, $C(\theta,\dot{\theta})$, $G(\theta)$ and $U$ (defined in B) are respectively the mass matrix, the Coriolis vector, the gravity vector and the generalized torques vector. Defining the state $x=[\theta_{1}\;\theta_{2}\;\theta_{3}\;\dot{\theta}_{1}\;\dot{\theta}_{2}\;\dot{\theta}_{3}]^{T},$ and the input $u=[u_{1}\;u_{2}]^{T}$, we can write the dynamics (10) in state-space form (2).

The jump event model takes into account (i) the impulse force arising when the swing leg touches the ground and (ii) the switch of the leg being in contact with the ground. See [29] for more details. The jump event model is given by (3) where

[TABLE]

with $A(\theta^{-})$ reported in C and

[TABLE]

The jump event occurs when the system state reaches the jump set

[TABLE]

where $\theta_{1}^{jmp}$ is set according to physical considerations.

4.2 Numerical computations

According to [29], we consider the following model parameters: mass of the legs $m=5$ kg; mass of the hip $M_{H}=15$ kg; mass of the torso $M_{T}=10$ kg; length of the legs $r=1$ m, and the length of the torso $l=0.5$ m. We use as jump angle $\theta_{1}^{jmp}=\pi/8$. We choose as time horizon $T=1.53$ and as initial condition $x_{0}=[-22.5\;\text{deg}\;\;22.5\;\text{deg}\;\;20\;\text{deg}\;\;50\;\text{deg/s}\;\;0\;\text{deg/s}\;\;90\;\text{deg/s}]^{T}$.

The first step of the proposed strategy requires the definition of a fully actuated dynamics and the design of a desired curve. In order to obtain a fully actuated dynamics, we consider the dynamics (10) and add a (generalized) torque acting directly on the torso angle velocity $\dot{\theta}_{3}$. That is, the swing phase model becomes

[TABLE]

where $U^{e}=Yu^{e}$,

[TABLE]

and $u^{e}=[u_{1}\;u_{2}\;u_{emb}]^{T}$. To design a desired curve that satisfies constraints on the initial and final states, we choose as desired angles $\theta_{d,k}(\cdot)$ with $k=1,2,3$ spline curves so that the initial and final point and their relative slopes, i.e., $\theta_{d,k}(0)$, $\dot{\theta}_{d,k}(0)$, $\theta_{d,k}(T)$ and $\dot{\theta}_{d,k}(T)$, can be assigned a priori. Then, the desired velocity and acceleration curves, respectively $\dot{\theta}_{d}(\cdot)$ and $\ddot{\theta}_{d}(\cdot)$, are obtained by symbolic time differentiation. According to the fully-actuated dynamic model (13), we compute $\forall t\in[0,T]$

[TABLE]

where $u_{d}^{e}=[u_{0}^{T}\;u_{0,emb}]^{T}$, with $u_{0}\in{\mathbb{R}}^{2}$ and $u_{0,emb}\in{\mathbb{R}}$. Furthermore, as regards the update rule for the penalty parameters, at iteration $i+1$, we set $\rho_{emb,i+1}=2\;\rho_{emb,i}$. The same update rule is adopted for $\rho_{f}$. Numerical computations characterized by two different design objectives are presented in the following. We also invite the reader to watch the attached video showing optimal walking gaits obtained by means of the proposed strategy.

In the first computation, we choose $x_{d}(t)=[\theta_{d}(t)^{T}\;\dot{\theta}_{d}(t)^{T}]^{T}$ and $u_{d}(t)=u_{0}(t)$, $\forall t\in[0,T]$. We find the optimal trajectory choosing diagonal Q and R matrices and penalizing the angles $10^{4}$ times the input, while the velocities $10^{3}$ times the input. That is, $Q=\text{diag}\left[100\;100\;100\;10\;10\;10\right]$ and $R=\text{diag}\left[0.01\;0.01\right]$. The optimal trajectory (blue) is depicted on the left column of Figure 1 and Figure 2 together with the desired curve (red), the embedding optimal trajectory (green) and temporary trajectories of the underactuated system (black). The optimization strategy shapes the desired curve in order to obtain a trajectory (a curve satisfying the dynamics), thus showing that in this case the desired curve is just a rough guess of the biped gait.

In the second computation, we look for a trajectory that minimizes the “energy” injected in the system. Indeed, we choose $x_{d}(t)=[\theta_{d}(t)^{T}\;\dot{\theta}_{d}(t)^{T}]^{T}$, $u_{d}(t)=[0\;0]^{T}$, $\forall t\in[0,T]$, and penalize the inputs $10^{3}$ times the angles and $10^{2}$ times the velocities. That is, we choose $Q=\text{diag}\left[0.01\;0.01\;0.01\;0.1\;0.1\;0.1\right]$ and $R=\text{diag}\left[10\;10\right]$. The optimal trajectory (blue) is depicted on the right column of Figure 1 and Figure 2 together with the desired curve (red), the embedding optimal trajectory (green) and temporary trajectories of the underactuated system (black). As expected, the desired curve and the optimal trajectory coincide at $T$, while the embedding optimal trajectory has a nonzero final state error. This shows the effectiveness of the third step of the strategy that enforces the final state constraint. This optimal trajectory is different from the previous one, thus showing another possible gait. In order to minimize the injected energy, the swing leg first goes behind the stance one, it gains potential energy and then, it passes in front of the stance leg. This gait requires a less amount of torque, with respect to the first gait. Approaching the final time $T$, the torque $u_{2}$ of the first gait (Figure 2c) is greater than $100\;Nm$ while it approaches zero in the second gait (Figure 2d).

5 Conclusion

In this paper we have developed an optimal control based strategy to compute periodic orbits for underactuated mechanical systems with impacts. The strategy combines trajectory optimization with dynamics embedding, optimal control relaxation and root finding techniques. The proposed strategy provides a systematic and numerically robust methodology to design periodic orbits for a particular class of hybrid systems.