A Convex, Quasi-static time-stepping scheme for rigid body contact with friction
Pang
Convex
- Time stepping simulation takes finite time steps, deals with impulses instead of forces, circumvents infinite forces in impacts.
- Time-stepping schemes for rigid body with Coulomb friction is commonly formulated as a Linear Complementarity Problem (LCP) [Stewart2000].
- Anitescu proposed an alternative formulation for Coulomb friction that can be written down as a quadratic program (QP) [Anitescu2006].
- It doesn't exactly satisfy the requirements of the Coulomb friction model.
- But it's pretty close! (explain later)
Quasi-static
- [Anitescu2006] Anitescu, Mihai. "Optimization-based simulation of nonsmooth rigid multibody dynamics." Mathematical Programming 105.1 (2006): 113-143.
- [Stewart2000] Stewart, David E. "Rigid-body dynamics with friction and impact." SIAM review 42.1 (2000): 3-39.
- Replacing Newton's 2nd law with force balance.
- ∑F=ma vs ∑F=0
- A lot of manipulation tasks are quasi-static.
- Benefits of quasi-static models: faster simulation.
- Half as many state variables as 2nd order models.
- Doesn't need the mass matrix.
Drake MultibodyPlant (1x)
QP Quasistatic (1x)
- A robot pushing a box under gravity.
- Box has 6 DOFs. Robot has 3 joints.
- Red bars represent contact forces on the box.
- Things to note:
- Contact mode changes between the box and the ground: sticking->sliding->sticking.
- After the robot loses contact with the box, it drops back to the floor.
- Looks almost identical to the full 2nd order simulation.
- The box fails to re-establish contact with the ground after losing contact with the robot.
Outline
- Recap of the Stewart-Trinkle LCP formulation for rigid body contact with Coulomb friction.
- Quasistatic systems.
- Anitescu's QP formulation for frictional contact.
Rigid body dynamics simulation with friction
Newton's 2nd law
Non-penetration
Coulomb friction
- [Stewart2000] Stewart, David E. "Rigid-body dynamics with friction and impact." SIAM review 42.1 (2000): 3-39.
LCP formulation of rigid contacts with Coulomb friction
Coulomb friction
- Sticking: contact force stays inside the friction cone.
- Sliding: the direction of contact force maximizes energy dissipation.
- Common trick: use a polyhedron approximation of the true friction cone.
- For a given relative sliding velocity, the friction force can be found by solving the following LP:
relative sliding velocity
Matrix of tangent vectors dj
Components of friction force along dj
Contact normal force.
vector of 1's
- The KKT condition of this LP gives the two friction cone constraint on the previous slide.
2nd-order vs quasi-static dynamical systems
g
g
x
x
y
y
r
r
xl
x_l
xr
x_r
y
y
ground
- qu=[xc,yc]: unactauted/object.
[xc,yc]
[x_c, y_c]
ϕ1
\phi_1
ϕ2
\phi_2
ϕ3
\phi_3
xt+1=f(xt,u)
- q=[qu,qa]
2nd Order systems: ∑F=ma
- x=[q,v]∈R10
- u=[Fxl,Fxr,Fy] ∈R3
- Force "drives" the system forward.
Quasistatic systems: ∑F=0
- x=qu∈R5
- u=qa,cmd ∈R3
- Force still exists, but it's the commanded positions of the robot that are "driving" the system forward.
- qa=[xl,xr,y]: actuated/robot.
λn1
\lambda_{n_1}
λn2
\lambda_{n_2}
Setting qa=qa,cmd can violate non-penetration constraints.
x
x
y
y
ground
- For 2nd-order systems, commanded force can never cause penetration.
x
x
y
y
ground
- But for quasistatic systems, there is nothing (yet) to stop you from commanding an infeasible configuration that has penetration!
u=[Fxl,Fxr,Fy]
u = [F_{x_l}, F_{x_r}, F_{y}]
u=[ul,ur,uy]
u = [u_l, u_r, u_y]
Fxr
F_{x_r}
Fxl
F_{x_l}
xl=ul
x_l = u_l
xr=ur
x_r = u_r
Connecting qa and qa,cmd with a spring of 0 rest length.
xl
x_l
ul
u_{l}
k
k
In free space ( fext=0 ), any difference between xl and xl,cmd would generate a non-zero force on the robot, violating force balance. Therefore xl=xl,cmd
xl
x_l
k
k
k(ul−xl)
k(u_{l} - x_l)
When facing an obstacle, the force generated by the difference between xl and xˉl is balanced by the contact force.
∑F=0⇒k(ul−xl)+fext=0
\sum F = 0 \Rightarrow k(u_l - x_l) + f_{\text{ext}} = 0
λn1
\lambda_{n_1}
ul
u_{l}
k(ul−xl)
k(u_{l} - x_l)
Deeper reasons for adding the spring...
- Hogan's view of manipulation [Hogan1985]:
- the environment is an admittance (force->velocity/displacement).
- the robot is an impedance (velocity/displacement->force).
- [Hogan1985]Hogan, Neville. "Impedance control: An approach to manipulation: Part I—Theory." (1985): 1-7.
- [Ott2008] Ott, Christian, et al. "On the passivity-based impedance control of flexible joint robots." IEEE Transactions on Robotics24.2 (2008): 416-429.
M(q)v˙q+C(q,vq)vq+Kq(q−qcmd)=τext
\bm{M}(\bm{q})\dot{\bm{v}}_{\bm{q}} + \bm{C}(\bm{q}, \bm{v_q}) \bm{v_q} + \bm{K_q} \left(\bm{q} - \bm{q}_{\text{cmd}}\right) = \bm{\tau}_{\text{ext}}
Setting vq and its derivatives to 0.
Kq(q−qcmd)=τext
\bm{K_q} \left(\bm{q} - \bm{q}_{\text{cmd}}\right) = \bm{\tau}_{\text{ext}}
- Closed-loop dynamics of the IIWA in joint-impedance mode [Ott2008]:
- Adding that spring is the same as assuming the robot is impedance-controlled.
Deeper reasons for adding the spring...
- Hogan also likes bond graphs!
- When commanded positions meet kinematic constraints, a "syntax error" is generated when parsing the graph.
- Adding a spring eliminates that error.
Combining quasi-static dynamics with the "classical" contact constraints:
Force balance of objects (unactuated).
Force balance of robots (actuated).
"Classical" complementarity constraints.
Anitescu's friction constraints
- For contact i:
vni−μivdi+hϕi≥0
v_{n_i} - \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
vni+μivdi+hϕi≥0
v_{n_i} + \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
−hϕi
- \frac{\phi_i}{h}
force∝β2i
\text{force} \propto \beta_{2i}
force∝β1i
\text{force} \propto \beta_{1i}
0≤β1⊥vn+μivd+hϕi≥0
0 \leq \beta_1 \perp v_n + \mu_i v_d + \frac{\phi_i}{h} \geq 0
0≤β2⊥vn−μivd+hϕi≥0
0 \leq \beta_2 \perp v_n - \mu_i v_d + \frac{\phi_i}{h} \geq 0
β1
\beta_1
β2
\beta_2
μβ1
\mu \beta_1
μβ2
\mu \beta_2
v~i
\tilde{\bm{v}}_i
vni
v_{n_i}
vdi
v_{d_i}
α=tan−1μi
\alpha = \tan^{-1} \mu_i
α
\alpha
α
\alpha
Feasible region for v
Friction cone
- Coordinate frame and alpha~mu.
Velocity:
- Feasible region of the contact velocity come from the RHS of complementarity constraints (1) and (2).
- Intersection of the velocity feasible region with normal axis is non-positive.
Force:
- beta: components of the contact force along the extreme rays of the friction cone.
- Normal force: β1+β2
- Tangent force: μ(β1−β2)
n~i
\tilde{\bm{n}}_i
d~i2
\tilde{\bm{d}}_{i2}
d~i1
\tilde{\bm{d}}_{i1}
α
\alpha
Separation
- vn+μivd+hϕi>0⇒ β1=0
- vn−μivd+hϕi>0⇒ β2=0
- ϕi is large ⇒v needs to be large for (1) or (2) to be active ⇒ usually violates force balance constraints.
−hϕi
- \frac{\phi_i}{h}
0≤β1⊥vn+μivd+hϕi≥0
0 \leq \beta_1 \perp v_n + \mu_i v_d + \frac{\phi_i}{h} \geq 0
0≤β2⊥vn−μivd+hϕi≥0
0 \leq \beta_2 \perp v_n - \mu_i v_d + \frac{\phi_i}{h} \geq 0
⇒no contact force.
\Rightarrow \text{no contact force.}
ni
\bm{\mathsf{n}}_i
di2
\bm{\mathsf{d}}_{i2}
di1
\bm{\mathsf{d}}_{i1}
vni−μivdi+hϕi≥0
v_{n_i} - \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
vni+μivdi+hϕi≥0
v_{n_i} + \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
vi
\bm{\mathsf{v}}_i
vni
v_{n_i}
vdi
v_{d_i}
−ϕi
-\phi_i
ni
\bm{\mathsf{n}}_i
δxi
\delta \mathsf{x}_i
δni
\delta \mathsf{n}_i
δti
\delta \mathsf{t}_i
δni−μiδti+ϕi≥0
\delta \mathsf{n}_i - \mu_i \delta \mathsf{t}_i + \phi_i \geq 0
δni+μiδti+ϕi≥0
\delta \mathsf{n}_i + \mu_i \delta \mathsf{t}_i + \phi_i \geq 0
ti
\mathsf{t}_i
Rolling/Sticking contact
- In contact: ϕi=0
- β1>0⇒ (1) is active ⇒vn+μivd=0
- β2>0⇒ (2) is active ⇒vn−μivd=0
−hϕi=0
- \frac{\phi_i}{h} = 0
βi1
\beta_{i1}
βi2
\beta_{i2}
μβi1
\mu \beta_{i1}
μβi2
\mu \beta_{i2}
0≤β1⊥vn+μivd+hϕi≥0
0 \leq \beta_1 \perp v_n + \mu_i v_d + \frac{\phi_i}{h} \geq 0
0≤β2⊥vn−μivd+hϕi≥0
0 \leq \beta_2 \perp v_n - \mu_i v_d + \frac{\phi_i}{h} \geq 0
⇒vn=vd=0
\Rightarrow v_n = v_d = 0
contact impulse
- Exactly the same as sticking Coulomb friction.
vni−μivdi+hϕi≥0
v_{n_i} - \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
vni+μivdi+hϕi≥0
v_{n_i} + \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
force∝βi2
\text{force} \propto \beta_{i2}
force∝βi1
\text{force} \propto \beta_{i1}
ni
\bm{\mathsf{n}}_i
di2
\bm{\mathsf{d}}_{i2}
di1
\bm{\mathsf{d}}_{i1}
−ϕi=0
-\phi_i = 0
βi1
\beta_{i1}
βi2
\beta_{i2}
μβi1
\mu \beta_{i1}
μβi2
\mu \beta_{i2}
contact impulse
δni−μiδti+ϕi≥0
\delta \mathsf{n}_i - \mu_i \delta \mathsf{t}_i + \phi_i \geq 0
δni+μiδti+ϕi≥0
\delta \mathsf{n}_i + \mu_i \delta \mathsf{t}_i + \phi_i \geq 0
ni
\mathsf{n}_i
ti
\mathsf{t}_i
impulse∝βi2
\text{impulse} \propto \beta_{i2}
impulse∝βi1
\text{impulse} \propto \beta_{i1}
Sliding contact
- Non-zero component of v in the tangent direction ⇒ϕi>0.
- β2>0⇒ (2) is active ⇒vn−μivd+hϕi=0.
−hϕi
- \frac{\phi_i}{h}
0≤β1⊥vn+μivd+hϕi≥0
0 \leq \beta_1 \perp v_n + \mu_i v_d + \frac{\phi_i}{h} \geq 0
0≤β2⊥vn−μivd+hϕi≥0
0 \leq \beta_2 \perp v_n - \mu_i v_d + \frac{\phi_i}{h} \geq 0
- One object is floating in a "boundary layer" above the other when sliding.
- The thickness of the "boundary layer"
- decreases with smaller simulation time step h,
- increases with coefficient of friction μ.
βi2
\beta_{i2}
μβi2
\mu \beta_{i2}
vni−μivdi+hϕi≥0
v_{n_i} - \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
vni+μivdi+hϕi≥0
v_{n_i} + \mu_i v_{d_i} + \frac{\phi_i}{h} \geq 0
force∝βi2
\text{force} \propto \beta_{i2}
ni
\bm{\mathsf{n}}_i
di2
\bm{\mathsf{d}}_{i2}
di1
\bm{\mathsf{d}}_{i1}
vi
\bm{\mathsf{v}}_i
−ϕi
- \phi_i
βi2
\beta_{i2}
μβi2
\mu \beta_{i2}
impulse∝βi2
\text{impulse} \propto \beta_{i2}
ni
\bm{\mathsf{n}}_i
ti
\mathsf{t}_{i}
δxi
\delta \mathsf{x}_i
δni−μiδti+ϕi≥0
\delta \mathsf{n}_i - \mu_i \delta \mathsf{t}_i + \phi_i \geq 0
δni+μiδti+ϕi≥0
\delta \mathsf{n}_i + \mu_i \delta \mathsf{t}_i + \phi_i \geq 0
Equations...
is the KKT condition of
Set of signed distance pairs that are less than ϵ apart.
Force balance of objects (unactuated).
Force balance of robots (actuated).
Anitescu complementarity constraints.
Example: parallel gripper
g
g
x
x
y
y
r
r
xl
x_l
xr
x_r
y
y
ground
- qu=[xc,yc]
- qa=[xl,xr,y] with stiffness Kqa=diag(1000,1000,1000) Nm.
- μ=0.8
- Initial configuration: qu=[0,r], qa=[−r,r,0]
- Commanded position qˉa=[−0.9r,0.9r,max(−0.2t,−0.03)]
- Squeeze and pull down, then stop pulling but keep squeezing.
- Sim duration: 0.5s.
- Expected behavior:
- Normal force at contact 1, 2: 10N
- Sum of friction at contact 1, 2:
- t = 0-0.08s: grows from 0 to 16N, sticking.
- t = 0.08-0.15s: remains at 16N, sliding.
- t > 0.15s: remains at 16N, sticking.
[xc,yc]
[x_c, y_c]
ϕ1
\phi_1
ϕ2
\phi_2
ϕ3
\phi_3
Quasistatic LCP Simulation
Friction
Normal force and distance
Contact velocity
Quasistatic QP
Simulation
Friction
Normal force and distance
Contact velocity
0.1x
A closer look at contact mode transitions
- Force magnitude is reduced to fit in the same plot as velocity.
Sliding starts
Sticking
Still sliding
Sliding starting to stop
Sliding almost stopped
(1): 0≤β1⊥vn+μivd+hϕi≥0
\text{(1): }0 \leq \beta_1 \perp v_n + \mu_i v_d + \frac{\phi_i}{h} \geq 0
(2): 0≤β2⊥vn−μivd+hϕi≥0
\text{(2): }0 \leq \beta_2 \perp v_n - \mu_i v_d + \frac{\phi_i}{h} \geq 0
Some more examples
time step=0.001, μ=0.1
time step=0.001, μ=0.8
Force balance of the cube is infeasible at the end of the simulation.
When it works... Example 1
- The sphere cannot rotate.
- One contact between the sphere and the ground.
- "Vibration" when contact mode changes.
- The magnitude of "vibration" decreases with smaller time steps.
time step=0.01, μ=0.8
time step=0.001, μ=0.8
Contact modeling paper
- How good do the results need to be before I start writing?
- Is this going to be useful for multi-contact planning/control?
Contact discrimination
- Works well on simulated data.
- Not so well on real data (collected on the robot in Jan)
- Noise in τext: might be able to overcome with filtering.
- Bias in τext: might need better estimation of EE inertia.
- Now might be a good point to start writing the ICRA paper?
- Propose improvements of current methods:
- Haddadin's: intersecting line of actions with all links downward from the first link with non-zero torque.
- CPF-like (MCMC?): rejection sampling (global) + gradient descent (local), which finds all local minima instead of just one.
- Claim that the best we can do from joint torque measurements (without tracking) is finding the local minima on the links.
- Propose improvements of current methods:
The rejected paper
- The steady state joint angle is the solution of
- Tweak the controller to ensure passivity?
Δqmin.21(Δq−Δqˉ)⊺Kq(Δq−Δqˉ)subject to:JfΔq=0
\begin{aligned}
&\underset{\Delta \bm{q}}{\min.} \frac{1}{2} \left( \Delta \bm{q} - \Delta \bar{\bm{q}} \right)^\intercal \bm{K_q} \left( \Delta \bm{q} - \Delta \bar{\bm{q}} \right)\\
&\text{subject to:} \\
&\bm{J}_f \Delta \bm{q} = 0
\end{aligned}
(a)
(b)
(a)
(b)
(e) CQDC, h=0.1s
(b) SAP, h=0.1s
(c) SAP, h=0.5s
(f) CQDC, h=0.5s
(a) SAP, h=0.01s
(d) CQDC, h=0.01s
A Convex, Quasi-static time-stepping scheme for rigid body contact with friction Pang