Robust trajectory optimization under frictional contact with iterative learning

Abstract

Optimization is often difficult to apply to robots due to the presence of errors in model parameters, which can cause constraints to be violated during execution on the robot. This paper presents a method to optimize trajectories with large modeling errors using a combination of robust optimization and parameter learning. In particular it considers the context of contact modeling, which is highly susceptible to errors due to uncertain friction estimates, contact point estimates, and sensitivity to noise in actuator effort. A robust time-scaling method is presented that computes a dynamically-feasible, minimum-cost trajectory along a fixed path under frictional contact. The robust optimization model accepts confidence intervals on uncertain parameters, and uses a convex parameterization that computes dynamically-feasible motions in seconds. Optimization is combined with an iterative learning method that uses feedback from execution to learn confidence bounds on modeling parameters. It is applicable to general problems with multiple uncertain parameters that satisfy a monotonicity condition that requires parameters to have conservative and optimistic settings. The method is applied to manipulator performing a “waiter” task, on which an object is moved on a carried tray as quickly as possible, and to a simulated humanoid locomotion task. Experiments demonstrate this method can compensate for large modeling errors within a handful of iterations.

Appendix

Here we derive the formula for the derivative of the objective function of an inequality-constrained LP with respect to changes in the constraint RHS. Consider the LP

$$\begin{aligned} \min _{x}{c^{T}x}~\text {such that}~Ax - b(t) \le 0. \end{aligned}$$

(23)

By the first-order KKT conditions, the optimal solution satisfies

$$\begin{aligned}&c + A^{T}\mu = 0 \nonumber \\&Ax^* - b \le 0\nonumber \\&\mu ^{T}(Ax^* - b) = 0\nonumber \\&\mu \le 0 \end{aligned}$$

(24)

where $\mu$ is the vector of KKT multipliers (unrelated to the friction coefficient) and $x^*$ is the optimal solution. If the LP is bounded, $x^*$ lies at a corner point of the feasible region, and is defined by active constraints with $A_{i}x^* - b_i = 0$ and $\mu _i < 0$. Denote $\hat{A}$ and $\hat{b}$ respectively as the matrix/vector containing the rows of A and b corresponding to active constraints, and let $\hat{\mu }$ be the set of active multipliers. Then the equations

$$\begin{aligned} \begin{aligned} c + \hat{A}^{T}\hat{\mu }&= 0\\ \hat{A} x^* - \hat{b}&= 0 \end{aligned} \end{aligned}$$

(25)

must be satisfied, with $\hat{A}$ an invertible matrix. Hence, $x^*(t) = \hat{A}^{-1} \hat{b}(t)$, and $\frac{d}{dt}(c^T x^*) = c^T \hat{A}^{-1}\hat{b}^{\prime }(t)$. Since $\hat{\mu }^{T} = -c^{T}\hat{A}^{-1}$, we obtain

$$\begin{aligned} \frac{d}{dt}(c^T x^*) = -\hat{\mu }^{T} \hat{b}^{\prime }(t) = -\mu ^{T} b^{\prime }(t) \end{aligned}$$

(26)

as desired.