Motion Sets for Contact-Rich Manipulation

What is sensitivity analysis really?

\begin{aligned} \min_x \quad & \frac{1}{2}x^\top \mathbf{Q} x + q^\top x \\ \text{s.t.}\quad & \mathbf{A}x \leq b \end{aligned}
\begin{aligned} \mathbf{Q}x^\star + q + {\lambda^\star}^\top \mathbf{A} & = 0 \\ \mathbf{A}x^\star & \leq b \\ \lambda^\star & \geq 0 \\ {\lambda^\star}^\top(\mathbf{A}x^\star - b) & = 0 \end{aligned}

Consider a simple QP, we want the gradient of optimal solution w.r.t. parameters,

and it's KKT conditions

\begin{aligned} \frac{\partial x^\star}{\partial q} \end{aligned}

As we perturb q, the optimal solution changes to preserve the equality constraints in KKT.

\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}
\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}

What is sensitivity analysis really?

\begin{aligned} \min_x \quad & \frac{1}{2}x^\top \mathbf{Q} x + q^\top x \\ \text{s.t.}\quad & \mathbf{A}x \leq b \end{aligned}
\begin{aligned} \mathbf{Q}x^\star + q + {\lambda^\star}^\top \mathbf{A} & = 0 \\ \mathbf{A}x^\star & \leq b \\ \lambda^\star & \geq 0 \\ {\lambda^\star}^\top(\mathbf{A}x^\star - b) & = 0 \end{aligned}

Consider a simple QP, we want the gradient of optimal solution w.r.t. parameters,

and it's KKT conditions

\begin{aligned} \frac{\partial x^\star}{\partial q} \end{aligned}

Differentiate the Equalities w.r.t. q

\begin{aligned} \begin{bmatrix} \mathbf{Q} & \mathbf{A}^\top \\ \text{D}(\lambda^\star) \mathbf{A} & \text{D}(\mathbf{A}x^\star - b) \end{bmatrix} \begin{bmatrix} \frac{\partial x^\star}{\partial q} \\ \frac{\partial \lambda^\star}{\partial q} \\ \end{bmatrix} = \begin{bmatrix} -1 \\ 0 \end{bmatrix} \end{aligned}
\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}
\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}

What is sensitivity analysis really?

\begin{aligned} \min_x \quad & \frac{1}{2}x^\top \mathbf{Q} x + q^\top x \\ \text{s.t.}\quad & \mathbf{A}x \leq b \end{aligned}
\begin{aligned} \mathbf{Q}x^\star + q + {\lambda^\star}^\top \mathbf{A} & = 0 \\ \mathbf{A}x^\star & \leq b \\ \lambda^\star & \geq 0 \\ {\lambda^\star}^\top(\mathbf{A}x^\star - b) & = 0 \end{aligned}

Consider a simple QP, we want the gradient of optimal solution w.r.t. parameters,

and it's KKT conditions

\begin{aligned} \frac{\partial x^\star}{\partial q} \end{aligned}

Throw away inactive constraints and linsolve.

Limitations of Sensitivity Analysis

\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}
\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}
\begin{aligned} \mathbf{Q}x^\star + q + {\lambda^\star}^\top \mathbf{A} & = 0 \\ \mathbf{A}x^\star & \leq b \\ \lambda^\star & \geq 0 \\ {\lambda^\star}^\top(\mathbf{A}x^\star - b) & = 0 \end{aligned}

1. Throws away inactive constraints

2. Throws away feasibility constraints

Gradients are "correct" but too local!

Remedy 1. Constraint Smoothing

\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}
\begin{aligned} {\color{red} \frac{d }{dq}} \end{aligned}
\begin{aligned} \mathbf{Q}x^\star + q + {\lambda^\star}^\top \mathbf{A} & = 0 \\ \mathbf{A}x^\star & \leq b \\ \lambda^\star & \geq 0 \\ {\lambda^\star}^\top(\mathbf{A}x^\star - b) & = {\color{red}\kappa} \end{aligned}

1. Throws away inactive constraints

2. Throws away feasibility constraints

Gradients are "correct" but too local!

Numerous benefits to smoothing that we already know so far

 

But doesn't address the second issue with feasibility!

For most solvers, feasibility has to be handled with line search.

Remedy 2. Handling Inequalities

Connection between gradients and "best linear model" still generalizes.

\begin{aligned} \min_x \quad & \frac{1}{2}x^\top \mathbf{Q} x + q^\top x \\ \text{s.t.}\quad & \mathbf{A}x \leq b \end{aligned}

What's the best linear model that locally approximates xstar as a function of q?

Remedy 2. Handling Inequalities

Connection between gradients and "best linear model" still generalizes.

\begin{aligned} \bar{x}^\star(q) \coloneqq x^\star(\bar{q}) + \frac{\partial x^\star}{\partial q}\bigg|_{q=\bar{q}} \delta q \end{aligned}
\begin{aligned} \min_x \quad & \frac{1}{2}x^\top \mathbf{Q} x + q^\top x \\ \text{s.t.}\quad & \mathbf{A}x \leq b \end{aligned}

What's the best linear model that locally approximates xstar as a function of q?

IF we didn't have feasibility constraints, the answer is Taylor expansion

Connection between gradients and "best linear model" still generalizes.

\begin{aligned} \bar{x}^\star(q) \coloneqq x^\star(\bar{q}) + \frac{\partial x^\star}{\partial q}\bigg|_{q=\bar{q}} \delta q \end{aligned}
\begin{aligned} \min_x \quad & \frac{1}{2}x^\top \mathbf{Q} x + q^\top x \\ \text{s.t.}\quad & \mathbf{A}x \leq b \end{aligned}

What's the best linear model that locally approximates xstar as a function of q?

\begin{aligned} \mathbf{Q}x^\star + q + {\lambda^\star}^\top \mathbf{A} & = 0 \\ \mathbf{A}x^\star & \leq b \\ \lambda^\star & \geq 0 \\ {\lambda^\star}^\top(\mathbf{A}x^\star - b) & = {\color{red}\kappa} \end{aligned}

But not every direction of dq results in feasibility!

 

KKT Conditions

Remedy 2. Handling Inequalities

\begin{aligned} \bar{x}^\star(q) \coloneqq x^\star(\bar{q}) + \frac{\partial x^\star}{\partial q}\bigg|_{q=\bar{q}} \delta q \end{aligned}
\begin{aligned} \mathbf{Q}x^\star + q + {\lambda^\star}^\top \mathbf{A} & = 0 \\ \mathbf{A}x^\star & \leq b \\ \lambda^\star & \geq 0 \\ {\lambda^\star}^\top(\mathbf{A}x^\star - b) & = {\color{red}\kappa} \end{aligned}

Linearize the Inequalities to limit directions dq.

KKT Conditions

\begin{aligned} \bar{\lambda}^\star(q) \coloneqq \lambda^\star(\bar{q}) + \frac{\partial \lambda^\star}{\partial q}\bigg|_{q=\bar{q}} \delta q \end{aligned}

Set of locally "admissible" dq.

\begin{aligned} \{\delta q \;\;|\;\; \mathbf{A}\bar{x}^\star(\delta q) \leq b, \;\;\bar{\lambda}^\star(\delta q) \geq 0\} \end{aligned}

Remedy 2. Handling Inequalities

Wrench Set

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i, \lambda_i\in\mathcal{K}_i, \|\lambda\|_2\leq \varepsilon\} \end{aligned}

Recall the "Wrench Set" from old grasping literature.

from Stanford CS237b

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i, \lambda_i\in\mathcal{K}_i, \|\lambda\|_2\leq \varepsilon\} \end{aligned}

Recall the "Wrench Set" from old grasping literature.

Under quasistatic dynamics, the movement of the object is a linear transform of this wrench set.

 

\begin{aligned} \mathcal{R}_\varepsilon = \{q^\mathrm{u}_+ | q^\mathrm{u}_+ = q^\mathrm{u} + h\mathbf{M}_\mathrm{u}^{-1}(h\tau^\mathrm{u} + w), w\in\mathcal{W}\} \end{aligned}

Wrench Set

Wrench Set: Limitations

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i, \lambda_i\in\mathcal{K}_i, \|\lambda\|_2\leq \varepsilon\} \end{aligned}

Recall the "Wrench Set" from old grasping literature.

Under quasistatic dynamics, the movement of the object is a linear transform of this wrench set.

 

\begin{aligned} \mathcal{R}_\varepsilon = \{q^\mathrm{u}_+ | q^\mathrm{u}_+ = q^\mathrm{u} + h\mathbf{M}_\mathrm{u}^{-1}(h\tau^\mathrm{u} + w), w\in\mathcal{W}\} \end{aligned}

But the wrench set has limitations if we go beyond point fingers

1. Singular configurations of the manipulator

2. Self collisions within the manipulator

3. Joint Limits of the manipulator

4. Torque limits of the manipulator

The Achievable Wrench Set

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i({\color{red} u}),\lambda_i({\color{red} u})\in\mathcal{K}_i, \|{\color{red} u}\|_2\leq \varepsilon\} \end{aligned}

Write down forces as function of input instead

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i, \lambda_i\in\mathcal{K}_i, \|\lambda\|_2\leq \varepsilon\} \end{aligned}

Recall the "Wrench Set" from old grasping literature.

The Achievable Wrench Set

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i({\color{red} u}),\lambda_i({\color{red} u})\in\mathcal{K}_i, \|{\color{red} u}\|_2\leq \varepsilon\} \end{aligned}

Write down forces as function of input instead

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i, \lambda_i\in\mathcal{K}_i, \|\lambda\|_2\leq \varepsilon\} \end{aligned}

Recall the "Wrench Set" from old grasping literature.

Recall we also have linear models for these forces!

\begin{aligned} \mathcal{W}_\varepsilon = \{w | w & = \sum_i \mathbf{J}^\top_{\mathsf{u}_i}\bar{\lambda}_i({\color{red} u}),\bar{\lambda}_i({\color{red} u})\in\mathcal{K}_i, \|{\color{red} u}\|_2\leq \varepsilon,\\ & \bar{\lambda}_i(u) = \lambda_i(\bar{u}) + \frac{\partial \lambda}{\partial u} \delta u \} \end{aligned}

The Achievable Wrench Set

\begin{aligned} w(u) & = \sum_i \mathbf{J}^\top_{\mathsf{u}_i} \bar{\lambda}_i(u) \\ & =\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + \sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \end{aligned}

Let's compute the Jacobian sum

\begin{aligned} \bar{\lambda}_i(u) & = \lambda_i(\bar{u}) + \frac{\partial \lambda_i}{\partial u} \delta u \end{aligned}

The Achievable Wrench Set

\begin{aligned} w(u) & = \sum_i \mathbf{J}^\top_{\mathsf{u}_i} \bar{\lambda}_i(u) \\ & =\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + \sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \end{aligned}

Let's compute the Jacobian sum

\begin{aligned} \bar{\lambda}_i(u) & = \lambda_i(\bar{u}) + \frac{\partial \lambda_i}{\partial u} \delta u \end{aligned}
\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) +h\mathbf{M}_\mathrm{u}^{-1}w \\ & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\left[\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + \sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u\right] \end{aligned}

Transform it to generalized coordinates

The Achievable Wrench Set

\begin{aligned} w(u) & = \sum_i \mathbf{J}^\top_{\mathsf{u}_i} \bar{\lambda}_i(u) \\ & =\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + \sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \end{aligned}

Let's compute the Jacobian sum

\begin{aligned} \bar{\lambda}_i(u) & = \lambda_i(\bar{u}) + \frac{\partial \lambda_i}{\partial u} \delta u \end{aligned}
\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) +h\mathbf{M}_\mathrm{u}^{-1}w \\ & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\left[\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + \sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u\right] \end{aligned}

Transform it to generalized coordinates

\begin{aligned} \color{red} q^\mathrm{u}_+(\bar{u}) \end{aligned}

The Achievable Wrench Set

\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) +h\mathbf{M}_\mathrm{u}^{-1}w(u) \\ & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \end{aligned}

Transform it to generalized coordinates

\begin{aligned} \color{red} q^\mathrm{u}_+(\bar{u}) \end{aligned}

To see the latter term, note that from sensitivity analysis,

\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) \\ & = h\mathbf{M}_\mathrm{u}^{-1}\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\frac{\partial\lambda_i}{\partial u} \end{aligned}
\begin{aligned} {\color{red} \frac{\partial }{\partial u}} \end{aligned}

The Achievable Wrench Set

\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) +h\mathbf{M}_\mathrm{u}^{-1}w(u) \\ & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \\ & = q^\mathrm{u}_+(\bar{u}) + {\color{red}\frac{\partial q^\mathrm{u}_+}{\partial u}}\delta u \end{aligned}

Transform it to generalized coordinates

\begin{aligned} \color{red} q^\mathrm{u}_+(\bar{u}) \end{aligned}

This is the B matrix for iLQR, inverse dynamics, etc.

that differentiable simulators give

The Achievable Wrench Set

\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) +h\mathbf{M}_\mathrm{u}^{-1}w(u) \\ & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \\ & = q^\mathrm{u}_+(\bar{u}) + {\color{red}\frac{\partial q^\mathrm{u}_+}{\partial u}}\delta u \end{aligned}

Transform it to generalized coordinates

\begin{aligned} \color{red} q^\mathrm{u}_+(\bar{u}) \end{aligned}

This is the B matrix for iLQR, inverse dynamics, etc.

that differentiable simulators give

But what happened to the constraints?

The Achievable Wrench Set

\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) +h\mathbf{M}_\mathrm{u}^{-1}w(u) \\ & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \\ & = q^\mathrm{u}_+(\bar{u}) + {\color{red}\frac{\partial q^\mathrm{u}_+}{\partial u}}\delta u \end{aligned}

Transform it to generalized coordinates

\begin{aligned} \color{red} q^\mathrm{u}_+(\bar{u}) \end{aligned}
\begin{aligned} {\color{blue} \lambda_i(\bar{u}) + \frac{\partial\lambda_i}{\partial u}\delta u\in\mathcal{K}_i, \|u\|_2\leq \varepsilon} \end{aligned}

Need to add these back in through model of dual variables

Trust Region Interpretation

\begin{aligned} q^\mathrm{u}_+(u) & = q^\mathrm{u}(\bar{u}) +h\mathbf{M}_\mathrm{u}^{-1}w(u) \\ & = q^\mathrm{u}(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathsf{u}_i}\lambda_i(\bar{u}) + h\mathbf{M}_{\mathrm{u}}^{-1}\sum_i \mathbf{J}^\top_{\mathrm{u}_i}\frac{\partial \lambda_i}{\partial u}\delta u \\ & = q^\mathrm{u}_+(\bar{u}) + {\color{red}\frac{\partial q^\mathrm{u}_+}{\partial u}}\delta u \end{aligned}

Transform it to generalized coordinates

\begin{aligned} \color{red} q^\mathrm{u}_+(\bar{u}) \end{aligned}
\begin{aligned} {\color{blue} \lambda_i(\bar{u}) + \frac{\partial\lambda_i}{\partial u}\delta u\in\mathcal{K}_i, \|u\|_2\leq \varepsilon} \end{aligned}

Need to add these back in through model of dual variables

Formally Defining Motion Set

Given          , solve the dynamics to obtain the Motion Set parameters.

\begin{aligned} q_+^{\mathrm{u}}(\bar{q},\bar{u}), \; \lambda_i^\mathrm{u}(\bar{q},\bar{u}),\\ \;\mathbf{B}(\bar{q},\bar{u}) \coloneqq \frac{\partial q_+^\mathrm{u}}{\partial u}(\bar{q},\bar{u}) \; \\ \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\coloneqq \frac{\partial\lambda_i^\mathrm{u}}{\partial u}(\bar{q},\bar{u}) \end{aligned}
\begin{aligned} \bar{q},\bar{u} \end{aligned}

Define the Motion Set as 

\begin{aligned} \mathcal{M}(\bar{q},\bar{u})\coloneqq \{\bar{q}^\mathrm{u}_+ | & \bar{q}^\mathrm{u}_+ = q^\mathrm{u}_+(\bar{q},\bar{u}) + \mathbf{B}^\mathrm{u}(\bar{q},\bar{u}) \delta u, \\ & \bar{\lambda}^\mathrm{u}_i = \lambda^\mathrm{u}(\bar{q},\bar{u})_i + \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\delta u \in \mathcal{K}_i, \\ & \|\delta u \|_2 \leq \varepsilon\} \end{aligned}

+ additional constraints (joint limit, torque limit)

Formally Defining Motion Set

Given          , solve the dynamics to obtain the Motion Set parameters.

\begin{aligned} q_+^{\mathrm{u}}(\bar{q},\bar{u}), \; \lambda_i^\mathrm{u}(\bar{q},\bar{u}),\\ \;\mathbf{B}(\bar{q},\bar{u}) \coloneqq \frac{\partial q_+^\mathrm{u}}{\partial u}(\bar{q},\bar{u}) \; \\ \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\coloneqq \frac{\partial\lambda_i^\mathrm{u}}{\partial u}(\bar{q},\bar{u}) \end{aligned}
\begin{aligned} \bar{q},\bar{u} \end{aligned}

Define the Motion Set as 

\begin{aligned} \mathcal{M}(\bar{q},\bar{u})\coloneqq \{\bar{q}^\mathrm{u}_+ | & \bar{q}^\mathrm{u}_+ = q^\mathrm{u}_+(\bar{q},\bar{u}) + \mathbf{B}^\mathrm{u}(\bar{q},\bar{u}) \delta u, \\ & \bar{\lambda}^\mathrm{u}_i = \lambda^\mathrm{u}(\bar{q},\bar{u})_i + \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\delta u \in \mathcal{K}_i, \\ & \|\delta u \|_2 \leq \varepsilon\} \end{aligned}

+ additional constraints (joint limit, torque limit)

This set is always convex

Folk Physics for Apes

Inverse Dynamics using Motion Set

\begin{aligned} \min_{\delta u} \quad & \|\bar{q}^\mathrm{u}_+ - \bar{q}^\mathrm{u}_{goal}\|^2_\mathbf{Q} + \|\delta u\|^2_\mathbf{R} \\ \text{s.t.} \quad & \bar{q}^\mathrm{u}_+ = q^\mathrm{u}_+(\bar{q},\bar{u}) + \mathbf{B}^\mathrm{u}(\bar{q},\bar{u}) \delta u, \\ & {\color{red}\bar{\lambda}^\mathrm{u}_i = \lambda^\mathrm{u}(\bar{q},\bar{u})_i + \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\delta u \in \mathcal{K}_i}, \\ & {\color{red}\|\delta u \|_2 \leq \varepsilon} \end{aligned}

Primal Only

Primal-Dual

Inverse Dynamics Performance

Primal Only

Primal Dual

First feedback controller (besides MPC) that shows good performance for me

Sim2Drake Performance

Primal Only

Open Loop

LQR

Primal-Dual

Done by Tong at BDAI

Inverse Dynamics Cost vs. Motion Set

Inverse Dynamics Cost vs. Motion Set

Inverse Dynamics Performance

Inverse Dynamics Performance

Inverse Dynamics Performance

3D In-hand Reorientation

Preliminary Results for Hand

Force Control using Dual Variables

\begin{aligned} \min_{\delta u} \quad & \|\bar{q}^\mathrm{u}_+ - \bar{q}^\mathrm{u}_{goal}\|^2_\mathbf{Q} + \|\delta u\|^2_\mathbf{R} + {\color{blue} \|\bar{\lambda}_i^\mathrm{u} - \bar{\lambda}_{i,goal}^\mathrm{u}\|^2_\mathbf{P}} \\ \text{s.t.} \quad & \bar{q}^\mathrm{u}_+ = q^\mathrm{u}_+(\bar{q},\bar{u}) + \mathbf{B}^\mathrm{u}(\bar{q},\bar{u}) \delta u, \\ & {\color{red}\bar{\lambda}^\mathrm{u}_i = \lambda^\mathrm{u}(\bar{q},\bar{u})_i + \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\delta u \in \mathcal{K}_i}, \\ & {\color{red}\|\delta u \|_2 \leq \varepsilon} \end{aligned}

Primal Only

Primal-Dual

Beautifully generalizes to hybrid force/velocity controllers

May allow incorporation of tactile forces when used for estimation

Inverse Dynamics Performance

How far can we go with greedy inverse dynamics?

Why manipulation seems hard

How do we push in this direction?

How do we rotate further in presence of joint limits?

The Action-space problem in Robotics

If we had the wrong action space, the carrot problem hides a horribly difficult planning & control problem.

 

(Can no longer argue that set distance is CLF)

Motivating the Hybrid Action Space

q_{now}
q_{now}

Local Control

(Inverse Dynamics)

q_{next}
q_{next}

Actuator Placement if feasible 

Can running RL / GCS / Online Planning in this action space give us significantly better results?

Same Story

.2s

~80s

The Hybrid Action Space

q_{now}
q_{now}

Local Control

(Inverse Dynamics)

q_{next}
q_{next}

Actuator Placement if feasible 

Where do we place the actuators?

What do we mean by feasible?

Where do we do local control to?

How do we choose when to choose between each action?

Quasi-dynamic Feasibility

We're feasible if we're stable.

Distance Metric induced by Reachable Set

In a finite-input reachable set, the longer we can travel along the ray, the easier it is to reach.

Distance Metric induced by Reachable Set

In a finite-input reachable set, the longer we can travel along the ray, the easier it is to reach.

\begin{aligned} \max_c \quad & c \\ \text{s.t.}\quad & c \geq 0 \\ & c v = \bar{q}^\mathrm{u}_+ \\ & \bar{q}^\mathrm{u}_+ = q^\mathrm{u}_+(\bar{q},\bar{u}) + \mathbf{B}^\mathrm{u}(\bar{q},\bar{u}) \delta u, \\ & \bar{\lambda}^\mathrm{u}_i = \lambda^\mathrm{u}(\bar{q},\bar{u})_i + \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\delta u \in \mathcal{K}_i, \\ & \|\delta u \|_2 \leq \varepsilon\} \end{aligned}
\begin{aligned} v = \frac{q - q_{goal}}{\|q - q_{goal}\|} \end{aligned}

Distance Metric induced by Reachable Set

Then, the distance is defined by

\begin{aligned} d(q;q_{goal}) = \frac{1}{(c^\star + {\color{red}\rho})} \end{aligned}

regularization

Greedy Solution for Actuator Placement

Contact Sampling

The distance metric is incredibly useful for how we should find contacts to begin with.

\begin{aligned} \min_{\tilde{q}^\mathrm{a}}\quad & d(\tilde{q}^\mathrm{a};q_{goal}) \\ \text{s.t.}\quad & \phi(q^\mathrm{a},q^\mathrm{u}) \geq 0 \end{aligned}

no penetration

Bad if the goal is towards left.

no penetration

movable actuators

Contact Sampling

Contact Sampling

But doesn't distinguish between these two configurations

If goal is to move right, what's a better configuration?

Contact Sampling

\begin{aligned} \min_{q^\mathrm{a}}\quad & d(q^\mathrm{a},q^\mathrm{u};q_{goal}) + \alpha {\color{red}r^2_{max}}\\ \text{s.t.}\quad & \phi(q^\mathrm{a},q^\mathrm{u}) \geq 0 \end{aligned}

We can add some robustness term from classical grasping

"Maximum inscribed sphere inside motion set".

Contact Sampling

But doesn't distinguish between these two configurations

If goal is to move right, what's a better configuration?

Simple Greedy Strategy

q_{now}
q_{now}

Local Control

(Inverse Dynamics)

q_{next}
q_{next}

Actuator Placement if feasible 

Where do we place the actuators?

Where do we do local control to?

How do we choose when to choose between each action?

Find one which will minimize distance to goal according to distance metric.

Always go towards the goal.

Do actuator placement after time T.

Greedy Planning

"Sparse graph" induces much less random behavior compared to 1-step based RRT.

Limitations

1

2

3

4

5

6

1

2

3

4

1

2

3

4

The hard manipulation problems are not greedy even in this space!

epsilon-Greedy extension

q_{now}
q_{now}

Local Control

(Inverse Dynamics)

q_{next}
q_{next}

Actuator Placement if feasible 

Where do we place the actuators?

Where do we do local control to?

How do we choose when to choose between each action?

q_u^{subgoal} = \begin{cases} q_u^{goal} & \text{ w.p.} \quad p_{id} \\ \sim {\color{red} \mathcal{M}(q)} & \text{ w.p. } \quad 1 - p_{id} \end{cases}
\begin{cases} \text{Regrasp} & \text{ w.p.} \quad p \\ \text{Inverse Dynamics for time}\quad T & \text{ w.p. } \quad 1 - p \end{cases}
q_{next} = \begin{cases} \text{Choose Best}(q^i_{next}) & \text{ w.p.} \quad p_{regrasp} \\ \text{Choose Random}(q^i_{next}) & \text{ w.p. } \quad 1 - p_{regrasp} \end{cases}
q_{next}^i \sim \mathcal{P}

Selecting nodes for tree building

q_{now}
q_{now}

Local Control

(Inverse Dynamics)

q_{next}
q_{next}

Actuator Placement if feasible 

How do we select which node to grow from?

q_{next} = \begin{cases} \min_{q\in\mathcal{T}} d(q, q^u_{goal}) & \text{ w.p.} \quad p_{select} \\ \text{Choose Random}(q\in\mathcal{T}) & \text{ w.p. } \quad 1 - p_{select} \end{cases}

Results for Tree Search

Tree Search

Greedy

Results for Tree Search

Why not do RRT?

1

2

3

4

1

2

3

4

Where do we do local control to?

q_u^{subgoal} = \begin{cases} q_u^{goal} & \text{ w.p.} \quad p_{id} \\ \sim {\color{red} \mathcal{M}(q)} & \text{ w.p. } \quad 1 - p_{id} \end{cases}

Why is sampling from a random direction in a motion set a better strategy compared to choosing subgoals at random?

Asking local control to do reach a subgoal here is very inefficient.

Why not do RRT?

1

2

3

4

1

2

3

4

Asking to go to locally feasible directions is more efficient.

-25.2334 / 0.0000

-2.5983 / 0.0023

-3.9133 / 0.0003

-11.6394 / 0.0002

-1.8232 / 0.0023

-3.6396 / 0.0001

-3.4371 / 0.0008

-2.4071 / 0.0011

-2.1063 / 0.0014

-12.7220 / 0.0001

-4.5312 / 0.0006

-2.7226 / 0.0011

-5.8354 / 0.0020

-10.0993 / 0.0001

-1.6193 / 0.0020

-4.9144 / 0.0007

-4.4108 / 0.0017

-4.3626 / 0.0010

-23.6115 / 0.0001

-3.1907 / 0.0022

-3.8313 / 0.0004

-5.4226 / 0.0005

-7.1475 / 0.0006

-2.7731 / 0.0024

-10.6818 / 0.002

-2.132 / 0.0007

-2.852 / 0.0020

-1.3987 / 0.0017

-7.1905 / 0.0001

-2.8716 / 0.0013

-2.0403 / 0.0012

-1.7866 / 0.0018

Min-weight Metric

\begin{aligned} \max \quad & l \\ \text{s.t.} \quad & \bar{q}^\mathrm{u}_+ = q^\mathrm{u}_+(\bar{q},\bar{u}) + \mathbf{B}^\mathrm{u}(\bar{q},\bar{u}) \delta u, \\ & \bar{\lambda}^\mathrm{u}_i = \lambda^\mathrm{u}(\bar{q},\bar{u})_i + \mathbf{\Lambda}^\mathrm{u}_i(\bar{q},\bar{u})\delta u \\ & \bar{\lambda}^\mathrm{u}_i \geq l \\ & \|\delta u \|_2 \leq \varepsilon \end{aligned}

Planning

Better Distance Metric w. Motion Sets

Better Extension w. Inverse Dynamics

Better Goal-Conditioned Regrasping

How much does this improve previous planning?

Planning

Example old tree

Example new tree

Number of nodes in trees for 320 successful RRTs from random initial object poses

Planning

Current node q

Next node q

Inverse dynamics

Collision-free motion planning

When do we choose between options?