Nimble source code reading: DantzigBoxedLcpSolver

Posted on Edited on In Waline: Symbols count in article: 12k Reading time ≈ 11 mins.
My first time reading such a large program! Pretty precious memory

The version of the Nimblephysics is still 0.7.7

This part is about using Dantzig method to solve the LCP problem stated as follow:

find $f,v_{t+1}$

such that $f \geq 0, v_{v+1} \geq 0, f^Tv_{t+1} = 0$

$v_{t+1} = Af + b$

Main Code address: dart/external/odelcpsolver/lcp.cpp

Start from the line 780

The parameter

1
2
bool dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
                dReal *outer_w/*=nullptr*/, int nub, dReal *lo, dReal *hi, int *findex, bool earlyTermination)

$n$ is the size of $A$, which means $A$ is a $n \times n$ matrix.

$A$ is the above $A$, it should be symmetry and positive definite.

$x$ is the solved $f$

$b$ is the above $b$

$nub$ is the number of unbounded variables

$lo$ and $hi$ is the lower bounds and higher bounds of the $x$

$findex$ is the friction index

$earlyTermination$ is whether not to use PGS algorithm

The body

Unbounded Variables

Only when $nub \geq n$ can this process be activated.

1
2
3
4
5
6
7
8
9
10
11
12
if (nub >= n) {
    dReal *d = new dReal[n];
    dSetZero (d, n);

    int nskip = dPAD(n);
    dFactorLDLT (A, d, n, nskip);
    dSolveLDLT (A, d, b, n, nskip);
    memcpy (x, b, n*sizeof(dReal));

    delete[] d;
    return true;
  }

The process is mainly about:

$$ \begin{align} A &= LDL^T\\\ x &= A^{-1}b = L^{-T}D^{-1}L^{-1}b \end{align} $$

The $b$ in the above parameter might be $-b$.

Lo-Hi LCP

Start the main part.

PrePare Containers

Unfortunately, usually the $nub$ is $0$. So we have to

Things we need:

1
dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);

These new parameters are all constructed by new.

Let’s dive into its constructor and see what happened:

1
dLCP::dLCP (int _n, int _nskip, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, dReal *_Dell, dReal *_ell, dReal *_tmp, bool *_state, int *_findex, int *_p, int *_C, dReal **Arows)

The dLCP is actually a struct, and the family variables are:

1
2
3
4
5
6
7
8
9
10
const int m_n;
const int m_nskip;
int m_nub;
int m_nC, m_nN;				// size of each index set
ATYPE const m_A;				// A rows //ATYPE is dReal **
dReal *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi;	// permuted LCP problem data
dReal *const m_L, *const m_d;				// L*D*L' factorization of set C
dReal *const m_Dell, *const m_ell, *const m_tmp;
bool *const m_state;
int *const m_findex, *const m_p, *const m_C;

Basically, it just puts these parameters in corresponding family variables. Besides, all unbounded rows and columns shall be swapped to the first $nub$ rows and columns. And p will record the new permutation.

Understand this, we could know what is the adj_nub:

1
int adj_nub = lcp.getNub();// sdj_nub = m_nub

From the above code, m_code is nub, which is the number of unbounded variables. Actually, if nub greater than $0$, depending on the constructor:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
const int nub = m_nub;
{
    dReal *Lrow = m_L;
    const int nskip = m_nskip;
    for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,AROW(j),(j+1)*sizeof(dReal));
}
dFactorLDLT (m_L,m_d,nub,m_nskip);
memcpy (m_x,m_b,nub*sizeof(dReal));
dSolveLDLT (m_L,m_d,m_x,nub,m_nskip);
dSetZero (m_w,nub);
{
    int *C = m_C;
    for (int k=0; k<nub; ++k) C[k] = k;
}
m_nC = nub;
It is quite familiar to us. These codes solve a $nub \times nub$ matrix embedded in $A$ and save the results in the $m\_x$. Of course, only the first $nub$ variables in the $m\_x$.

Keep in mind that the current $x$ is all normal forces!!!

Start Solving

Now here is where the dream started:

1
for (int i=adj_nub; i<n; ++i)

This loop will finish the computation of these bounded questions.

The author’s words:

Loop over all indexes adj_nub..n-1. For index $i$, if $x(i),w(i)$ satisfy the LCP conditions then $i$ is added to the appropriate index set. Otherwise, $x(i),w(i)$ is driven either $+ve$ or $-ve$ to force it to the valid region. As we drive $x(i), x(C)$ is also adjusted to keep $w(C)$ at zero. While driving $x(i)$ we maintain the LCP conditions on the other variables $0..i-1$. We do this by watching out for other $x(i),w(i)$ values going outside the valid region, and then switching them between index sets when that happens.

The index i is the driving index and indexes i+1..n-1 are “don’t care”, i.e. When we make changes to the system those x’s will be zero and we don’t care what happens to those w’s. In other words, we only need to consider an $(i+1)(i+1)$ sub-problem of $Ax=b+w$.

Remember, the algorithm goes from $A[0][0], A[i][i]$, to $A[n][n]$.

Why $A*x=b+w$???

1
bool hit_first_friction_index = false;//this line is in the outside of the loop!!!

This bool variable is used to control to initialize the $lo$ and $hi$ constraints:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
for (int j=0; j<n; ++j) delta_w[p[j]] = x[j]; //unpermuted the answer x to make the findex valid
//x is the F_N

// set lo and hi values
for (int k=i; k<n; ++k) {
    dReal wfk = delta_w[findex[k]];
    if (wfk == 0) {//the contact force is 0
        hi[k] = 0;
        lo[k] = 0;
    }
    else {
        hi[k] = dFabs (hi[k] * wfk);//turn it to be the type 'double' //F_N * f_e
        lo[k] = -hi[k];
    }
}

The author’s words:

If we’ve hit the first friction index, we have to compute the $lo$ and $hi$ values based on the values of $x$ already computed. we have been permuting the indexes, so the values stored in the $findex$ vector are no longer valid. thus we have to temporarily unpermute the $x$ vector. for the purposes of this computation, $0*\infty = 0$ … so if the contact constraint’s normal force is $0$, there should be no tangential force applied.

Because of the existence of hit_first_friction_index, the above process could only be executed once.

Now we initialize the $Lo$ and $Hi$, giving bounds to the tangential forces.

1
bool s_error = false;

This bool variable is aimed at killing the loop, when the $s$ is negtive, which is illegal.

1
w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];

The author’s words:

Thus far we have not even been computing the $w$ values for indexes greater than $i$, so compute $w[i]$ now.

This line of code is:

$$ \begin{align} w_i &= A_{i(0C)}*x_{0C} + A_{i(C\ C+N)}*x_{C\ (C+N)} - b[i] \\ &= A_{i(0(C+N))}*x_{0(C+N)} - b[i] \end{align} $$

So :

$$ w = A_{n\ C+N}*x - b $$ 
1
2
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
bool *state = new bool[n];

Definition of the $state$

1
2
3
4
5
6
7
8
if (lo[i]==0 && w[i] >= 0) {
    lcp.transfer_i_to_N (i);
    state[i] = false;
}
else if (hi[i]==0 && w[i] <= 0) {
    lcp.transfer_i_to_N (i);
    state[i] = true;
}

Weird… Why $w[i] \geq 0\ and\ Lo[i]$ means that $x[i] = 0$? Same question to the $Hi[n]$.

$x[i] = 0$, so $i$ in $N$.

The author’s words:

If $Lo=Hi=0$ (which can happen for tangential friction when normals are $0$. Then the index will be assigned to set $N$ with some state. However, set $C$’s line has zero sizes, so the index will always remain in set $N$. With the “normal” switching logic, if $w$ changed sign then the index would have to switch to set $C$ and then back to set $N$ with an inverted state. This is pointless, and also computationally expensive. To prevent this from happening, we use the rule that indexes with $Lo=Hi=0$ will never be checked for set changes. This means that the state for these indexes may be incorrect, but that doesn’t matter.

1
2
3
4
else if (w[i]==0) {
    lcp.solve1 (delta_x,i,0,1);
    lcp.transfer_i_to_C (i);
}

In this state, $Lo[i] < 0 = w[i] = 0 < Hi[i]$. $w[i] = (Af(or\ x)-b)[i] = 0$, $i$ in $C$.

Now we only have two instances:

$$ \begin{align} w[i] > 0\ &and\ Lo[i] < 0 \\ w[i] < 0\ &and\ Hi[i] > 0 \end{align} $$

Both situations $w*x \neq 0$, so we need to do our job.

Eternal Loop

The code used a permanent loop to adjust the answer until the sets $C$ and $N$ are not changing.

1
2
3
4
5
6
7
8
9
10
int dir;
dReal dirf;
if (w[i] <= 0) {
    dir = 1;
    dirf = REAL(1.0);
}
else {
    dir = -1;
    dirf = REAL(-1.0);
}

The author’s words:

Find direction to push on $x(i)$.

As you can see, when $w[i] < 0$, $x[i]$ must increase to let $w[i] = 0$, so the direction should be $1$. The opposite is the same reason.

1
lcp.solve1 (delta_x,i,dir);

The author’s word:

compute: delta_x(C) = -dir*A(C,C)\A(C,i).

I believe it should correspond to the pseudo-code dfirection in the paper Fast Contact Force Computation for Nonpenetrating Rigid Bodies, and:

$$ delta\_x = -A_{CC}^{-1}A_{Ci} $$ 
1
2
3
4
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);//delta_w[N] = A[N]*delta_x
lcp.pN_plusequals_ANi (delta_w,i,dir);//delta_w[N] += dir*A[i][N]
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf;
//delta_w[i] = A[i][C]*delta_x[C] + dir*A[i][i]

The author’s words:

Compute: delta_w = A*delta_x … note we only care about delta_w(N) and delta_w(i), the rest is ignored

Still, this part corresponds to $\Delta a = A \Delta f$.

Now we step into the process to culculate the largest step $s$.

1
2
int cmd = 1;		// index switching command
int si = 0;		// si = index to switch if cmd>3

Used to deal with different situations.

1
dReal s = -w[i]/delta_w[i];

$s$ is used to record the maximum step.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
if (dir > 0) {
    if (hi[i] < dInfinity) {
        dReal s2 = (hi[i]-x[i])*dirf;	// was (hi[i]-x[i])/dirf	
        // step to x(i)=hi(i)
        if (s2 < s) {
            s = s2;
            cmd = 3;
        }
    }
}// try yout best to achieve higher bound
//situation 3
else {
    if (lo[i] > -dInfinity) {
        dReal s2 = (lo[i]-x[i])*dirf;	// was (lo[i]-x[i])/dirf	
        // step to x(i)=lo(i)
        if (s2 < s) {
            s = s2;
            cmd = 2;
        }
    }
}// try yout best to achieve lower bound
//situation 2

Really simple code…

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
{
	const int numN = lcp.numN();//x should be 0
    for (int k=0; k < numN; ++k) {
        const int indexN_k = lcp.indexN(k);
        if (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) 
        //have no idea why they need this judjement
        
        {
            // don't bother checking if lo=hi=0
            if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
            dReal s2 = -w[indexN_k] / delta_w[indexN_k];
            if (s2 < s) {
                s = s2;
                cmd = 4;
                si = indexN_k;
            }
        }
    }
}
//situation 4

{
    const int numC = lcp.numC();//w should be 0
    for (int k=adj_nub; k < numC; ++k) {
        const int indexC_k = lcp.indexC(k);
        if (delta_x[indexC_k] < 0 && lo[indexC_k] > -dInfinity) {
            dReal s2 = (lo[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
            if (s2 < s) {
                s = s2;
                cmd = 5;
                si = indexC_k;
            }
        }//situation 5
        if (delta_x[indexC_k] > 0 && hi[indexC_k] < dInfinity) {
            dReal s2 = (hi[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
            if (s2 < s) {
                s = s2;
                cmd = 6;
                si = indexC_k;
            }
        }
    }//situation 6
}

Need to judge whether the step is broken:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
if (s <= REAL(0.0)) {

    if (earlyTermination) {//We get PGS!! Go away, Danzig
            if (!outer_w) delete[] w;
        delete[] L;
        delete[] d;
        delete[] delta_w;
        delete[] delta_x;
        delete[] Dell;
        delete[] ell;
        #ifdef ROWPTRS
        delete[] Arows;
        #endif
        delete[] p;
        delete[] C;

        delete[] state;

        return false;
    }

    // We shouldn't be overly aggressive about printing this warning,
    // because sometimes it gets spammed if s is just a tiny bit beneath
    // 0.0.
    if (s < REAL(-1e-6)) {
        dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",
        (double)s);
    }

    if (i < n) {
        dSetZero (x+i,n-i);//bye-bye, our efforts are totally in vain 
        dSetZero (w+i,n-i);//bye-bye, our efforts are totally in vain 
    }
    s_error = true;
    break;
}

Plus these step and direction to $x$ and $w$.

1
2
3
4
5
6
7
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x, s, delta_x);
x[i] += s * dirf;

// apply w = w + s * delta_w
lcp.pN_plusequals_s_times_qN (w, s, delta_w);
w[i] += s * delta_w[i];

The equations are:

$$ \begin{align} x[C]\ &+=\ s*delta\_x[C] \\ x[i]\ &+=\ s*dir \\ w[N]\ &+=\ s*delta\_w[N] \\ w[i]\ &+=\ s*dir \end{align} $$

Since we get the maximum step length $s$, it is time to move these indexes.

1
2
3
4
case 1:		// done
    w[i] = 0;
    lcp.transfer_i_to_C (i);
    break;
cmd is $1$, which means $ s = -w[i]/delta\_w[i]$. $w[i] = 0$, so $i$ should in $C$. 
1
2
3
4
5
case 2:		// done
    x[i] = lo[i];
    state[i] = false;
    lcp.transfer_i_to_N (i);
    break;
cmd is 2, which means $s = (lo[i]-x[i])*dirf$. $x[i] = 0$, so $i$ should in $N$. 
1
2
3
4
5
case 3:		// done
    x[i] = hi[i];
    state[i] = true;
    lcp.transfer_i_to_N (i);
    break;
cmd is 3, which means $s = (hi[i]-x[i])*dirf$. $x[i] = 0$, so $i$ should in $N$. 
1
2
3
4
case 4:		// keep going
    w[si] = 0;
    lcp.transfer_i_from_N_to_C (si);
    break;
cmd is 4, which means $s = -w[indexN\_k] / delta\_w[indexN\_k]$. $w[si] = 0$, so $si$ should transfer from $N$ to $C$. 
1
2
3
4
5
case 5:		// keep going
    x[si] = lo[si];
    state[si] = false;
    lcp.transfer_i_from_C_to_N (si, nullptr);
    break;
cmd is 5, which means $s = (lo[indexC\_k]-x[indexC\_k]) / delta\_x[indexC\_k]$. $x[si] = 0$, so $si$ should transfer from $C$ to $N$. 
1
2
3
4
5
case 6:		// keep going
    x[si] = hi[si];
    state[si] = true;
    lcp.transfer_i_from_C_to_N (si, nullptr);
    break;
cmd is 6, which means $s = (hi[indexC\_k]-x[indexC\_k]) / delta\_x[indexC\_k]$. $x[si] = 0$, so $si$ should transfer from $C$ to $N$.

If the $cmd \leq3$, this means the set $N$ and $C$ are all stable, we could be saved from the while(true) loop.

Finish Inner Loop

Note that if $s_error = true$, we need break from the main loop right now:

1
2
3
if (s_error) {
    break;
}

TaDa! Finish it!

When everything is done, we need to upermute the $x$ and $w$:

1
lcp.unpermute();

and delete everything, return true:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
    if (!outer_w)
          delete[] w;
    delete[] L;
    delete[] d;
    delete[] delta_w;
    delete[] delta_x;
    delete[] Dell;
    delete[] ell;
#ifdef ROWPTRS
  	delete[] Arows;
#endif
	delete[] p;
	delete[] C;

	delete[] state;

	return true;

Conclusion

It takes me two weeks to realize what is going on in this function. Really nice experience about reading a giant program. It is my first time reading these things ^_^.