Reconsider all uses of `inverse()` etc.

[Tarrasch]

In numerical calculations one is told to avoid a lot of explicit calculations due to instability and or inefficiency. The most common example is to almost never do matrix inverse, but there are other stuff as well like explicitly calculating the Q in QR when factored using Householder reflectors. (iirc)

Whoever knows a lot about the numerical parts of dart may feel free to jump in this discussion and shed some light.

Just to show that there is at least some uses that one could investigate:

$ git grep "inverse()"

apps/balance/Controller.cpp:    MatrixXd invM = (mSkel->getMassMatrix() + mKd * mTimestep).inverse();
apps/balance/Controller.cpp:    VectorXd deltaQdot = aggregateMat.transpose() * (aggregateMat * aggregateMat.transpose()).inverse() * _deltaMomentum;
apps/hybrid/MyWindow.cpp:        VectorXd x = A.inverse() * b;
apps/pdController/Controller.cpp:    MatrixXd invM = (mSkel->getMassMatrix() + mKd * mTimestep).inverse();
docs/dart-tutorial/pdControl.tex:    .inverse();
src/dynamics/ContactDynamics.cpp:        MatrixXd Minv = mMInv;//.inverse();
src/dynamics/SkeletonDynamics.cpp:            mMInv = mM.inverse();
src/dynamics/SkeletonDynamics.cpp:            mMInv = mM.inverse();
src/dynamics/SkeletonDynamics.cpp:                    // new_qdot = newJw.inverse()*w
src/dynamics/SkeletonDynamics.cpp:                    Vector3d new_qdot = newJwBody.inverse()*oldJwBody*old_qdot;
src/dynamics/SkeletonDynamics.cpp:                    // new_qdot = newJw.inverse()*w
src/dynamics/SkeletonDynamics.cpp:                    Vector3d new_qdot = newJwBody.inverse()*oldJwBody*old_qdot;
src/kinematics/BodyNode.h:        inline Eigen::Matrix4d getWorldInvTransform() const { return mW.inverse(); } ///< Transformation from the world coordinates to the local coordinates of this body node
src/kinematics/BodyNode.h:        inline Eigen::Matrix4d getLocalInvTransform() const { return mT.inverse(); } ///< Transformation from the local coordinates of the parent node to the local coordinates of this body node
src/kinematics/Transformation.cpp:        return mTransform.inverse();
src/utils/UtilsRotation.cpp:            Quaterniond qinv = q.inverse();

The rule of thumb I think is that matrix inverse is only ok for very small matrices. So I guess investigation concretely here means that we should remove inverse where the matrices actually aren't tiny and add a "trust me I know what I'm doing comment" on the other places.

[cerdogan]

How big is a mass matrix?

[karenliu]

I thought about this recently and did some tests on 50x50 matrix. Here is the reason why the linear solver doesn't pay off.

There are two places where M^{-1} is involved.

1. When computing \ddot{q} from EOM.
2. When formulating LCP for collision, we need to compute M^{-1} N and M^{-1} B (see my tutorial on LCP).

We could use Cholesky factorization which factorizes M = LL' and solves two back substitutions to get M^{-1}b, where b is the vector on the RHS of the linear system. This is faster than inverting M and if we only care about 1 it indeed speeds up the simulation. However, computing M^{-1}N or M^{-1}B in 2 through many back substitutions is in theory similar but in practice more time consuming than directly multiplying M^{-1} with another matrix. Overall the gain in Cholesky factorization does not cover the cost in a large number of back substitutions for a matrix around 50x50.

Karen

On Feb 5, 2013, at 8:50 PM, Arash Rouhani wrote:

In numerical calculations one is told to avoid a lot of explicit calculations due to instability and or inefficiency. The most common example is to almost never do matrix inverse, but there are other stuff as well like explicitly calculating the Q in QR when factored using Householder reflectors. (iirc)

Whoever knows a lot about the numerical parts of dart may feel free to jump in this discussion and shed some light.

Just to show that there is at least some uses that one could investigate:

$ git grep "inverse()"

apps/balance/Controller.cpp: MatrixXd invM = (mSkel->getMassMatrix() + mKd * mTimestep).inverse(); apps/balance/Controller.cpp: VectorXd deltaQdot = aggregateMat.transpose() * (aggregateMat * aggregateMat.transpose()).inverse() * _deltaMomentum; apps/hybrid/MyWindow.cpp: VectorXd x = A.inverse() * b; apps/pdController/Controller.cpp: MatrixXd invM = (mSkel->getMassMatrix() + mKd * mTimestep).inverse(); docs/dart-tutorial/pdControl.tex: .inverse(); src/dynamics/ContactDynamics.cpp: MatrixXd Minv = mMInv;//.inverse(); src/dynamics/SkeletonDynamics.cpp: mMInv = mM.inverse(); src/dynamics/SkeletonDynamics.cpp: mMInv = mM.inverse(); src/dynamics/SkeletonDynamics.cpp: // new_qdot = newJw.inverse()_w src/dynamics/SkeletonDynamics.cpp: Vector3d new_qdot = newJwBody.inverse()_oldJwBody_old_qdot; src/dynamics/SkeletonDynamics.cpp: // new_qdot = newJw.inverse()_w src/dynamics/SkeletonDynamics.cpp: Vector3d new_qdot = newJwBody.inverse()_oldJwBody_old_qdot; src/kinematics/BodyNode.h: inline Eigen::Matrix4d getWorldInvTransform() const { return mW.inverse(); } ///< Transformation from the world coordinates to the local coordinates of this body node src/kinematics/BodyNode.h: inline Eigen::Matrix4d getLocalInvTransform() const { return mT.inverse(); } ///< Transformation from the local coordinates of the parent node to the local coordinates of this body node src/kinematics/Transformation.cpp: return mTransform.inverse(); src/utils/UtilsRotation.cpp: Quaterniond qinv = q.inverse(); The rule of thumb I think is that matrix inverse is only ok for very small matrices. So I guess investigation concretely here means that we should remove inverse where the matrices actually aren't tiny and add a "trust me I know what I'm doing comment" on the other places.

— Reply to this email directly or view it on GitHub.

[Tarrasch]

Alright. What about stability @karenliu ? I'm not really that savvy when it comes to linear algebra and particularly not in robotics domain, so I don't have any hunch if stability matters here or not.

Do you have your benchmarking code anywhere? It would be interesting to see if I could reproduce your results on my machine.

[Tarrasch]

I don't know enough about the actual matrices and math (back substitutions etc.) involved to change things here.