Mediating Between Contact Feasibility and Robustness of Trajectory Optimization Through Chance Complementarity Constraints

As robots move from the laboratory into the real world, motion planning will need to account for model uncertainty and risk. For robot motions involving intermittent contact, planning for uncertainty in contact is especially important, as failure to successfully make and maintain contact can be catastrophic. Here, we model uncertainty in terrain geometry and friction characteristics, and combine a risk-sensitive objective with chance constraints to provide a trade-off between robustness to uncertainty and constraint satisfaction with an arbitrarily high feasibility guarantee. We evaluate our approach in two simple examples: a push-block system for benchmarking and a single-legged hopper. We demonstrate that chance constraints alone produce trajectories similar to those produced using strict complementarity constraints; however, when equipped with a robust objective, we show the chance constraints can mediate a trade-off between robustness to uncertainty and strict constraint satisfaction. Thus, our study may represent an important step towards reasoning about contact uncertainty in motion planning.

1 Introduction

As robots move into the real world, accounting for model uncertainty and risk in motion planning will become increasingly important. While model-based planning and control has demonstrated success in designing and executing dynamic motion plans for robots in a variety of tasks in the laboratory (Mordatch et al., 2012; Dai et al., 2014; Winkler et al., 2018; Patel et al., 2019), real world environments are difficult or intractable to precisely model, and as such the resulting motion plans could be prone to failure due to modeling errors. Planning for uncertainty and risk is especially important when the task involves intermittent contact, as incorrectly modeling friction can cause robots to drop and break objects or slip and fall, and incorrectly modeling contact geometry can cause mobile robots to trip and fall or collide with obstacles. While decent controller design can mitigate the effects of small modeling errors and disturbances (Toussaint et al., 2014; Gazar et al., 2020), incorporating uncertainty and risk into planning can help improve performance by generating reference trajectories that have a high success rate for execution.

Trajectory optimization (TO) is powerful for planning continuous dynamic motions that obey constraints such as actuation limits, obstacle avoidance, and contact dynamics (Dai and Tedrake, 2012; Dai et al., 2014; Posa et al., 2014; Mordatch et al., 2015; Kuindersma et al., 2016; Carius et al., 2018; Yeganegi et al., 2019; Gazar et al., 2020). While the optimal strategies produced by TO typically lie on the boundary of the feasible region, recent works have begun to incorporate risk and uncertainty to improve the robustness of the planned motion. Uncertainty about the state or dynamics can be accounted for by an expected exponential transformation of the cost, resulting in risk-sensitive TO (Farshidian and Buchli, 2015; Ponton et al., 2018). Alternatively, uncertainty about the constraints has been approached by defining failure probabilities and optimizing for motion plans that do not exceed some user-defined total failure probability (Hackett et al., 2020; Shirai et al., 2020). Planning under contact uncertainty, however, has only recently begun to be investigated. One recent work developed a risk-sensitive cost term to plan for uncertainty in the contact model for systems with intermittent contact (Drnach and Zhao, 2021). However, while the robust cost formulation for uncertainty in contact produced robust trajectories, it also produced infeasible motion plans at high uncertainty, including setting friction forces to zero during sliding and allowing for positive contact reactions at nonzero contact distance.

In this work, we explicitly investigate uncertainty resulting from the terrain contact parameters and develop a method for trading off between motion feasibility and robustness. In contrast to the previous work (Drnach and Zhao, 2021), which controlled robustness only by varying the uncertainty, we aim to achieve a tradeoff at fixed uncertainty by introducing tunable risk parameters. Specifically, we:

• Design chance constraints for contact with uncertainty in contact distance and friction coefficient.

• Provide a risk-bounded interpretation to the relaxed chance complementarity constraints.

• Demonstrate that chance constraints, combined with a contact-sensitive objective, can control the trade-off between robustness to contact uncertainty and contact constraint satisfaction at fixed values of uncertainty.

2 Related Work

2.1 Contact-Robust Trajectory Optimization

Planning motions for robots with intermittent contact can be achieved through either hybrid (Dai and Tedrake, 2012; Dai et al., 2014) or contact-implicit TO (Mordatch et al., 2012; Posa et al., 2014; Patel et al., 2019). In the hybrid case, contact is modeled by specifying end-effector location at contact and defining constrained dynamics for each mode. Robustness to contact uncertainty has been studied by sampling contact locations and minimizing an expected cost (Dai and Tedrake, 2012; Seyde et al., 2019), by using Bayesian optimization to learn a robust cost function (Yeganegi et al., 2019), and by constraining the risk of slipping (Shirai et al., 2020). However, developing general methods for contact uncertainty is difficult within the hybrid optimization framework as contact conditions are specified in the dynamical modes.

In contrast, contact-implicit methods specify contact through a complementarity model which includes the nearest contact distance and friction coefficient (Stewart and Trinkle, 1996; Posa et al., 2014), and thus may provide a natural avenue for representing and planning for uncertainty in contact. Despite this potential, there have been few works exploring contact uncertainty within the contact-implicit framework. In (Mordatch et al., 2015), contact point locations were sampled and an expected cost was minimized to produce robust motions. Recently, uncertainty in contact was modeled using probabilistic residual functions, and the expected residual was added to the cost to produce contact-sensitive trajectories (Drnach and Zhao, 2021), at the expense of producing potentially infeasible trajectories as uncertainty increased.

2.2 Chance Constraints

To trade-off between robustness and constraint satisfaction, chance constraints can be added to an optimization problem to enforce a probabilistic version of the uncertain constraints (Mesbah, 2016; Celik et al., 2019; Paulson et al., 2020). Chance constraints model uncertainty by defining a probability of constraint satisfaction, which can be tuned to enforce a conservative constraint or to relax the constraint. Previous works have achieved robust vehicle trajectory planning under obstacle (Blackmore et al., 2011) and agent (Wang et al., 2020) uncertainty using chance constraints. Chance constraints have also been applied to robot locomotion to increase the likelihood of avoiding collision with obstacles in uncertain locations (Gazar et al., 2020), or to model slipping risk due to errors in the friction model (Brandão et al., 2016; Shirai et al., 2020). In contrast to collision avoidance, intermittent contact with the environment is required for robot locomotion, and while chance constraints have been applied to parts of the contact problem, they have yet to be applied to the full complementarity constraints for contact. Here, we investigate if chance constraints can trade-off between constraint satisfaction and robustness under contact uncertainty by combining them with our previously developed robust objectives (Drnach and Zhao, 2021).

3 Problem Formulation

In this section, we present a robust contact-implicit TO with both contact-robust costs and chance constraints to provide robustness to contact uncertainties while maintaining the feasibility of physical contact models.

3.1 Contact-Implicit Trajectory Optimization

Planning robot motions that are subject to contact reaction forces can be achieved through contact-implicit TO (Posa et al., 2014). The traditional problem solves for generalized positions q, velocities v, controls u, and contact forces λ through a discretized optimal control problem:

$\underset{\mathbf{h,q,v,u,}\mathit{\lambda },\mathit{\gamma }}{\min }\sum _{k=0}^{K-1}{h}_{k}L\left(x_k,u_k,{\lambda }_k\right)(\mathrm{1a})$

$\text{s.t.}\left\{\begin{array}{c}{x}_0=x(0),x_K=x(T_f)\left(1\text{b}\right)\hfill \\ M(v_{k+1}-v_k)+C=B{u}_{k+1}+J_c^{\top }{\lambda }_{k+1}\left(1\text{c}\right)\hfill \\ 0\le {\lambda }_{N,k+1}\perp \varphi (q_{k+1})\ge 0\left(1\text{d}\right)\hfill \\ 0\le {\lambda }_{T,k+1}\perp {\gamma }_{k+1}+J_T{v}_{k+1}\ge 0\left(1\text{e}\right)\hfill \\ 0\le {\gamma }_{k+1}\perp \mu {\lambda }_{N,k+1}-e^{\top }{\lambda }_{T,k+1}\ge 0\left(1\text{f}\right)\hfill \\ \hfill \forall k\in \{0,\dots ,K-1\}\hfill \end{array}\right.$

where L is the running cost, h_k is the timestep, x = (q, v) is the state, Eq. 1b are boundary constraints, M is the generalized mass matrix, C contains Coriolis and conservative force effects, B is the control selection matrix, J_c is the contact Jacobian, λ_N and λ_T are the normal and tangential contact reaction forces, ϕ is the contact distance, γ is a slack variable corresponding to the magnitude of the sliding velocity, μ is the coefficient of friction, and e is a matrix of 1s and 0s.

The contact Jacobian can be decomposed into normal and tangential components, $J_c^{\top }=[J_N^{\top },J_T^{\top }]$. The normal component $J_N^{\top }$ maps the normal reaction force at the contact point to the generalized joint torques and is derived by projecting the contact point Jacobian onto the surface normal at the nearest contact point. The tangential component $J_T^{\top }$ maps the frictional forces at the contact point to generalized torques, and is the projection of the contact point Jacobian onto the plane tangent to the contact surface at the nearest point of contact.

Eqs. 1d, f are complementarity constraints governing intermittent contact with the environment, where the notation 0 ≤ a ⊥ b ≥ 0 represents the complementarity constraints a ≥ 0, b ≥ 0, ab = 0. Eq. 1d enforces that normal contact reaction forces are only imposed when the distance between the two objects is zero. Likewise (1e) and (1f) govern the sticking and sliding phases of friction; when in sliding (1f) forces the friction forces to the edge of the friction cone and (1e) requires γ and the corresponding relative tangential velocities to be nonzero. In sticking, however (1f) forces the variable γ to zero and (1e) requires the corresponding relative tangential velocity to also be zero. We replaced the friction cone with a polyhedral approximation (Stewart and Trinkle, 1996), denoted by the use of the e in (1f), which contains only 1s and 0s, instead of the use of the 2-norm, and we consider λ_T to be the non-negative components of the friction force projected onto the polyhedron. The polyhedral approximation presented here can readily extend to the full three-dimensional case, although we do not study three-dimensional contact in this work.

In general, the running cost is a function of all the decision variables, including the timesteps, states, controls, and reaction forces. However, in this work, we use a quadratic function of only the states and controls:

$L\left(x_k,u_k,{\lambda }_k\right)={\left(x_k-x\left(T_f\right)\right)}^{\top }Q\left(x_k-x\left(T_f\right)\right)+u_k^{\top }R{u}_k.$

where R is the weight matrix on the control effort and Q is the weight matrix on the deviation from the final state. Our initial cost design does not depend on the reaction forces λ, although this is purely a design choice. Quadratic costs are common in the optimal control literature (Posa et al., 2014; Kuindersma et al., 2016; Patel et al., 2019), although other cost functions can be used, such as the cost of transport (Posa et al., 2014).

Problem (1) is a mathematical program with equilibrium constraints, a type of nonlinear program (NLP) that can be difficult to solve. Two approaches to solve the problem numerically using standard NLP solvers like SNOPT (Gill et al., 2005) include relaxing the complementarity constraints ab ≤ ϵ (Figure 1D) and solving the problem from progressively smaller values of ϵ (Scholtes, 2001; Posa et al., 2014; Manchester et al., 2019), and replacing the constraints with an exact penalty term ρab in the cost, where ρ is chosen sufficiently large to drive the term ab to zero (Baumrucker and Biegler, 2009; Patel et al., 2019). In this work, we found that the choice to use either the ϵ-relaxation method or the exact penalty method was problem dependent. We also note that the robust cost we use is a probabilistic variant of the penalty method.

FIGURE 1

FIGURE 1. (A,B) Contact geometry of the hopper and block examples, respectively, with uncertainty in (A) terrain height and (B) friction coefficient. (C) Gaussian distribution with mean m and standard deviation σ, where p(Z < z) = θ. (D) Relaxed complementarity constraint region for comparison with (E) chance complementarity constraint feasible regions for different risk bounds. (F) Overlap between ERM cost map and chance relaxed feasible region at σ = 10. At high uncertainty, low ERM values approach the positive m_F axis and the chance constraint region widens around the non-negative z axis.

3.2 Expected Residual Minimization

The complementarity constraints in (1) assume that perfect information about the contact model is available. However, if any of the model parameters are uncertain, the problem has stochastic complementarity constraints (SCP) (Luo and Lu, 2013) 0 ≤ z ⊥ F(z, ω) ≥ 0, ω ∈ Ω where z is a deterministic variable, and F(⋅) is a vector-valued stochastic function, and ω represents a random variable on probability space $(\mathrm{\Omega },\mathcal{F},\mathcal{P})$, with sample space Ω, event space $\mathcal{F}$, and probability distribution $\mathcal{P}$.

Prior works on SCPs (Chen et al., 2009; Tassa and Todorov, 2010; Luo and Lu, 2013) commonly replace the complementarity constraint with a residual function ψ that attains its roots when the complementarity constraints are satisfied: ψ(z, F) = 0⇔z ≥ 0, F ≥ 0, zF = 0. Although this formulation is for scalars z and F, it generalizes to the case when z and F are vectors by applying the complementarity constraints and/or the residual function elementwise. In the Expected Residual Minimization (ERM) approach (Chen et al., 2009; Tassa and Todorov, 2010), the expected squared residual is minimized:

$\underset{z}{\min }\mathbb{E}\left[{‖\psi \left(z,F\left(z,\omega \right)\right)‖}^2\right](2)$

One advantage of the ERM is that its solutions have minimum sensitivity to random variations in the parameters (Chen et al., 2009).

Prior work using an ERM cost to plan for uncertainty in contact resulted in solutions that were robust to variations in the contact parameters (Drnach and Zhao, 2021). However, while the ERM method produced robust trajectories, as contact uncertainty increased, it also produced trajectories which were infeasible with respect to the expected values of the constraints. In this work, we use an ERM cost for Gaussian-distributed friction coefficient and normal distance (Tassa and Todorov, 2010; Drnach and Zhao, 2021), and we add the ERM to the running cost as:

$\underset{\mathbf{z}=\{\mathbf{x},\mathbf{u},\mathit{\lambda }\}}{\min }\sum _{k=0}^{K-1}\left(L\left(x_k,u_k,{\lambda }_k\right)+\alpha \mathbb{E}\left[{‖\psi \left(z_k,F\left(z_k,\omega \right)\right)‖}^2\right]\right)(3)$

where α is a penalty weighting factor selected to keep the ERM cost a few orders of magnitude higher than the other cost terms, as in the penalty method. In (3), the variable z_k and the function F are generic decision variables and constraint functions, respectively. In our work, we consider uncertainty in the terrain geometry and in the friction coefficient separately. In the case of uncertain terrain geometry, F is the normal distance function ϕ(q) and z includes the normal forces λ_N. Likewise, in the case of uncertainty in friction, F is the linearized friction cone in (1f) and z includes the sliding velocity slack variable γ.

3.3 Chance Complementarity Constraints

Chance constraints are another general method for encoding uncertainty into constraints. Optimization with chance constraints enforces that the constraint is satisfied to within some user-specified probability, $\text{Pr}(z\in \mathcal{Z})\ge 1-\theta$, where $\mathcal{Z}$ is the constraint set and θ is the specified probability of violation (Figure 1C). In this as in other works, we assume that z is Gaussian, $z\sim \mathcal{N}({\mu }_z,\mathrm{\Sigma })$, and that the constraint is linear, $\mathcal{Z}=\{z|c^{\top }z\le b\}$ (Blackmore et al., 2011). In this case, we can write the chance constraint using the error function erf (Celik et al., 2019):

$\text{Pr}\left(c^{T}z\le b\right)=\frac{1}{2}\left(1+\text{erf}\left(\frac{b-c^T{m}_z}{\sqrt{2c^T\mathrm{\Sigma }c}}\right)\right)\ge 1-\theta (4)$

$\Rightarrow c^T{m}_z\le b-\sqrt{2c^T\mathrm{\Sigma }c}{\text{erf}}^{-1}\left(1-2\theta \right)(5)$

As erf⁻¹ takes values in (−1, 1), Eq. 5 can represent either a relaxed (θ > 0.5) or a conservative (θ < 0.5) constraint.

To complement the robust ERM approach, in this work we investigate contact uncertainty by converting the stochastic complementarity constraints to deterministic, chance complementarity constraints. As with the Gaussian ERM, we assume the complementarity function is normally distributed $F\sim \mathcal{N}(m_F,{\sigma }^2)$, and we place probabilistic requirements on the components of the complementarity constraints Pr(F ≥ 0) ≥ 1 − β and Pr(zF ≤ 0) ≥ 1 − θ. Assuming that z is a deterministic variable, by Eq. 5 we have the following chance-complementarity constraints:

$z\ge 0,m_F\ge -\sqrt{2}\sigma {\text{erf}}^{-1}\left(2\beta -1\right),z{m}_F\le -\sqrt{2}z\sigma {\text{erf}}^{-1}\left(1-2\theta \right)(6)$

Remark 1. If either σ = 0 or β = θ = 0.5, then the chance constraints recover the strict complementarity constraints.

Remark 2. If β = 0.5 and θ > 0.5, we recover a relaxed version of the complementarity constraints (Figure 1E): z ≥ 0, m_F ≥ 0, zm_F ≤ ϵ where $ϵ=-\sqrt{2}z\sigma {\text{erf}}^{-1}(1-2\theta )>0$.

Remark 3. If β ≥ 1 − θ, z > 0, the chance constraints relax the complementarity constraints into a tube around the mean:

$-\sqrt{2}\sigma {\text{erf}}^{-1}\left(2\beta -1\right)\le m_F\le -\sqrt{2}\sigma {\text{erf}}^{-1}\left(1-2\theta \right)(7)$

Note that, in this case, the chance constraints provide potentially asymmetric upper and lower bounds on the constraint violation, as by assumption z > 0. For example, if m_F and z represent the normal distance and normal force, the chance constraints provide upper and lower bounds for the distance at which a non-zero normal force can be applied.

We also note that chance constraints cannot provide robustness by making the complementarity constraints more conservative, as the original constraints have an empty interior. In contrast, previous works have used chance constraints to achieve robustness to uncertainty by removing part of the interior of the constraint set, making the constraint more conservative (Gazar et al., 2020; Shirai et al., 2020). Chance complementarity constraints, however, always provide a relaxation of the original constraints, and give a probabilistic interpretation to previous methods using relaxed constraints (Manchester et al., 2019; Patel et al., 2019).

The chance complementarity constraints presented here possess nonempty solution sets only when β > 1 − θ; however, we note that not every choice of β and θ is recommended, as choosing θ > 0.5 and β < 0.5 requires the mean value m_F to be strictly positive, whereas choosing θ < 0.5 forces the mean m_F to be strictly negative, both of which induce a bias into the complementarity problem. Therefore, we recommend further restricting the choice of parameter values to β, θ ≥ 0.5, as this choice ensures the mean m_F can be zero, but still allows m_F to take on positive and negative values.

In this work, we apply the chance constraints to relax the friction cone constraint (Eq. 1f) and the normal distance constraint (Eq. 1d), assuming normal distributions over the friction coefficient and the normal distance. We also include the corresponding ERM cost to examine the effects of chance constraints on the robustness of ERM solutions. We note that the failure probabilities β, θ can also be interpreted as risk bounds (Shirai et al., 2020). By varying these risk bounds, we examine the tradeoff between strict feasibility under the expected value of the constraint when β, θ = 0.5 and robustness to parameter variations under the ERM cost when β, θ > 0.5.

3.4 Chance Constrained Contact Robust Trajectory Optimization

In this work, we use both the ERM cost (Eq. 3) and the chance constraints (Eq. 6) to model uncertainty in the contact constraints of contact-implicit TO (Eq. 1). When applying our methods to uncertainty in contact distance, our contact-robust TO follows as:

$\underset{\mathbf{h,q,v,u,}\mathit{\lambda },\mathit{\gamma }}{\min }\sum _{k=0}^{K-1}{h}_{k}L\left(x_k,u_k,{\lambda }_k\right)+\alpha \mathbb{E}\left[{‖\psi \left({\lambda }_{N,k},\varphi \left(q_k\right)\right)‖}^2\right](\mathrm{8a})$

$\text{s.t.}\left\{\begin{array}{c}{x}_0=x(0),x_K=x(T_f)\left(8\text{b}\right)\hfill \\ M(v_{k+1}-v_k)+C=B{u}_{k+1}+J_c^{\top }{\lambda }_{k+1}\left(8\text{c}\right)\hfill \\ {\lambda }_{N,k+1}\ge 0,\varphi (q_{k+1})\ge -\sqrt{2}\sigma {\text{erf}}^{-1}(2\beta -1),\hfill \\ {\lambda }_{N,k+1}\varphi (q_{k+1})\le -\sqrt{2}{\lambda }_{N,k+1}\sigma {\text{erf}}^{-1}(1-2\theta )\left(8\text{d}\right)\hfill \\ 0\le {\lambda }_{T,k+1}\perp {\gamma }_{k+1}+J_T{v}_{k+1}\ge 0\left(8\text{e}\right)\hfill \\ 0\le {\gamma }_{k+1}\perp \mu {\lambda }_{N,k+1}-e^{\top }{\lambda }_{T,k+1}\ge 0\left(8\text{f}\right)\hfill \\ \hfill \forall k\in \{0,\dots ,K-1\}\hfill \end{array}\right.$

Likewise, we can also apply the chance constraints and ERM cost to the friction constraint to derive an optimization that is robust against friction.

Throughout our work, we compare our contact-robust TO against the standard contact-implicit TO in Eq. 1 and against a contact-robust TO with the ERM cost only; that is, without the chance constraints. Under contact distance uncertainty, the ERM-only robust optimization is identical to Eq. 8a, except without the chance distance constraint (Eq. 8d). Likewise, under friction uncertainty the ERM-only robust optimization has only the ERM cost for friction, and not the associated chance constraints.

3.5 Quantifying Feasibility

To quantify the feasibility of our solutions, we adopt a modified merit function $\mathcal{M}(z)$ (Seyde et al., 2019):

$\mathcal{M}\left(z\right)=\frac{1}{K}\sum _{k=0}^{K-1}\left(g_{EC,k}{\left(z\right)}^2+\min {\left(0,g_{IC,k}\left(z\right)\right)}^2\right)(9)$

where g_EC are the equality constraints, g_IC are the inequality constraints, and z are the decision variables. Here, the merit score only penalizes constraint violation, and provides a quantification of the feasibility of the solutions. For the purposes of this study, we focus solely on contact feasibility under the expected value of the uncertain contact parameters, and apply the merit score to the friction cone constraint (Eq. 1f) for frictional uncertainty and to the normal distance constraint (Eq. 1d) for contact distance uncertainty.

4 Simulation Experiments

We compared the chance-constrained risk-sensitive optimization approach to the ERM-only risk-sensitive approach (Drnach and Zhao, 2021) and the traditional non-robust optimization approach in two experiments: a block sliding over a surface with uncertain friction and a single-legged hopper robot hopping over a flat terrain with uncertain height. All our examples were implemented in Python 3 using Drake (Russ Tedrake and the Drake Development Team, 2019) and solved using SNOPT (Gill et al., 2005) to major optimality and feasibility tolerances of 10^–6. Unless otherwise noted, all of our robust and chance-constrained problems were initialized with the reference, non-robust solution, and we used the same value for uncertainty σ in the ERM objective as in the chance-constraints. Our code is available at https://github.com/GTLIDAR/ChanceConstrainedRobustCITO.

4.1 Sliding Block With Uncertain Friction

Our first example is a planar 1 m, 1 kg cube sliding over a surface with nonzero friction (Figure 1B). The state of the system x = [p_CoM, v_CoM] includes the planar position and velocity of the center of mass of the block, p_CoM and v_CoM respectively, and the control u is a horizontal force applied at the center of mass. We optimized for a 1s trajectory, discretized with 101 knot points, to travel between the initial state x₀ = [0,0.5,0,0]^⊤ and final state x_N = [5,0.5,0,0]^⊤. The running cost had weight matrices R = 10 and Q = diag([1, 1, 1, 1]). We first solved the optimization to a tolerance of 10^–6 and then to 10^–8; in this example, solving to the tighter tolerance improves the visual quality of the solutions. In the reference trajectory, we used friction coefficient μ = 0.5. For the uncertain cases, we assumed a mean friction of $\bar{\mu }=0.5$ and tested under 5 uncertainties σ ∈ {0.01, 0.05, 0.10, 0.30, 1.00}. When including chance constraints, we tested several combinations of the risk bounds θ, β ∈ {0.51, 0.6, 0.7, 0.8, 0.9} For completeness, we also tested the chance constraints without the ERM cost for uncertainties σ ∈ {0.1, 1.0}. We quantified the feasibility of our motion plans using the merit score (Eq. 9) with the expected friction cone constraint (Eq. 1f), and we quantified the robustness using the maximum sliding velocity, as a higher velocity indicates less time in sliding.

We evaluated the performance of the non-robust reference controls, the ERM controls, and the ERM with chance constraints controls in open-loop time-stepping simulations (Stewart and Trinkle, 1996). To evaluate the robustness, we perturbed friction with 4 values uniformly spaced between μ = 0.3 and μ = 0.7 and evaluated the control performance as the difference between the block position at 1s and the target position. We quantified robustness as the range of final position errors under all friction perturbations. We further evaluated the effect of the risk bounds on performance by first testing the chance constraints across a range of friction uncertainties with θ, β = 0.7. We also evaluated the performance of the chance constraints at high uncertainty (σ = 1.0) by testing 9 combinations of β, θ ∈ {0.51, 0.7, 0.9}.

4.2 Single-Legged Hopper Over an Uncertain Terrain

Our second example is a 2D single-legged hopper with collision points at the toe and heel. The configuration q includes the planar position (horizontal and vertical) of the base p_CoM and the angles of the hip θ_H, knee θ_K, and ankle θ_A; that is, q = [p_CoM, θ_H, θ_K, θ_A]. Thus, the state vector is $x=[q,\dot{q}]$, and the controls are the torques on the hip, knee, and ankle joints. In this example, the hopper travels 4 m in 3 s starting and ending at rest with the base 1.5 m above the heel. We used 101 knot points and cost weights R = diag([0.01, 0.01, 0.01]) and Q = diag([1, 10, 10, 100, 100, 1, 1, 1, 1, 1]).

We first solved for the reference trajectory using the exact penalty cost method to enforce the complementarity constraints for contact (Baumrucker and Biegler, 2009; Patel et al., 2019), and we initialized the reference optimization by linearly interpolating between the start and goal states. In our experiments with uncertainty, we assumed known friction coefficient μ = 0.5 and uncertain terrain height with expected distance between initial hopper base height and terrain of 1.5 m. We tested the ERM and ERM with chance constraints approaches under 6 uncertainties roughly logarithmically spaced between σ = 0.001 and σ = 0.5 m. To more effectively utilize the ERM cost at high uncertainty, we scaled the normal distance by 10 during optimization, expressing the distance and its uncertainty in decimeters. At each uncertainty level, we tested 5 values of the chance parameters, θ ∈ {0.51, 0.60, 0.70, 0.80, 0.90}, with β = 0.5 in all cases to ensure no ground penetration. Note that when we apply chance constraints, we do not apply any other relaxation to the complementarity constraints. Instead, we use the strictly feasible solution from our progressive tightening procedure to warm-start the optimization with chance constraints. We quantified the feasibility of the hopping motion plans using the merit score (Eq. 9) and the distance constraint (Eq. 1d). We used average foot height to quantify robustness, as higher foot heights indicate the hopper is less likely to trip over unexpected variations in ground height.

5 Results

5.1 Chance Constraints Improve Friction Feasibility Under High Uncertainty

In the sliding block example, optimizing under moderate uncertainty (σ = 0.1) using chance constraints without the ERM cost produced trajectories that were nearly identical to the reference trajectory under moderate uncertainty (σ = 0.1) (Figure 2A). When σ = 1.0, however, the friction forces varied both above and below the reference value of -4.9N, demonstrating that chance constraints relax the friction cone around both sides of the mean. However, the optimized control was still nearly identical to the reference control (Figure 2B), indicating chance constraints alone may not offer any robustness to uncertainty in contact.

FIGURE 2

FIGURE 2. Effects of including chance constraints on contact-robust optimization at different uncertainty levels, for different risk bounds. (A,B) Including chance constraints without a robust cost, such as the ERM, does not have much effect on the optimized open-loop control, but can allow the friction force to vary under high uncertainty. (C,D) Including chance constraints with a contact robust cost has little effect on the robust solution at low uncertainty, but tightening the risk bounds θ and β increases the friction force magnitude at high uncertainty.

In our optimizations combining the ERM with chance constraints, when the friction uncertainty was σ < 0.1, the ERM with chance constraints method produced friction forces around 4.9 N during sliding, similar to those produced by the ERM method alone (Figure 2C). However, when the uncertainty was large (σ = 1.0), the ERM produced friction forces at 0 N during the entire motion, which is infeasible for all friction coefficients except μ = 0. In contrast, the ERM with chance constraints produced nonzero friction forces, and the magnitude of the friction forces increased as the risk bounds decreased and converged towards the expected value for friction at 4.9 N (Figure 2D), indicating a solution with improved feasibility under the expected friction coefficient.

Across all uncertainties, the solutions of the ERM and ERM with chance constraints tended to improve in friction cone feasibility as the uncertainty decreased, as indicated by a decrease in the merit score (Figure 3A). Moreover, at any fixed uncertainty, the friction merit score decreased as the risk parameters decreased, with the ERM-only solution and reference solution acting as upper and lower bounds, respectively. Similarly, the maximum sliding velocity of the block increased with increasing uncertainty, indicating less sliding time under uncertainty, but decreased with decreasing the risk parameters (Figure 3B), except in the highest uncertainty case. The range of maximum velocity across chance parameters also increased with increasing uncertainty, from 0.02 m/s at σ = 0.01 to 1.73 m/s at σ = 0.3. However, at the highest uncertainty, the sliding velocity for the ERM and chance constraints were all identical and less than that of the reference. In the σ = 1 case, the ERM failed to provide robustness to friction uncertainty; in this case, the ERM does not model the friction cone constraint well, and allows the optimization to set the friction forces to zero. Without friction, the optimal control is an impulsive, bang-bang controller (Figure 2D) and the resulting trajectory has almost constant velocity at 5 m/s. However, the addition of chance constraints did improve the feasibility of the final motion plans with respect to the friction cone constraint, but did not alter the sliding velocity. Taken together, these results indicate that the chance constraints can mediate a trade-off between the robustness to friction uncertainty provided by the ERM and the strict feasibility provided by the reference solution.

FIGURE 3

FIGURE 3. Chance constraint mediated trade-off between expected friction cone feasibility and robustness to friction uncertainty (signified by maximum sliding velocity). (A) Merit scores across uncertainty and risk tolerances, quantifying violation of the expected friction cone constraint. (B) Maximum sliding velocity across uncertainty and risk tolerances, signifying robustness as larger velocities indicate shorter sliding times. Both constraint violation and maximum velocity increase with increasing uncertainty and with increasing risk bounds. Missing data points indicate the optimization was not solved successfully.

5.2 Chance Constraints Improve Average Performance Against Friction Perturbations in Simulation

In our open loop simulations with the block example, the controls generated under ERM with chance constraints performed similarly to those generated under only the ERM for uncertainties less than 0.1 (mean position error 0.04 and error range 0.44 for ERM only, mean −0.03 and range 0.61 for ERM with chance constraints at σ = 0.1) (Figure 4). However, at high uncertainty σ = 1.0, the ERM with chance constraint simulation achieved a lower average position error compared to the ERM alone (0.26 for chance constraints, 2.41 for ERM only), although both had a similar range of position errors (Figure 5A). By varying the chance parameters during optimization, we found that changing β had little effect on simulation results, while increasing θ resulted in a slight increase in the final position error, from an average error of 0.01 at θ = 0.51 to 0.65 at θ = 0.9, for all values of β (Figure 5B). Moreover, changing θ and β at high uncertainty had no effect on the range of final positions achieved, indicating again that the chance constraints modulate the feasibility of the motion plan, while the robustness is provided by the ERM cost.

FIGURE 4

FIGURE 4. Example block simulations demonstrating chance constraints retain robustness at moderate uncertainty and improve feasibility performance at high uncertainty, compared to the (A) simulations using the reference controls, for four different values of the friction coefficient. Simulations using controls generated under only the contact-robust ERM cost result in a low spread around the desired position for moderate uncertainty (B), but can result in a large average position error when the friction uncertainty is large (C). Simulations using controls generated using ERM with chance constraints (θ = β = 0.7) maintain a low spread at moderate uncertainty (E), and have a low final position error at high uncertainty (F). (D) Illustration of the motion of the block for the reference, ERM, and ERM with chance constraint controls under high friction uncertainty.

FIGURE 5

FIGURE 5. Effects of chance constraints on robustness of sliding block controls in open loop simulations. (A) Mean and range of final position errors for the ERM with and without chance constraints planned under different uncertainties, compared to those of the reference. The addition of chance constraints maintains the low range of final position errors produced by the ERM, but at high uncertainty the chance constraints reduce the average final position error. (B) Mean and range of final position error of simulated chance constraint controls under different risk tolerances compared to the mean and range for the ERM under the highest friction uncertainty case (σ =1.0). Increasing the upper risk bound β has little effect, while increasing the lower risk bound θ can increase the average final position error.

5.3 Chance Constraints Mediate the Distance at Which Contact Forces Are Applied

In the hopping example with contact distance uncertainty, the ERM alone produced higher average foot height with increasing uncertainty, up to an average of 0.46 m at our highest value of uncertainty (σ = 0.5 m). However, the ERM also allows contact forces to be applied at a nonzero distance from the expected terrain; such motion plans are not physically realizable on the expected terrain, and could be dangerous to execute on real robots because the robot may expect a large force when one is not provided by the environment. Introducing chance constraints, however, reduced the foot height and reduced the distance at which the contact normal forces were nonzero, and the decrease in foot height trended with decreasing the risk parameters θ, β (Figures 6B,C). Across all uncertainties and risk parameters, the chance constraints tended to reduce foot height as the risk parameters decreased, and the range of foot heights generated by the risk parameters tended to increase with increasing uncertainty (Figure 7B), although there are exceptions which could be due to the highly nonlinear and nonconvex nature of the problem. However, we note that the chance constraints do not completely eliminate the force-at-a-distance effect introduced by the ERM; only in the strict case where θ = β = 0.5, the chance constraints eliminate force-at-a-distance from the expected terrain. Thus, although the chance constraints reduce the contact infeasibility, unless the risk bounds are made sufficiently close to 0.5, the planned motions could still be dangerous to execute. Nonetheless, by bringing the foot closer to the expected terrain, the chance constraints reduce the risk associated with expecting a large force at a distance from the terrain, as the robot would need to accelerate the foot over a shorter distance to make contact compared to the larger distance prescribed using the ERM only. Finally, we note that neither the ERM nor the ERM with chance constraints had much effect on the optimized reaction forces; in this example, the effects were limited mainly to the contact distance.

FIGURE 6

FIGURE 6. Effect of including chance constraints on hopping under distance uncertainty. (A) Selected frames of the hopper trajectory comparing the reference, non-robust trajectory, the ERM only trajectory, and the ERM with chance constraints trajectory. Only the θ = 0.6 case is illustrated for brevity. (B) Planned foot heights for the hopper under high distance uncertainty (σ = 0.5 m) for different risk bounds, compared to the ERM and reference trajectories, and (C) the associated normal ground reaction forces. The ERM cost allows for contact forces to be applied at nonzero distances; however, as the risk bounds decrease, the distance at which forces are applied also decreases.

FIGURE 7

FIGURE 7. Chance constraint mediated trade-off between contact distance feasibility and average foot height for robustness. (A) Merit scores across distance uncertainty and risk bounds, quantifying the violation of the expected contact distance constraint. (B) Average foot height across uncertainty and risk bounds, where higher average height indicates more contact-robust hopping. Both constraint violation and maximum foot height increase with increasing uncertainty and with increasing risk bounds. Missing data points indicate the optimization was not solved successfully.

By using the merit score, we also observed that the contact distance infeasibility decreased with both decreasing uncertainty and decreasing the risk parameters (Figure 7A). While the reference case provides a lower bound for the infeasibility, as it did in the block example, in this example the ERM only trajectory was not strictly the upper bound for all uncertainties, although this may be due to the presence of multiple local minima in the optimization.

6 Discussion and Conclusion

In this work we proposed a novel framework for accounting for contact uncertainty in TO. As previously explored, the ERM cost represents a robust contact-averse objective but also results in infeasible trajectories as the contact uncertainty grows (Drnach and Zhao, 2021). Here we developed chance complementarity constraints to convert the stochastic constraints into deterministic constraints and showed that the chance constraints can mediate a trade-off between feasibility and robustness by changing the risk bounds θ and β. The improved feasibility is achieved because the chance constraints limit the region of allowable solutions to the ERM to those near the non-negative m_F and z axes, i.e., the solution set of the non-stochastic complementarity constraints; moreover, as the risk bounds are decreased, the allowable set approaches the complementarity solution under the mean values of the constraints, representing the limit of perfect feasibility under the mean but no robustness.

Our work with chance-constraints is similar to previous works which have applied chance-constraints to obstacle avoidance (Gazar et al., 2020) or to modeling frictional uncertainty (Shirai et al., 2020) for locomotion. These works claim that the chance constraints provide a measure of robustness by using risk bounds to make the constraints more conservative, which can be thought of as making an obstacle larger or by making the friction cone narrower. This type of robustness is similar to worst-case robustness; the generated plan accounts for the worst possible constraint violations, but may still be sensitive to variations in the constraint parameters (Drnach and Zhao, 2021). In this work, we applied chance constraints to problems which require intermittent contact, and we noted that the complementarity constraints cannot be made more conservative as their solution sets have an empty interior. Instead, we demonstrated that chance constraints relaxed the contact constraints and improved the physical feasibility of trajectories generated with a robust cost; lower risk bounds produced trajectories which were feasible under the expected constraints but were potentially sensitive to variations, while higher risk bounds allowed trajectories to violate the expected constraints to achieve robustness.

Here we considered solely the problem of accounting for uncertainty in contact during motion planning; we specifically have not investigated handling uncertainty in contact with control. Future work could convert our technique into a feedback control policy by re-planning in a receding horizon fashion; however, current methods for solving contact-implicit problems are too slow to be used reactively in real-time. Thus, advancements in efficient solvers for contact-implicit problems are necessary before our work can be used in a receding horizon control fashion, such as those used in hybrid optimization to generate gait libraries (Hereid et al., 2019). Apart from replanning, other methods for controlling through contact have already been developed, including contact mode-invariant stabilizing control using Lyapunov analysis (Posa et al., 2016) and a risk-sensitive impedance optimization for handing control through uncertain contact (Hammoud et al., 2021). Although these approaches show promise for stabilizing and controlling locomotion through contact, the former has yet to be demonstrated on terrain with variations and the latter requires a reference trajectory with a contact schedule. The overarching goal of our work is to complement these approaches by generating a reference trajectory, including the contact sequence, which is robust to terrain variations. By planning trajectories which are robust to contact uncertainty - for example, by avoiding areas of the terrain with large variations - we aim to alleviate some of the burden on the controller and improve the overall performance of the system.

In this work, we parameterized uncertainty in the distance to the terrain and in the friction coefficient using Gaussian distributions, as this distribution provides analytical formulas for the ERM cost and for the chance constraints. Having access to analytical formulas means we only needed to generate one robust trajectory, instead of generating multiple samples to achieve robustness (Mordatch et al., 2015; Seyde et al., 2019). Given that generating a single trajectory using the contact-implicit approach requires substantial computation time, the analytical formulas saved us considerable computation time by avoiding solving the problem for multiple samples of the terrain geometry or friction coefficient. However, using the Gaussian distribution has distinct disadvantages in theory, as it places non-zero probability mass over regions which are physically impossible, such as over negative friction coefficients or over terrain heights which result in interpenetration (e.g., terrain heights that are above the current contact point location). Such physically impossible regions could be avoided in future works by using distributions over a subset of the reals, such as the truncated Gaussian distribution or the Gamma distribution. However, using such distributions might require considerable effort to evaluate the ERM cost and chance constraints, which have so far been developed largely for Gaussian distributed variables.

One challenge in developing a contact-robust TO is in propagating contact uncertainty through the system dynamics as the state evolves, as contact events are intermittent. In this work, we introduced an ERM cost to improve trajectory robustness as the ERM cost minimizes the sensitivity of the solutions to variations in the contact parameters (Chen et al., 2009); nonetheless, we note that neither the ERM nor the chance constraints propagate uncertainty through the dynamics. Following other robust TO approaches, an alternative to our work would be to sample the uncertain contact models and then minimize either the expected cost (Dai and Tedrake, 2012; Kuindersma et al., 2013; Mordatch et al., 2015) or an expected exponential transformation of the cost (Jacobson, 1973; Farshidian and Buchli, 2015; Ponton et al., 2018), taking the expectation numerically over an ensemble of trajectories. However, developing an ensemble approach to contact-robust optimization is not without its challenges. Unlike state uncertainty, which can be propagated directly through the dynamics, contact model uncertainty enters in through additional constraints, and the effects of these constraints are only propagated intermittently to the dynamics through potentially impulsive contact forces. Propagating uncertainty through impulsive forces and nonlinear dynamics could cause the state uncertainty tube (or equivalently the idea of “funnel”) to diverge from the nominal state trajectory, making calculating the expected cost challenging. Apart from diverging state uncertainty tube, different trajectories within the ensemble will likely also have different implicit contact mode sequences, due to sampling the underlying contact models. In this case, it’s not clear how the expectation should be calculated and whether it represent an appropriate metric to quantify uncertainty. Our work here with the ERM cost and chance constraints made an attempt to develop contact-robust optimization by circumventing these problems; future works may instead aim to solve the aforementioned challenges by developing an alternative strategy more in line with ensemble techniques.

The main advantage of our chance-constrained ERM approach is that we can generate trajectories with varying degrees of robustness to contact uncertainty without changing the uncertainty. Thus, when faced with uncertain terrain, we can choose between being robust to terrain variations or being optimal with respect to our original objective without artificially changing the uncertainty in the model. Our work here focused on investigating these behaviors in simple systems on 2-dimensional terrain. In future works we could scale up our approach to full-scale robots traversing 3-dimensional terrain. We expect the complexity of solving the ERM and chance constraints to scale only with the number of contacts and not with the state dimension of the robot, as the number of complementarity constraints, and therefore the number of ERM costs and chance constraints, is linear in the number of contact points and not dependent on the state dimension - for example, adding several contact points to the sliding block and putting obstacles in the environment would make the contact problem more challenging, even though the state dimension is the same. Once we have scaled up to three dimensions, we could also evaluate our methods experimentally on full-scale robots, such as a quadruped, and compare the performance of our robust motion plans against the traditional approach using a simple controller, and against other risk-sensitive control approaches such as (Hammoud et al., 2021).

Data Availability Statement

The datasets presented in this study can be found in online repositories. The names of the repository/repositories and accession number(s) can be found below: https://github.com/GTLIDAR/ChanceConstrainedRobustCITO.

Author Contributions

LD conceived the idea; JZ developed the software for the experiments; JZ and LD designed, carried out the experiments, collected the data, performed the analysis and wrote the manuscript. JZ and LD contributed equally and share first authorship. YZ provided student mentoring, discussed research results, and edited the manuscript.

Funding

This work was supported by the United States National Science Foundation under grant Nos. DEG-1650044 and IIS-1924978. Any views and conclusions contained herein are those of the authors, and do not necessarily represent the official positions, express or implied, of the funders.

Conflict of Interest

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

Publisher’s Note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.

References

Baumrucker, B. T., and Biegler, L. T. (2009). MPEC Strategies for Optimization of a Class of Hybrid Dynamic Systems. J. Process Control. 19, 1248–1256. doi:10.1016/j.jprocont.2009.02.006

CrossRef Full Text | Google Scholar

Blackmore, L., Ono, M., and WIlliams, B. C. (2011). Chance-Constrained Optimal Path Planning With Obstacles. IEEE Trans. Robotics. 27, 1080. doi:10.1109/tro.2011.2161160

CrossRef Full Text | Google Scholar

Brandão, M., Shiguematsu, Y. M., Hashimoto, K., and Takanishi, A. (2016). “Material Recognition CNNs and Hierarchical Planning for Biped Robot Locomotion on Slippery Terrain,” in 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids), 81–88. ISSN: 2164-0580. doi:10.1109/humanoids.2016.7803258

CrossRef Full Text | Google Scholar

Carius, J., Ranftl, R., Koltun, V., and Hutter, M. (2018). Trajectory Optimization With Implicit Hard Contacts. IEEE Robot. Autom. Lett. 3, 3316–3323. doi:10.1109/lra.2018.2852785

CrossRef Full Text | Google Scholar

Celik, O., Abdulsamad, H., and Peters, J. (2019). "Chance-Constrained Trajectory Optimization for Non-Linear Systems With Unknown Stochastic Dynamics," in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 6828–6833. doi:10.1109/iros40897.2019.8967794

CrossRef Full Text | Google Scholar

Chen, X., Zhang, C., and Fukushima, M. (2009). Robust Solution of Monotone Stochastic Linear Complementarity Problems. Math. Program. 117, 51–80. doi:10.1007/s10107-007-0163-z

CrossRef Full Text | Google Scholar

Dai, H., and Tedrake, R. (2012). “Optimizing Robust Limit Cycles for Legged Locomotion on Unknown Terrain,” in 2012 IEEE 51st IEEE Conference on Decision and Control (CDC) (IEEE), 1207–1213. doi:10.1109/cdc.2012.6425971

CrossRef Full Text | Google Scholar

Dai, H., Valenzuela, A., and Tedrake, R. (2014). “Whole-Body Motion Planning With Centroidal Dynamics and Full Kinematics,” in IEEE-RAS International Conference on Humanoid Robots, 295–302. doi:10.1109/humanoids.2014.7041375

CrossRef Full Text | Google Scholar

Drnach, L., and Zhao, Y. (2021). Robust Trajectory Optimization Over Uncertain Terrain With Stochastic Complementarity. IEEE Robot. Autom. Lett. 6, 1168–1175. doi:10.1109/lra.2021.3056064

CrossRef Full Text | Google Scholar

Farshidian, F., and Buchli, J. (2015). Risk Sensitive, Nonlinear Optimal Control: Iterative Linear Exponential-Quadratic Optimal Control With Gaussian Noise. arXiv:1512.07173 [cs].

Google Scholar

Gazar, A., Khadiv, M., Del Prete, A., and Righetti, L. (2020). Stochastic and Robust MPC for Bipedal Locomotion: A Comparative Study on Robustness and Performance. IEEE-RAS 20th International Conference on Humanoid Robots (Humanoids), 61–68. doi:10.1109/HUMANOIDS47582.2021.9555783

CrossRef Full Text | Google Scholar

Gill, P. E., Murray, W., and Saunders, M. A. (2005). SNOPT: An SQP Algorithm for Large-Scale Constrained Optimization. SIAM Rev. 47, 99–131. doi:10.1137/s0036144504446096

CrossRef Full Text | Google Scholar

Hackett, J., Gao, W., Daley, M., Clark, J., and Hubicki, C. (2020). “Risk-Constrained Motion Planning for Robot Locomotion: Formulation and Running Robot Demonstration,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 3633–3640. doi:10.1109/iros45743.2020.9340810

CrossRef Full Text | Google Scholar

Hammoud, B., Khadiv, M., and Righetti, L. (2021). Impedance Optimization for Uncertain Contact Interactions Through Risk Sensitive Optimal Control. IEEE Robot. Autom. Lett. 6, 4766–4773. doi:10.1109/LRA.2021.3068951

CrossRef Full Text | Google Scholar

Hereid, A., Harib, O., Hartley, R., Gong, Y., and Grizzle, J. W. (2019). “Rapid Trajectory Optimization Using C-Frost with Illustration on a Cassie-Series Dynamic Walking Biped,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE), 4722–4729. doi:10.1109/iros40897.2019.8968056

CrossRef Full Text | Google Scholar

Jacobson, D. (1973). Optimal Stochastic Linear Systems With Exponential Performance Criteria and Their Relation to Deterministic Differential Games. IEEE Trans. Automat. Contr. 18, 124–131. doi:10.1109/tac.1973.1100265

CrossRef Full Text | Google Scholar

Kuindersma, S., Deits, R., Fallon, M., Valenzuela, A., Dai, H., Permenter, F., et al. (2016). Optimization-Based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot. Auton. Robot. 40, 429–455. doi:10.1007/s10514-015-9479-3

CrossRef Full Text | Google Scholar

Kuindersma, S. R., Grupen, R. A., and Barto, A. G. (2013). Variable Risk Control via Stochastic Optimization. Int. J. Robotics Res. 32, 806–825. doi:10.1177/0278364913476124

CrossRef Full Text | Google Scholar

Luo, M.-J., and Lu, Y. (2013). Properties of Expected Residual Minimization Model for a Class of Stochastic Complementarity Problems. J. Appl. Mathematics. 2013, 1–7. doi:10.1155/2013/497586

CrossRef Full Text | Google Scholar

Manchester, Z., Doshi, N., Wood, R. J., and Kuindersma, S. (2019). Contact-Implicit Trajectory Optimization Using Variational Integrators. Int. J. Robotics Res. 38, 1463–1476. doi:10.1177/0278364919849235

CrossRef Full Text | Google Scholar

Mesbah, A. (2016). Stochastic Model Predictive Control: An Overview and Perspectives for Future Research. IEEE Control. Syst. Mag. 36, 30–44. doi:10.1109/MCS.2016.2602087

CrossRef Full Text | Google Scholar

Mordatch, I., Lowrey, K., and Todorov, E. (2015). “Ensemble-cio: Full-Body Dynamic Motion Planning that Transfers to Physical Humanoids,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 5307–5314. doi:10.1109/iros.2015.7354126

CrossRef Full Text | Google Scholar

Mordatch, I., Todorov, E., and Popović, Z. (2012). Discovery of Complex Behaviors Through Contact-Invariant Optimization. ACM Trans. Graphics (Tog). 31, 8. doi:10.1145/2185520.2185539

CrossRef Full Text | Google Scholar

Patel, A., Shield, S. L., Kazi, S., Johnson, A. M., and Biegler, L. T. (2019). Contact-Implicit Trajectory Optimization Using Orthogonal Collocation. IEEE Robot. Autom. Lett. 4, 2242–2249. doi:10.1109/lra.2019.2900840

CrossRef Full Text | Google Scholar

Paulson, J. A., Buehler, E. A., Braatz, R. D., and Mesbah, A. (2020). Stochastic Model Predictive Control With Joint Chance Constraints. Int. J. Control. 93, 126–139. doi:10.1080/00207179.2017.1323351

CrossRef Full Text | Google Scholar

Ponton, B., Schaal, S., and Righetti, L. (2018). The Role of Measurement Uncertainty in Optimal Control for Contact Interactions. arXiv:1605.04344 [eess].

Google Scholar

Posa, M., Cantu, C., and Tedrake, R. (2014). A Direct Method for Trajectory Optimization of Rigid Bodies through Contact. Int. J. Robotics Res. 33, 69–81. doi:10.1177/0278364913506757

CrossRef Full Text | Google Scholar

Posa, M., Tobenkin, M., and Tedrake, R. (2016). Stability Analysis and Control of Rigid-Body Systems With Impacts and Friction. IEEE Trans. Automat. Contr. 61, 1423–1437. doi:10.1109/TAC.2015.2459151

CrossRef Full Text | Google Scholar

Scholtes, S. (2001). Convergence Properties of a Regularization Scheme for Mathematical Programs With Complementarity Constraints. SIAM J. Optim. 11, 918–936. doi:10.1137/s1052623499361233

CrossRef Full Text | Google Scholar

Seyde, T., Carius, J., Grandia, R., Farshidian, F., and Hutter, M. (2019). “Locomotion Planning Through a Hybrid Bayesian Trajectory Optimization,” in 2019 International Conference on Robotics and Automation (ICRA), 5544–5550. doi:10.1109/icra.2019.8794067

CrossRef Full Text | Google Scholar

Shirai, Y., Lin, X., Tanaka, Y., Mehta, A., and Hong, D. (2020). Risk-Aware Motion Planning for a Limbed Robot With Stochastic Gripping Forces Using Nonlinear Programming. IEEE Robot. Autom. Lett. 5, 4994–5001. doi:10.1109/lra.2020.3001503

CrossRef Full Text | Google Scholar

Stewart, D. E., and Trinkle, J. C. (1996). An Implicit Time-Stepping Scheme for Rigid Body Dynamics With Inelastic Collisions and Coulomb Friction. Int. J. Numer. Meth. Engng. 39, 2673–2691. doi:10.1002/(sici)1097-0207(19960815)39:15<2673::aid-nme972>3.0.co;2-i

CrossRef Full Text | Google Scholar

Tassa, Y., and Todorov, E. (2010). Stochastic Complementarity for Local Control of Discontinuous Dynamics. Robotics: Sci. Syst., 169–176. doi:10.15607/rss.2010.vi.022

CrossRef Full Text | Google Scholar

Russ Tedrake and the Drake Development Team (2019). Drake: Model-Based Design and Verification for Robotics. Available at: https://drake.mit.edu.

Google Scholar

Toussaint, M., Ratliff, N., Bohg, J., Righetti, L., Englert, P., and Schaal, S. (2014). “Dual Execution of Optimized Contact Interaction Trajectories,” in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, 47–54. doi:10.1109/IROS.2014.6942539

CrossRef Full Text | Google Scholar

Wang, A., Jasour, A., and Williams, B. C. (2020). Non-Gaussian Chance-Constrained Trajectory Planning for Autonomous Vehicles Under Agent Uncertainty. IEEE Robotics Automation Lett. 5 (4), 6041–6048. doi:10.1109/lra.2020.3010755

CrossRef Full Text | Google Scholar

Winkler, A. W., Bellicoso, C. D., Hutter, M., and Buchli, J. (2018). Gait and Trajectory Optimization for Legged Systems Through Phase-Based End-Effector Parameterization. IEEE Robot. Autom. Lett. 3, 1560–1567. doi:10.1109/lra.2018.2798285

CrossRef Full Text | Google Scholar

Yeganegi, M. H., Khadiv, M., Moosavian, S. A. A., Zhu, J., Prete, A. D., and Righetti, L. (2019). Robust Humanoid Locomotion Using Trajectory Optimization and Sample-Efficient Learning*. IEEE Int. Conf. Humanoid Robots., 170–177. doi:10.1109/Humanoids43949.2019.9035003

CrossRef Full Text | Google Scholar