Soft Robot Optimal Control Via Reduced Order Finite Element Models

Finite element methods are a numerical approach for solving partial differential equations (PDEs) and have been used for physics-based modeling and simulation in many domains, including fluid and structural mechanics as well as soft robotics [20]. These methods solve PDEs by performing a spatial discretization at a finite number of nodes defined by a mesh, which results in a finite set of ordinary differential equations corresponding to the state of each mesh node.

In the context of soft robotics, the state of each node consists of the node’s position and velocity, resulting in six degrees of freedom. Therefore, for a spatial discretization with $\mathcal{N}$ nodes the resulting FEM model will have a total dimension of $n^{f}=6\mathcal{N}$ (FEM model variables are denoted using $(\cdot)^{f}$). In particular, the soft robot FEM model is defined as in [19] using Newton’s second law:

where $q^{f}(t)\in\mathbb{R}^{3\mathcal{N}}$ is the vector of node positions, $v^{f}(t)\in\mathbb{R}^{3\mathcal{N}}$ is the vector of node velocities, and $u(t)\in\mathbb{R}^{m}$ is the control input. Additionally, $M^{f}\in\mathbb{R}^{3\mathcal{N}\times 3\mathcal{N}}$ is a constant mass matrix, $P^{f}\in\mathbb{R}^{3\mathcal{N}}$ represents constant external forces (e.g. gravity), $F^{f}(q^{f},v^{f}):\mathbb{R}^{3\mathcal{N}}\times\mathbb{R}^{3\mathcal{N}}\rightarrow\mathbb{R}^{3\mathcal{N}}$ is a nonlinear function that defines the internal forces (e.g. stresses from deformation), and $H^{f}(q^{f}):\mathbb{R}^{3\mathcal{N}}\rightarrow\mathbb{R}^{3\mathcal{N}\times m}$ is a nonlinear function that specifies the input matrix. The internal forces $F^{f}(q^{f},v^{f})$ can be modeled in several ways depending on the material properties of the robot. A classic example is to use a linear-elastic model, which assumes a linear stress-strain relationship through Hooke’s law.

To enable output feedback control we assume a linear measurement model given by:

where $x^{f}=[{v^{f}}^{T},{q^{f}}^{T}]^{T}\in\mathbb{R}^{n^{f}}$ is the combined state vector and $y(t)\in\mathbb{R}^{p}$ is the measurement. Additionally, in FEM-based control problems only a small set of variables may be of particular interest (e.g. end-effector position and velocity). We denote these performance variables as $z(t)\in\mathbb{R}^{o}$ and assume a linear performance output model:

This output model can be used to simplify the formulation of the optimal control problem by expressing the cost function and constraints in terms of the relevant performance variables rather than the full state $x^{f}$ of the FEM model.

In this section we introduce the soft robot optimal control problem (OCP). First, a set of constraints on the performance variables $z$ and control $u$ are assumed to be defined by:

where $\mathcal{U}\vcentcolon=\{u\mid H_{u}u\leq b_{u}\}$ and $\mathcal{Z}\vcentcolon=\{z\mid H_{z}z\leq b_{z}\}$ are convex polytopes with $H_{u}\in\mathbb{R}^{n_{u}\times m}$ and $H_{z}\in\mathbb{R}^{n_{z}\times o}$. Second, a cost function is defined over a finite horizon as:

where $[t_{0},t_{f}]$ defines the time horizon, $Q,Q_{f}\in\mathbb{R}^{o\times o}$ are positive semi-definite performance variable cost matrices and $R\in\mathbb{R}^{m\times m}$ is a positive definite control cost matrix. The terms $\delta z(t)=z(t)-z_{d}(t)$ represent deviations of the performance variables with respect to a desired target trajectory $z_{d}(t)$, and $\delta u(t)=u(t)-u_{d}(t)$ represents deviations from a desired target input $u_{d}(t)$. For example, in trajectory tracking tasks $z_{d}(t)$ defines the reference trajectory (and typically $u_{d}(t)=0$), and in regulation tasks $z_{d}(t)=z_{0}$, $u_{d}(t)=u_{0}$ where $z_{0}$ and $u_{0}$ are the desired equilibrium values.

The FEM-based soft robot constrained optimal control problem is then formulated as:

A common approach for solving these types of optimal control problems is to transcribe the finite-horizon OCP into a nonlinear programming problem that can be solved with existing numerical algorithms. This problem can then be used for closed-loop feedback control by repeatedly solving the optimization problem in a receding horizon fashion.

However, practical implementations of receding horizon control schemes require the OCP to be solved in real-time, which is challenging in this context for several reasons. First, the FEM model (1) is nonlinear and therefore the OCP contains non-convex constraints. Second, even the simple case where the optimization problem is a convex quadratic program has a computational complexity that is $\mathcal{O}(T(\mathcal{N}+m)^{3})$, where $T$ is the number of time steps in the OCP horizon [21]. This generally precludes the direct use of FEM models for optimization-based control of soft robots since fine spatial discretizations (a large number of nodes $\mathcal{N}$) are required for high-fidelity modeling. In fact, it is not uncommon for the full state dimension $n^{f}$ to be on the order of thousands to tens of thousands, making it impossible to solve the OCP in real-time with existing state-of-the-art optimization algorithms.

Our proposed approach to address this computational challenge is to leverage model order reduction techniques to derive a high-fidelity (but low-dimensional) approximation to the original soft robot FEM model. This reduced order model can then be directly used for real-time soft robot control.

In this work we consider projection-based methods, which are a general class of model order reduction methods that project the high-dimensional model onto a reduced order subspace and include widely-used approaches such as balanced truncation and proper orthogonal decomposition (POD) [13]. In particular, we use POD to define a Galerkin projection specified by a projection matrix $UU^{T}$, where $U\in\mathbb{R}^{3\mathcal{N}\times r}$ is an orthogonal basis matrix and $r$ is the dimension of the reduced-order subspace (typically $r\ll 3\mathcal{N}$). This projection is applied to the high-dimensional FEM position and velocity vectors to define reduced order position and velocity vectors:

and to approximately reconstruct the original states by:

where ${q^{f}_{\text{ref}}},{v^{f}_{\text{ref}}}\in\mathbb{R}^{3\mathcal{N}}$ are constant reference states that are used to better condition the basis matrix $U$. In particular, we select ${q^{f}_{\text{ref}}}$ and ${v^{f}_{\text{ref}}}$ to correspond to a static equilibrium (i.e. ${v^{f}_{\text{ref}}}=0$) of the soft robot.

The reduced order model is then defined by projecting the FEM model (1) onto the reduced order subspace:

with $M=U^{T}M^{f}U$, $P=U^{T}P^{f}$, and ${v_{\text{ref}}}=U^{T}{v^{f}_{\text{ref}}}$. Additionally, with the combined reduced order state $x=[{v}^{T},{q}^{T}]^{T}\in\mathbb{R}^{n}$ (where $n=2r$) and reference state ${x^{f}_{\text{ref}}}=[{v^{f}_{\text{ref}}}^{T},{q^{f}_{\text{ref}}}^{T}]^{T}\in\mathbb{R}^{n}$, the reduced order measurement and performance models are given by:

where $C_{y}=C_{y}^{f}V$ and $C_{z}=C_{z}^{f}V$ with $V$ defined as $V=\text{blkdiag}(U,U)$, and ${y_{\text{ref}}}=C_{y}^{f}{x^{f}_{\text{ref}}}$ and ${z_{\text{ref}}}=C_{z}^{f}{x^{f}_{\text{ref}}}$ are constants. Crucially, this projection results in a ROM (9) with combined dimension $n$, which can be orders of magnitude smaller than the original FEM model (1) of dimension $n^{f}$.

While the ROM (9) has a reduced dimension, evaluation of the nonlinear terms is still computationally expensive because of their dependence on the high-dimensional state (e.g. the evaluation of the internal force $U^{T}F^{f}(Uq+{q^{f}_{\text{ref}}},\>Uv+{v^{f}_{\text{ref}}})$ scales as $\mathcal{O}(n^{f})$). In this work we address this by approximating the nonlinear terms by reduced order piecewise affine functions, resulting in a piecewise affine ROM.

Specifically, the piecewise affine approximations are generated by considering $N$ linearization points $(q_{i},v_{i})$, which are represented in the high-dimensional space by $q^{f}_{i}=Uq_{i}+{q^{f}_{\text{ref}}}$ and $v^{f}_{i}=Uv_{i}+{v^{f}_{\text{ref}}}$. Around each of these linearization points, the internal forces $F^{f}$ are approximated by a first-order Taylor expansion:

The internal force terms from (9) can then be written as:

with reduced order terms:

The input matrix $H^{f}$ in (9) is then simply approximated by the zeroth-order Taylor series expansion:

With these simplifications the ROM (9) can be approximated in the vicinity of $x_{i}=[v_{i}^{T},q_{i}^{T}]^{T}$ by:

where:

Combined, these $N$ linearization points provide a global approximation of (9) via the piecewise affine ROM:

where $W\in\mathbb{R}^{n\times n}$ is a positive semi-definite weighting matrix that defines a distance metric for determining the nearest linearization point. For example, we choose $W=\text{blkdiag}(0,I)$ for the soft robot in Figure 1 since the internal forces are heavily dependent on the robot’s configuration $q^{f}$.

Since the matrices $A_{i}$, $B_{i}$, and $d_{i}$ are of reduced order, the ROM (12) can easily be stored in memory, efficiently used for simulation, and its Jacobians can be trivially extracted for use in optimization-based control. A discrete-time version of this model can be defined by:

where $A_{i,d},B_{i,d}$ and $d_{i,d}$ are the discretized reduced order matrices, which can also be computed a priori.

Not only is the use of piecewise affine models common for nonlinear control applications, it is also a popular approach in the model order reduction community and is known as the trajectory piecewise linear (TPWL) method [22].

This section proposes an automated approach for the development of the piecewise affine ROM (12), specifically the computation of the reduced order basis matrix $U$ and the selection of the linearization points $(q_{i},v_{i})$. This process relies on simulations of the high-dimensional FEM model (1), but can be done entirely offline.

As mentioned in Section III-A, the basis matrix $U$ that defines the reduced order subspace is computed via POD, a data-driven method for compressing high-dimensional physics models. In this work, POD is implemented by simulating the FEM model (1) to collect a set of “snapshots”, which can include the soft robot’s configuration $q^{f}$, velocity $v^{f}$, and acceleration $\dot{v}^{f}$, and implicitly defines a basis that characterizes the robot’s behavior. A principal component analysis of the snapshots is then used to identify the reduced order subspace. In particular, the subspace is selected by defining a “snapshot matrix” $S$ with columns corresponding to snapshots, and then computing a singular value decomposition $S=\bar{U}\bar{\Sigma}\bar{V}$. The basis matrix $U$ is then defined by taking the $r$ columns of $\bar{U}$ associated with the $r$ largest singular values. The dimension of the subspace, $r$, is typically chosen to be as small as possible while still providing good approximation accuracy. In practice, a commonly used heuristic for quantifying the approximation accuracy is the “energy” of the truncated singular values [13].

The linearization points used to define the piecewise affine ROM (12) are also determined via an offline data-driven procedure. In particular, at each time step of a FEM simulation a linearization point is added to the ROM if the reduced order state predicted by the ROM diverges too significantly from the FEM result. In other words, the ROM is built incrementally over the course of the simulation.

For good ROM accuracy, the FEM simulation that is used for POD snapshot collection and linearization point selection should sufficiently cover the full range of possible robot motions. To ensure sufficient data is collected, a simple yet effective approach is to apply an open-loop control sequence in the FEM simulation that approximately spans the range of possible actuations for the robot. Specifically, we choose this sequence through Latin hypercube sampling of the soft robot’s admissible actuations. A summary of the automated procedure for developing the piecewise affine ROM is given in Algorithm 1, where FEM simulates the FEM model (1) over one time step, POD computes the reduced order basis $U$, project corresponds to the projection (7), and ROM corresponds to simulating the piecewise affine ROM (12).

A discretized optimal reduced order trajectory $(\mathbf{x}^{*},\mathbf{u}^{*})_{k}=(\{x^{*}_{i}\}_{i=k}^{k+T},\{u^{*}_{i}\}_{i=k}^{k+T-1})$ for the robot is defined over a finite horizon $T$ by solving a reduced order approximation of the high-dimensional OCP (6):

where $i=k,\ldots,k+T-1$, and $x_{0,k}$ is the initial state at time step $k$. This finite horizon problem is then solved in a receding horizon fashion to define the optimal trajectory over an arbitrarily long horizon. Specifically, the OCP is initialized at time $k=0$ by setting $x_{0,0}=\hat{x}_{0}$, where $\hat{x}_{0}$ is the reduced order state estimate. The OCP is then recursively solved every $T_{r}<T$ time steps (i.e. at time steps $k=T_{r},2T_{r},\dots$ over the receding horizon $[k,k+T]$) by setting $x_{0,k}=x^{*}_{k}$, where $x^{*}_{k}$ is the optimal state from the previous solution (computed at time step $k-T_{r}$).

We solve the nonconvex OCP (14) using sequential convex programming (SCP), by solving a sequence of quadratic program (QP) approximations until convergence. Specifically, we use a slightly modified version of [23]. Crucially, since the ROM is constructed such that $n\ll n^{f}$ this approach can enable real-time control.

The reduced order OCP (14) defines an optimized open-loop reduced order trajectory that the robot should follow. A simple output feedback control scheme is now defined to drive the robot to track this trajectory.

To estimate the robot’s current reduced order state from measurements a reduced order state estimator is defined as:

where $L_{k}\in\mathbb{R}^{n\times p}$ is the estimator gain, $y_{k}$ is the robot measurement, and $u_{k-1}$ is the previous control input. We choose the gain $L_{k}$ to be the extended Kalman filter gain based on the discrete-time ROM dynamics (13).

The control applied to the robot is then computed using a reduced order linear feedback control law:

where $G_{k}\in\mathbb{R}^{m\times n}$ is the controller gain matrix, and $(x^{*}_{k},u^{*}_{k})$ is the optimized state-input pair computed by the OCP (14). To define the controller gains $G_{k}$ we propose a simple and computationally efficient method based on gain scheduling:

where the gains $G_{i}$ are the discrete-time LQR gains at each linearization point $(q_{i},v_{i})$, which can be computed a priori. In particular, the gains $G_{i}$ are computed using the discrete-time reduced order dynamics matrices $A_{i,d}$ and $B_{i,d}$, and positive semi-definite cost matrix $Q_{G}\in\mathbb{R}^{n\times n}$ and positive definite control cost matrix $R_{G}\in\mathbb{R}^{m\times m}$ (e.g. $Q_{G}=C_{z}^{T}QC_{z}$ and $R_{G}=R$).

The formulation of the proposed control scheme has a couple of practical advantages. First, since the OCP (14) defines the initial condition based on the previous solution the next solve can be started as soon as the previous solve is completed. For example, suppose the optimized trajectory $(\mathbf{x}^{*},\mathbf{u}^{*})_{k}$ has already been computed at time step $k$, then the OCP problem associated with time step $k+T_{r}$ can be solved starting at time step $k$ since $x^{*}_{k+T_{r}}$ is already known. If the model is discretized with a sampling time of $h$ this gives $hT_{r}$ seconds to solve the next OCP. Additionally, this choice of initialization can help with warm-starting of the SCP algorithm. Second, even though the optimal trajectory computed by the OCP is discretized with a sampling time $h$, the state estimator and control law can be operated at a higher frequency by simply interpolating the optimal trajectory $(\mathbf{x}^{*},\mathbf{u}^{*})_{k}$.

For the proposed approach we construct a piecewise affine ROM for the Diamond robot using the procedure in Algorithm 1. In particular, we use acceleration snapshots in the POD step to define a reduced order subspace with dimension $r=21$ (i.e. $n=42$), we choose ${v^{f}_{\text{ref}}}=0$, and select ${q^{f}_{\text{ref}}}$ to be the equilibrium configuration of the robot with no actuation. For selecting the linearization points we choose the threshold parameter $\eta$ (with $W_{q}=0,W_{v}=I$) such that a total of $N=642$ points $(q_{i},v_{i})$ are selected. Additionally, the piecewise affine ROM is defined with $W=\text{blkdiag}(0,I)$, such that only the robot’s configuration $q$ is used to select the nearest linearization point. The reduced order OCP (14) is then defined with a horizon of $T=5$, a rollout horizon of $T_{r}=2$, and the ROM is discretized with a sampling time of $h=0.05$ seconds. By interpolating the optimal trajectory computed by the OCP, the feedback controller (16) and state estimator (15) operate with a sampling time of $0.01$ seconds.

The ROMPC controller [17] uses a ROM generated by linearizing the FEM model (1) around the point ${x^{f}_{\text{ref}}}$ and then reusing the POD projection from our proposed controller. We consider the same horizons $T,T_{r}$ and sampling time $h$ as in our controller, and the ROMPC feedback controller and state estimator also operate with a sampling time of $0.01$ seconds by interpolating the optimized trajectory. Finally, for the Koopman operator-based controller [10] we build a model for the performance variables $z$ with a delay $d=1$ and all monomials of maximum degree $2$ to define a lifted state of dimension $N_{K}=66$ and a sampling time of $h=0.05$ seconds. The data used to build this model came from the same FEM model simulation used to define our proposed controller.

Simulation results of the Diamond FEM model are presented for each controller in Figures 2 and 3. As can be seen, the ROMPC scheme, which uses a linearized ROM, offers poor tracking and severely violates the constraints. In contrast, our approach and the Koopman controller offer good tracking performance and generally satisfy the constraints, highlighting the importance of capturing the soft robot’s nonlinear behavior.

To demonstrate the computational requirements of each controller we report the amount of time spent solving QPs (which is the most significant computational component) in Table I. Note that the ROMPC and Koopman controllers only solve one QP at a time while the proposed SCP approach may require multiple QPs to be solved. These results show that the proposed FEM model-based control scheme is real-time capable.

Our proposed approach offers similar performance to the Koopman operator-based controller in this simulation (see Table I), while offering several advantages. First, the Koopman approach only models the behavior of a prespecified choice of the performance variables $z$, while the FEM model in our approach captures the robot’s entire state (independent of the choice of $z$) and can therefore be used for any number of different control problems. In contrast to our approach, the Koopman controller is also restricted to only consider outputs $z$ that are a subset of the measured variables $y$. Second, the dimension of the Koopman model does not scale well with the number of measured variables. For example, including all the measured variables $y$ (i.e. setting $z=y\in\mathbb{R}^{30}$) would result in a model of dimension $N_{K}=2145$ (using the same delay $d=1$ and all monomials with maximum degree $2$). Third, the Koopman controller must operate at whatever frequency the model is discretized at (and the frequency the QP is solved at), while our controller can be operated at much higher frequencies by subsampling the optimized trajectory.