Extract rotation and diagonlize do not work.

[stephendb]

Example inertia tensor: 0.00076275 -0.00047298 0 -0.00047298 0.0019185 0 0 0 0.0022293

btMatrix3x3 inertia(0.00076275, -0.00047298, 0, -0.00047298, 0.0019185, 0, 0, 0, 0.0022293); btQuaternion q = btQuaternion::getIdentity(); inertia.extractRotation(q);

The omega computed in extractRotation is 0 on the very first iteration of the loop resulting in exiting. This basically just returns your identity quaternion.... This function has been in bullet for a long time I'm hoping I'm just doing something wrong but...

Omega computes the sum of cross product of the columns of the identity q and the inertia tensor. This result in 0,0,0 since you get 0,0,-0.4730

• 0,0,0.4730 and 0,0,0

Since extract rotation doesn't work, neither does diagonlize.

Running eig to do vd decomp in matlab reveals correct rotation and principle axis. Most probably won't notice that these things are broken since their code is probably just ripping the diagonal from the inertia matrix and using identity for the rotation to principle axis.

[erwincoumans]

The diagonalize method is a convenience/utility function that isn't used by default in Bullet. I recently replaced the btMatrix3x3::diagonalize implementation (using Jacobi method) with a new one based on the extractRotation from this paper "A robust method to extract the rotational part of deformations", see pdf link. The original paper extractRotation code has the same issue, see below.

Can you contact the authors of the paper and report back?

How did you generate this inertia matrix? Is it positive semi-definite?

#include "E:\develop\eigen\Eigen\Dense"

void extractRotation(const Eigen::Matrix3d &A, Eigen::Quaterniond &q,
    const unsigned int maxIter)
{
    for (unsigned int iter = 0; iter < maxIter; iter++)
    {
        Eigen::Matrix3d R = q.matrix();
        Eigen::Vector3d omega = (R.col(0).cross(A.col(0)) + R.col
        (1).cross(A.col(1)) + R.col(2).cross(A.col(2))
            ) * (1.0 / fabs(R.col(0).dot(A.col(0)) + R.col
            (1).dot(A.col(1)) + R.col(2).dot(A.col(2))) +
                1.0e-9);
        double w = omega.norm();
        if (w < 1.0e-9)
            break;
        q = Eigen::Quaterniond(Eigen::AngleAxisd(w, (1.0 / w) * omega)) *
            q;
        q.normalize();
    }
}

int main()
{
    Eigen::Matrix3d A;
    A << 0.00076275, -0.00047298, 0,
        -0.00047298, 0.0019185, 0,
        0, 0, 0.0022293;
    Eigen::Quaterniond q(1, 0, 0, 0);

    const unsigned int maxIter = 100;

    extractRotation(A, q, maxIter);
    return 0;
}

[stephendb]

Thanks, I was referencing the paper you linked while looking at the functions.

Can you contact the authors of the paper and report back? I'll see if I can ping the authors.

How did you generate this inertia matrix? Solidworks, this was just one assembly link in a humanoid robot. This same problem occurs for every link in the robot. Since the inertia tensor is always symmetric it appears this will always give you 0,0,0 unless I am missing something.

SVD seems to work fine giving (which is one of many, many solutions):

[v,d] = eig(x) v = -0.9418 -0.3363 0 -0.3363 0.9418 0 0 0 1.0000 d = 0.0006 0 0 0 0.0021 0 0 0 0.0022

Once I have my simulation working I'm thinking about commenting up btMultiBody and related in doxygen format which I'll submit.

As for diagonalize being a utility function, the whole diagonal/principle axis requirement is a bit painful for implementing real robots and adds a bit of extra obfuscation. The principle axis of a link is usually fairly difficult to think about in a design context since on real robots they will be wonky and not aligned to meaningful frames, i.e., world, joint, various useful surfaces like bottoms of feet etc (usually the frames for the visual meshes).

Edit*: Since inertia tensors are always 3x3 symmetric-real there is a closed form solution for the eigenvalues (diag inertial elements) and vectors (which make up rotation matrix). Nearly all linalg solvers have this. No iteration needed. For example: Eigen::SelfAdjointEigenSolver https://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html

[erwincoumans]

The previous implementation of btMatrix3x3::diagonalize works fine with your matrix, implementation is here: https://github.com/bulletphysics/bullet3/blob/fe6c8775da6a07b56297311c32cc14bb8c9ed771/src/LinearMath/btMatrix3x3.h#L660

Probably good to restore the old implementation.

[stephendb]

Hi,

our method is intended to extract the rotational part of an arbitrary 3x3 matrix. The inertia tensor is symmetric and therefore contains no rotation information. Hence, the rotational part is zero.

To make this a bit clearer. Our method can be seen as a kind of polar decomposition which decomposes a matrix A in a orthogonal matrix R and a symmetric matrix S:

A = R*S.

Our method ensures that R is always a rotation matrix and contains no reflection. If A is already symmetric, then R will be the identity matrix.

What you search for is a main axis transformation which diagonalizes the inertia tensor. This can be done by an SVD. A polar decomposition is not the same as an SVD and cannot be directly used to diagonalize the tensor. But an SVD can also be used to extract the rotational part of a matrix.

Hope that helps,

Jan

Am 03.05.2018 um 03:48 schrieb Stephen Butler:

Howdy,

Your paper https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf was used to implement finding the rotation to diagonalize an inertia tensor.

I'm finding that there is a problem with this, when you use a symmetric matrix with the algorithm you presented no solution can be found. Inertia tensors are always symmetric.

For example try:

0.00076275, -0.00047298, 0 -0.00047298, 0.0019185, 0 0, 0, 0.0022293

Trying various starting q's as suggested in the paper always converges to q=0. However, SVD finds the solution.

You can see the discussion unfolding here: https://github.com/bulletphysics/bullet3/issues/1667

Best regards,

Stephen

[erwincoumans]

Ah, extractRotation extracts an orthogonal matrix, so the matrix R may include both rotation and scaling. Let's revert to the previous btMatrix3x3::diagonalize implementation using the Jacobi method.

[stephendb]

Yes, this is the best quick solution. However, I would suggest replacing with the 3x3 symmetric closed form solution for eigenvalues/eigenvectors, no iteration, guaranteed solution, fastest possible solution etc. I'm just using Eigen's solver, but the implementation is very few lines of code.

I looked at the previous bug report on this method, not sure how the user got a non-symmetric inertia tensor as this cannot be an inertia tensor.

[erwincoumans]

Fixed in this PR: https://github.com/bulletphysics/bullet3/pull/1668 If you like to contribute a closed-form diagonalize method, feel free to submit a PR.

[stephendb]

Thanks, will do.