Inverse Dynamics Control through Optimization-Based Dynamics
Terry
2023/09/15
Today's Topic
How should we be thinking about local feedback control for manipulation?
Inverse Dynamics
Inverse Dynamics as Local Control
\mathbf{M}(q)\ddot{q} + \mathbf{C}(q,\dot{q})\dot{q} + \mathbf{G}(q) = \tau + \sum_{i=1}^{n_c}\mathbf{J}^\top_i(q) \lambda_i
Forward Dynamics
Inverse Dynamics
q' = f(q,u)
u = f(q,q')
Inverse Dynamics Control for Smooth Systems
Given a desired acceleration of the robot, and the current state (joint state + velocities + contact forces), which torque should I apply to the robot?
Inverse Dynamics: Short comings
\mathbf{M}(q)\ddot{q} + \mathbf{C}(q,\dot{q})\dot{q} + \mathbf{G}(q) = \tau + \sum_{i=1}^{n_c}\mathbf{J}^\top_i(q) \lambda_i
Inverse Dynamics Control for Smooth Systems
Given a desired acceleration of the robot, the current state, and the contact Jacobian (joint state + velocities), which torque should I apply to the robot?
Shortcomings
Only considers a fixed contact mode, and the controller no longer reasons about change of contact sequences during local control.
Okay for stabilizing locomotion (two feet are always on the ground), but spells trouble for manipulation!
How can we do better?
Background: Contact Dynamics
\begin{aligned}
\text{find}\quad & \delta q^u, \delta q^a, \lambda_i \\
\text{s.t.}\quad & \mathbf{M}_u/h \delta q^u = \sum^{n_c}_{i=1} \mathbf{J}_{u_i}^\top \lambda_i \\
& h\mathbf{K}_a(q^a + \delta q^a - u) = \sum^{n_c}_{i=1} \mathbf{J}_{a_i}^\top \lambda_i \\
& \lambda_i \geq 0 \\
& \phi_i + \mathbf{J}_{a_i} \delta q^a + \mathbf{J}_{u_i} \delta q^u \geq 0 \\
& \lambda_i (\phi_i + \mathbf{J}_{a_i} \delta q^a + \mathbf{J}_{u_i} \delta q^u) = 0
\end{aligned}
Momentum-Impulse on Object
Impulse Balance on Robot Impedance
Contact Forces can only push, but not pull
Linearized Non-Penetration
Complementarity
q^a
q^u
u
\lambda
\lambda
Actuated
Unactuated
\begin{aligned}
\text{find}\quad & \delta q^u, \delta q^a, \lambda_i \\
\text{s.t.}\quad & \mathbf{M}_u/h \delta q^u = \sum^{n_c}_{i=1} \mathbf{J}_{u_i}^\top \lambda_i \\
& h\mathbf{K}_a(q^a + \delta q^a - u) = \sum^{n_c}_{i=1} \mathbf{J}_{a_i}^\top \lambda_i \\
& \lambda_i \geq 0 \\
& \phi_i + \mathbf{J}_{a_i} \delta q^a + \mathbf{J}_{u_i} \delta q^u \geq 0 \\
& \lambda_i (\phi_i + \mathbf{J}_{a_i} \delta q^a + \mathbf{J}_{u_i} \delta q^u) = 0
\end{aligned}
Background: Contact Dynamics
\begin{aligned}
\min_{\delta q^u,\delta q^a}\quad & \;\; \frac{1}{2} \delta {q^u}^\top \mathbf{M}/h \delta {q^u} + \frac{1}{2}\delta {q^a}^\top h\mathbf{K}_a \delta q^a - h\mathbf{K}_a (u - q^a) \delta q^a \\
\text{s.t.}\quad & \phi_i + \mathbf{J}_{a_i} \delta q^a + \mathbf{J}_{u_i} \delta q^u \geq 0 \quad \forall i\in \{1,\cdots,n_c\}\\
\end{aligned}
q^a
q^u
u
\lambda
\lambda
\begin{aligned}
\text{find}\quad & \delta q^u, \delta q^a, \lambda_i \\
\text{s.t.}\quad & \mathbf{M}_u/h \delta q^u = \sum^{n_c}_{i=1} \mathbf{J}_{u_i}^\top \lambda_i \\
& h\mathbf{K}_a(q^a + \delta q^a - u) = \sum^{n_c}_{i=1} \mathbf{J}_{a_i}^\top \lambda_i \\
& \lambda_i \geq 0 \\
& \phi_i + \mathbf{J}_{a_i} \delta q^a + \mathbf{J}_{u_i} \delta q^u \geq 0 \\
& \lambda_i (\phi_i + \mathbf{J}_{a_i} \delta q^a + \mathbf{J}_{u_i} \delta q^u) = 0
\end{aligned}
Dynamics Problem
Equivalent optimization problem
"Find me a minimum work configuration while respecting non-penetration."
Background Contact Dynamics
\begin{aligned}
\min_{\delta q^a}\quad & \;\;\frac{1}{2}\delta {q^a}^\top h\mathbf{K}_a \delta q^a - h\mathbf{K}_a (u - q^a) \delta q^a \\
\text{s.t.}\quad & \phi_i + \delta q^a \geq 0 \quad \forall i\in \{1,\cdots,n_c\}\\
\end{aligned}
Optimal solution to the unconstrained problem:
\begin{aligned}
\delta q^a = u - q^a
\end{aligned}
Position command is exactly obeyed
\begin{aligned}
q^a_\text{next} = q^a + \delta q^a = u
\end{aligned}
If this solution meets constraints, then this is the optimal solution.
If not, optimality happens right at the boundary of the constraint.
\begin{aligned}
u
\end{aligned}
\begin{aligned}
q^a_\text{next}
\end{aligned}
Optimal Control with Contact Dynamics
\begin{aligned}
\min_{u} \quad & \|q_\text{next} - q_g\|^2_\mathbf{Q} + \|u\|^2_\mathbf{R} \\
\text{s.t.} \quad & q_\text{next} = f(q, u) \\
\end{aligned}
What we care about is optimizing for some parameters of an optimal control problem.
We will take one-step trajectory optimization as an example.
But this is difficult because f describes an optimal solution to an optimization problem!
\begin{aligned}
\min_{u} \quad & \|q_\text{next} - q_g\|^2_\mathbf{Q} + \|u\|^2_\mathbf{R} \\
\text{s.t.} \quad & q_\text{next} = q + \delta q^* \\
& \delta q^* = \text{arg}\min_{\delta q} \;\; g(\delta q,u) \;\; \text{s.t.} \;\; h(\delta q) \geq 0\\
\end{aligned}
Bi-level Optimization
Optimal Control with Contact Dynamics
KKT Reformulation Approach
Sensitivity Analysis Approach
\begin{aligned}
\min_{u} \quad & \|q_\text{next} - q_g\|^2_\mathbf{Q} + \|u\|^2_\mathbf{R} \\
\text{s.t.} \quad & q_\text{next} = q + \delta q^* \\
& \delta q^* = \text{arg}\min_{\delta q} \;\; g(\delta q, u) \;\; \text{s.t.} \;\; h(\delta q) \geq 0\\
\end{aligned}
Bi-level Optimization
\begin{aligned}
\min_{u, \lambda, \delta q} \quad & \|q_\text{next} - q_g\|^2_\mathbf{Q} + \|u\|^2_\mathbf{R} \\
\text{s.t.} \quad & q_\text{next} = q + \delta q^* \\
& \nabla_{\delta q} g(\delta q) - \lambda^\top \nabla_{\delta q} h(\delta q) = 0 \\
& h(\delta q) \geq 0\\
& \lambda \geq 0 \\
& h(\delta q)\lambda = 0
\end{aligned}
Two approaches to tackling these in general.
Solvers hate the nonconvexity from complementarity.
Two approaches to tackling these in general.
If we have the gradients
\begin{aligned}
\frac{\partial \delta q^*}{\partial u}
\end{aligned}
through sensitivity analysis of the inner optimization problem, then possible to plug this into iterative solvers.
Sensitivity Analysis
What do gradients do? They allow us to construct a locally linear model around the current iterate.
\begin{aligned}
\min_{u} \quad & \|q_\text{next} - q_g\|^2_\mathbf{Q} + \|u\|^2_\mathbf{R} \\
\text{s.t.} \quad & q_\text{next} = \mathbf{B}(q,\bar{u})(u - \bar{u}) + f(q,\bar{u}) \\
\end{aligned}
We had used this sensitivity extensively in previous projects.
\begin{aligned}
\mathbf{B}(q,\bar{u}) = \frac{\partial q_\text{next}(q,u)}{\partial u}\bigg|_{q=q,u=\bar{u}}
\end{aligned}
1. This is a linearization of the dynamics, allows us to tools such as iLQR.
2. Linearization also allows us to approximate reachability effectively.
Inverse Dynamics Interpretation
If we strictly enforce the goal displacement, we obtain inverse dynamics from locomotion.
\begin{aligned}
\min_{u} \quad & \|u\|^2_\mathbf{R} \\
\text{s.t.} \quad & q_\text{next} = q_g \\
& q_\text{next} = \mathbf{B}(q,\bar{u})(u - \bar{u}) + f(q,\bar{u}) \\
\end{aligned}
\begin{aligned}
\mathbf{B}(q,\bar{u}) = \frac{\partial q_\text{next}(q,u)}{\partial u}\bigg|_{q=q,u=\bar{u}}
\end{aligned}
Note that B is highly abstracted out here, but actually corresponds to a lot of Jacobian computations.
It answers queries of "if I think of active contacts as a joint, how does my actuator torques affect forces onto the configuration of the object locally?"
\begin{aligned}
\min_{u} \quad & \|q^u_\text{next} - q^u_g\|^2_\mathbf{Q} \\
\text{s.t.} \quad & q^u_\text{next} = \mathbf{B}^u(q,\bar{u})(u - \bar{u}) + f^u(q,\bar{u}) \\
& |u - \bar{u}| \leq \varepsilon\mathbf{1}
\end{aligned}
Shortcomings of Linearization
\begin{aligned}
u
\end{aligned}
\begin{aligned}
q^u_\text{next}
\end{aligned}
q^u
q^a
u
\lambda
\lambda
\begin{aligned}
q^u_g
\end{aligned}
Current position
\begin{aligned}
q^a
\end{aligned}
No Gradient information!
If the object is not in contact, traditional inverse dynamics is not effective.
Fortunately, smoothing comes to the rescue.
\begin{aligned}
\min_{u} \quad & \|q^u_\text{next} - q^u_g\|^2_\mathbf{Q} \\
\text{s.t.} \quad & q^u_\text{next} = {\color{red}\mathbf{B}^u_\rho}(q,\bar{u})(u - \bar{u}) + {\color{red}f^u_\rho}(q,\bar{u}) \\
& |u - \bar{u}| \leq \varepsilon\mathbf{1}
\end{aligned}
q^u
q^a
u
\lambda
\lambda
rho subscript denotes smoothing
Shortcomings of Linearization
0.5m
0.0m
We have linearized the smoothened dynamics around u = qa.
Depending on where we set the goal to be, we see three distinct regions.
\begin{aligned}
\min_{u} \quad & \|q^u_\text{next} - q^u_g\|^2_\mathbf{Q} \\
\text{s.t.} \quad & q^u_\text{next} = {\color{red}\mathbf{B}^u_\rho}(q,\bar{u})(u - \bar{u}) + {\color{red}f^u_\rho}(q,\bar{u}) \\
& |u - \bar{u}| \leq \varepsilon\mathbf{1}
\end{aligned}
q^u
q^a
u
\lambda
\lambda
rho subscript denotes smoothing
Shortcomings of Linearization
0.5m
0.0m
Region 1. Beneficial Bias
Goal = 0.61m
Optimal input
The linearized model provides helpful bias, as the optimal input moves the actuated body towards making contact.
\begin{aligned}
\min_{u} \quad & \|q^u_\text{next} - q^u_g\|^2_\mathbf{Q} \\
\text{s.t.} \quad & q^u_\text{next} = {\color{red}\mathbf{B}^u_\rho}(q,\bar{u})(u - \bar{u}) + {\color{red}f^u_\rho}(q,\bar{u}) \\
& |u - \bar{u}| \leq \varepsilon\mathbf{1}
\end{aligned}
q^u
q^a
u
\lambda
\lambda
rho subscript denotes smoothing
Shortcomings of Linearization
0.5m
0.0m
Region 2. Hurtful Bias
Goal = 0.52m
Optimal input
If you command the actuated body to hold position, the unactuated body will be pushed away due to smoothing.
The actuated body wants to go backwards in order to decrease this effect if the goal is not too in front of the unactuated body.
\begin{aligned}
\min_{u} \quad & \|q^u_\text{next} - q^u_g\|^2_\mathbf{Q} \\
\text{s.t.} \quad & q^u_\text{next} = {\color{red}\mathbf{B}^u_\rho}(q,\bar{u})(u - \bar{u}) + {\color{red}f^u_\rho}(q,\bar{u}) \\
& |u - \bar{u}| \leq \varepsilon\mathbf{1}
\end{aligned}
q^u
q^a
u
\lambda
\lambda
rho subscript denotes smoothing
Shortcomings of Linearization
0.5m
0.0m
Region 3. Violation of unilateral contact
Goal = 0.45m
Optimal input
If you set the goal to behind the unactuated body, the linear model thinks that it can pull, and will move backwards.
Reachable Set Computation
\begin{aligned}
q^u_g
\end{aligned}
Simulator rollouts of a action norm-ball
Ellipsoid informed by the local B matrix
Similarly, we are horribly off in our approximation of the reachable set constructed from the B matrix.
Clearly we need to think about forces in order to property define reachable sets under linearization
But we don't have contact forces as a decision variable...how?
Linearizing an Optimal Solution
\begin{aligned}
\min_x \quad & x^\intercal \mathbf{Q} x + b(\theta)^\intercal x \\
\quad & \mathbf{A}x \leq b
\end{aligned}
What does it mean to create a locally linear model of a solution of an optimization problem?
\begin{aligned}
x(\theta) & = x^*(\bar{\theta}) + \mathbf{J}(\bar{\theta})(\theta - \bar{\theta}) \\
\end{aligned}
Generic QP
Generic QP
Linear Model of Primal solution vs. Parameters
\begin{aligned}
\mathbf{J}(\bar{\theta})\coloneqq \frac{\partial \delta q^*(\theta)}{\partial \theta}\bigg|_{\theta=\bar{\theta}}
\end{aligned}
Linearizing an Optimal Solution
\begin{aligned}
\min_x \quad & x^\intercal \mathbf{Q} x + b(\theta)^\intercal x \\
\quad & \mathbf{A}x \leq b
\end{aligned}
What does it mean to create a locally linear model for an optimization problem?
\begin{aligned}
x(\theta) & = x^*(\bar{\theta}) + \mathbf{J}(\bar{\theta})(\theta - \bar{\theta}) \\
\end{aligned}
Generic QP
Generic QP
Linear Model of Primal solution vs. Parameters
\begin{aligned}
\mathbf{J}(\bar{\theta})\coloneqq \frac{\partial x^*(\theta)}{\partial \theta}\bigg|_{\theta=\bar{\theta}}
\end{aligned}
Linear Model of Dual solution vs. Parameters
\begin{aligned}
\lambda(\theta) & = \lambda^*(\bar{\theta}) + \mathbf{\Lambda}(\bar{\theta})(\theta - \bar{\theta}) \\
\end{aligned}
\begin{aligned}
\mathbf{\Lambda}(\bar{\theta})\coloneqq \frac{\partial \lambda^*(\theta)}{\partial \theta}\bigg|_{\theta=\bar{\theta}}
\end{aligned}
We can enforce primal and dual feasibility as the domain of the linear model.
\begin{aligned}
\mathcal{D} \coloneqq \{\theta | \lambda(\theta)\geq 0, Ax(\theta) \leq b\}
\end{aligned}
New Approach at Linearization
Linearizing the dual and forcing the linear model of the dual to be positive discards the unilateral violation region.
New Approach at Linearization
Interestingly, one solution to getting rid of the hurtful bias region is to keep the slope of the linearization, but make it pass through the non-smooth dynamics.
(e.g. keep the direction, but respect the fact that if no contact will result in no movement)
Applications
- is not limited to limit surfaces ;)
- accounts for non-fixed contact modes
- respects unilateral contact
Inverse Dynamics on Optimization-based Dynamics
Proper Generalization of a Motion Cone
\begin{aligned}
\min_{u, \lambda_i} \quad & \|q^u_\text{next} - q^u_g\|^2_\mathbf{Q} \\
\text{s.t.} \quad & q^u_\text{next} = \mathbf{B}^u_\rho(\bar{q},\bar{u})(u - \bar{u}) + f^u(\bar{q},\bar{u}) \\
& \lambda_i = \mathbf{\Lambda}_\rho(\bar{q},\bar{u})(\lambda - \bar{\lambda}) + \lambda(\bar{q},\bar{u}) \\
& \lambda_i \geq 0 \\
& |u - \bar{u}| \leq \varepsilon\mathbf{1}
\end{aligned}
Key Takeaway
Whenever someone linearizes optimization-based dynamics, ask them if they've considered their domains for dual feasibility :)
Group Meeting Short Talk
By Terry Suh
