Intern Meeting
2023/08/17
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.
Copy of CLF
By Terry Suh
