Bug with the (Eigen) svd.solve()

[rafaelsaback]

Hey guys, I've found a bug concerning the AccelerationController's SVD solution. Sascha had told me about a strange behavior of the task when using the SVD solution (the output went to infinity). What I could note is that when the thruster matrix is not full rank, the svd.solution() breaks and the result goes to infinity.

I'm running an Ubuntu 14.04, where the Eigen's version is 3.2.1-1. I checked the problem for the version 3.2.5 and this was solved.

Long story short, the svd solution doesn't work properly for Eigen's version 3.2.1-1, but it works fine for the version 3.2.5.

Possible solutions:

1. Implement the pseudo-inverse calculation via SVD, instead of using the svd.solve(). The calculation is simple (I've made one version of my own) and it can be seen here.

2. Override the Eigen's version in Rock for a newer one (I don't know how to do this or even if it's possible for external packages).

Please let me know which solution sounds better for you guys.

[doudou]

The issue I see with doing our own pseudo-inverse is that we would not get all the "niceties" of eigen regarding numerical robustness and so on ... So it might work better in some cases and worst in others.

A summary of the situation:

• there are a bunch of fixes regarding jacobisvd between 3.2.1 and 3.2.5. Did you try different QR preconditioners to see if some are not affected by the bug on 3.2.1 ?
• we must add sanity checks on the output and fail hard when the generated solution is problematic. Testing for NaNs/INF is a first step. In general, we should verify that the saturated solution is not too far from what was expected (and fail hard when it happens).
• we will have to change the inversion method anyway to take saturations into account. The current situation is not sustainable IMO when we start getting some performance out of our vehicles.

We can also install eigen from source in the flat fish project.

[rafaelsaback]

So, answering your points:

1. As you have just recommended, I've tried the different available QR preconditioners, and I have also checked the different U and V computation methods (it can be Full or Thin), but the code still breaks for matrices with null singular values (non-full rank).

2. We could create an error state for when a specific field of Joints (speed, effort, and so on, depending on the chosen type) is NaN or too big (e.g. 10e+6). This would be good because right now if the saturation is not set, this problem is breaking the system.

3. I don't know exactly how you're planing to take the saturations into account in the process of inversion, but for example, if we decide to use that method that apply a weighing matrix to the thrusters, then we need to compute the pseudo inverse. Right now we're not calculating the pseudo inverse, the Eigen::solve() gives a solution for the linear system, but it doesn't generates the pseudo inverse. A solution to still use the "niceties" of the Eigen library and calculate the pseudo inverse is to use the following piece of code (apply the identity matrix instead of the efforts and torques vector):

svd->solve(Eigen::MatrixXd::Identity(thrusterMatrix.rows(), thrusterMatrix.rows()))

However, since the current version of Eigen has a bug with the solve function, this is not working for non full rank matrices. Currently I'm locally calculating the pseudo inverse using a piece of code similar to this one published in the Eigen FAQ.

The Eigen library is very (very!) important for robotics purposes and in my opinion a newer stable version of it should be included as default inside rock's environment.

[chhtz]

The issue should be fixed since Eigen 3.2.3 (which apparently did not make it into all linux-distros, yet). The relevant commit is this https://bitbucket.org/eigen/eigen/commits/ce84611 (it had been fixed in the devel-branch long time ago).
I'd generally suggest using the latest stable of Eigen (3.2.5 at the moment), but I don't know how complicated it is to make Rock use that.

[doudou]

I'd generally suggest using the latest stable of Eigen (3.2.5 at the moment), but I don't know how complicated it is to make Rock use that.

We're using 3.2.5 in our builds. Don't really want to go through the pain of pushing it to Rock mainline...