Copy of CLF

Formalizing the Contact-Sampling Problem

While True:

u^* = \text{argmin}_{u} \|f_u(q,u) - q^u_g\|^2

find u s.t.

periodically:

q_a^* = \text{argmin}_{q_a} \min_u \|f_u(q^u,q^a,u) - q^u_g\|^2

q\leftarrow f(q,u^*)

q\leftarrow q_u, q_a^*

What are we really trying to do here?

What about here?

Formalizing the Contact-Sampling Problem

While True:

u^* = \text{argmin}_{u} V(u)

find u s.t.

periodically:

q_a^* = \text{argmin}_{q_a} \min_u V(q^u, q^a, u)

q\leftarrow f(q,u^*)

q\leftarrow q_u, q_a^*

Solve optimal control problem

Choose a state that would have a minimum value function.

The Optimal Control Problem

\begin{aligned} \min_{u_{1:T}} \quad & \sum^T_{t=0} \|x_t - x_g\|^2_\mathbf{Q} + \|u_t\|^2_\mathbf{R} \\ \text{s.t.} \quad & q_{t+1} = f(q_t, u_t) \quad \forall t \\ & q_t^\mathsf{a} - \Delta \leq u_t \leq q_t^\mathsf{a} + \Delta \\ & q_\text{min}^{\mathsf{a}} \leq q_t^\mathsf{a} \leq q_\text{max}^{\mathsf{a}} \end{aligned}

First, spent a long time thinking about this from an optimization p.o.v.

Minimize some measure of distance

Dynamics constraint

Torque limit / step limit

Joint limits

The Greedy Controller

\begin{aligned} \min_{u_{1:T}} \quad & \|f^u(q_t, u_t) - q^u_g\|^2 \\ \text{s.t.}\quad & q_t^\mathsf{a} - \Delta \leq u_t \leq q_t^\mathsf{a} + \Delta \\ & q_\text{min}^{\mathsf{a}} \leq q_t^\mathsf{a} \leq q_\text{max}^{\mathsf{a}} \end{aligned}

Greedy Controller

Minimize distance between the next object state and the goal object state.

Torque limit / step limit

Joint limits

This is a constrained optimization, and I have 3 ways to offer how to solve it.

Comparable performances

\begin{aligned} \min_{u_{1:T}} \quad & \|f^u(q_t, u_t) - q^u_g\|^2 \\ & q_t^\mathsf{a} - \Delta \leq u_t \leq q_t^\mathsf{a} + \Delta \\ & q_\text{min}^{\mathsf{a}} \leq q_t^\mathsf{a} \leq q_\text{max}^{\mathsf{a}} \end{aligned}

Greedy Controller

1. Pass it to SNOPT with custom gradients.

3. Write my own constrained optimizer with Augmented Lagrangian

2. Random sampling within bounds and choosing best.

Discussion on solvers

\begin{aligned} \min_{u_{1:T}} \quad & \|f^u(q_t, u_t) - q^u_g\|^2 \\ & q_t^\mathsf{a} - \Delta \leq u_t \leq q_t^\mathsf{a} + \Delta \\ & q_\text{min}^{\mathsf{a}} \leq q_t^\mathsf{a} \leq q_\text{max}^{\mathsf{a}} \end{aligned}

Greedy Controller

1. Pass it to SNOPT with custom gradients.

3. Write my own constrained optimizer with Augmented Lagrangian

2. Random sampling within bounds and choosing best.

Finds an good solution, but with constraint violation within the iter. limit.

Perhaps best performing for this problem. Not sure if it will scale.

Found it to be more robust than SNOPT.

Solving the contact sampling problem.

\begin{aligned} \min_{q_a,u}\quad & \|f_u(q^u, q^a, u) - q^u_g\|^2 \\ \text{s.t.}\quad & q^a_\text{min} \leq u \leq q^a_\text{max} \\ & q^a_\text{min} \leq q^a \leq q^a_\text{max} \\ & q^a - \Delta \leq u \leq q^a + \Delta \end{aligned}

The "dynamics" formulation - jointly minimize over state-action pair.

(i.e. minimize the Q function)

This problem is a lot harder for multiple reasons. Let's try passing it on to the augmented Lagrangian multiplier.

Solving the contact sampling problem.

\begin{aligned} \min_{q_a,u}\quad & \|f_u(q^u, q^a, u) - q^u_g\|^2 \\ \text{s.t.}\quad & q^a_\text{min} \leq u \leq q^a_\text{max} \\ & q^a_\text{min} \leq q^a \leq q^a_\text{max} \\ & q^a - \Delta \leq u \leq q^a + \Delta \\ & \phi(q^u, q^a) \geq 0 \end{aligned}

The "dynamics" formulation - jointly minimize over state-action pair.

(i.e. minimize the Q function)

Important: qa should be non-penetrating!

The Kinematic Formulation

Instead of jointly optimizing over qa and u, is there something that qa tells us about the likely location of places that we can actuate the object in?

Step 1. If we are allowed to put any actuator on the surface of the object, how would the object move in response to the forces?

\mathbf{M}\Delta q^u = \mathbf{J}(p, q^a)^\top \lambda

Locally, for quasi-dynamic systems, this relation is described by a Jacobian.

The Kinematic Formulation

Instead of jointly optimizing over qa and u, is there something that qa tells us about the likely location of places that we can actuate the object in?

Step 2. But we need to take into consideration that this impulse was produced by a robot (e.g. cannot be produced in the null-space)

\mathbf{M}\Delta q^u = \mathbf{J}_u(p, q^u)^\top \lambda

Locally, for quasi-dynamic systems, this relation is described by a Jacobian.

\tau = \mathbf{J}_a(p, q^a)^\top \lambda

Contact as a Joint

\mathbf{M}\Delta q^u = \mathbf{J}_u(p, q^u)^\top \lambda

In the absence of any contact constraints, this imposes a "joint Jacobian" that maps actuator torques to motion of the objects.

\tau = \mathbf{J}_a(p, q^a)^\top \lambda

\mathbf{M}\Delta q^u = \underbrace{\mathbf{J}_u(p, q^u)^\top\mathbf{J}_a(p,q^a)^\dagger}_{\mathbf{J}(p,q^u,q^a)} \tau

So locally, contact can be thought of as a joint.

Contact as a Joint

\mathbf{M}\Delta q^u = \underbrace{\mathbf{J}_u(p, q^u)^\top\mathbf{J}_a(p,q^a)^\dagger}_{\mathbf{J}(p,q^u,q^a)} \tau

Note that this is intimately connected to the "Bu matrix" of the dynamics, the sensitivity of object position w.r.t. the torques.

If we inject a norm-ball in tau, the Bu matrix maps the norm ball to the "manipulability ellipsoid" of the object.

Contact Placement as Mechanism Design

The problem of contact placement can be thought of as "mechanism design" that maximizes the manipulability ellipsoid along a certain direction.

\begin{aligned} \text{find} \quad & {q^a, p, \lambda, \tau} \\ \text{s.t.} \quad & \mathbf{M}\Delta q^u = \mathbf{J}_u(p, q^u)^\top \lambda \\ \quad & \tau = \mathbf{J}_a(p, q^a)^\top \lambda \end{aligned}

The Unilateral nature of contact

\begin{aligned} \text{find} \quad & {q^a, p, \lambda, \tau} \\ \text{s.t.} \quad & \mathbf{M}\Delta q^u = \mathbf{J}_u(p, q^u)^\top \lambda \\ \quad & \tau = \mathbf{J}_a(p, q^a)^\top \lambda \\ \quad & \lambda_n(p) \geq 0 \\ \quad & \|\lambda_t(p)\|^2 \leq \mu^2 \|\lambda_n(p)\|^2 \end{aligned}

Step 3. We are still missing one important nature of contact!

We can only push, but not pull. This introduces additional constraints on the "contact joint" that often differs from other kind of joints.

The Unilateral nature of contact

\begin{aligned} \text{find} \quad & {q^a, p, \lambda, \tau} \\ \text{s.t.} \quad & \mathbf{M}\Delta q^u = \mathbf{J}_u(p, q^u)^\top \lambda \\ \quad & \tau = \mathbf{J}_a(p, q^a)^\top \lambda \\ \quad & \lambda_n(p) \geq 0 \\ \quad & \|\lambda_t(p)\|^2 \leq \mu^2 \|\lambda_n(p)\|^2 \\ \quad & \|\tau\|^2 \leq \varepsilon \end{aligned}

Points computed from this set

Simulator rollouts of a action norm-ball

Ellipsoid informed by the local

B matrix

The Unilateral nature of contact

\begin{aligned} \text{find} \quad & {q^a, p, \lambda, \tau} \\ \text{s.t.} \quad & \mathbf{M}\Delta q^u = \mathbf{J}_u(p, q^u)^\top \lambda \\ \quad & \tau = \mathbf{J}_a(p, q^a)^\top \lambda \\ \quad & \lambda_n(p) \geq 0 \\ \quad & \|\lambda_t(p)\|^2 \leq \mu^2 \|\lambda_n(p)\|^2 \\ \quad & \|\tau\|^2 \leq \varepsilon \end{aligned}

Points computed from this set

Simulator rollouts of a action norm-ball

Ellipsoid informed by the local

B matrix

The Unilateral nature of contact

\begin{aligned} \text{find} \quad & {q^a, p, \lambda, \tau} \\ \text{s.t.} \quad & \mathbf{M}\Delta q^u = \mathbf{J}_u(p, q^u)^\top \lambda \\ \quad & \tau = \mathbf{J}_a(p, q^a)^\top \lambda \\ \quad & \lambda_n(p) \geq 0 \\ \quad & \|\lambda_t(p)\|^2 \leq \mu^2 \|\lambda_n(p)\|^2 \\ \quad & \|\tau\|^2 \leq \varepsilon \end{aligned}

Points computed from this set

Simulator rollouts of a action norm-ball

Ellipsoid informed by the local

B matrix

The Smoothed Contact Joint

Step 4. The contact point and the point qa are connected by an inverse kinematics constraint, which is awkward to deal with.

We can relax this constraint by considering a "smoothed contact" relaxation that allows body to exert force from a distance.

Intern Meeting