issue with spherical joint humanoid, mass matrix assert

[erwincoumans]

Unzip/copy attached into the examples directory and compile and run. They are modified to load the humanoid urdf with spherical joints. Below are also callstacks pointing to the assert (in Visual Studio).

Eigen version: laikago_opengl_example.zip

callstack:

>   laikago_opengl_eigen_example.exe!tds::EigenAlgebraT<double>::assign_block(Eigen::Matrix<double,-1,-1,0,-1,-1> & output, const Eigen::Matrix<double,-1,-1,0,-1,-1> & input, int i, int j, int m, int n, int input_i, int input_j) Line 452   C++
    laikago_opengl_eigen_example.exe!tds::mass_matrix<tds::EigenAlgebraT<double>>(tds::MultiBody<tds::EigenAlgebraT<double>> & mb, const Eigen::Matrix<double,-1,1,0,-1,1> & q, Eigen::Matrix<double,-1,-1,0,-1,-1> * M) Line 84    C++
    laikago_opengl_eigen_example.exe!tds::mass_matrix<tds::EigenAlgebraT<double>>(tds::MultiBody<tds::EigenAlgebraT<double>> & mb, Eigen::Matrix<double,-1,-1,0,-1,-1> * M) Line 132    C++
    laikago_opengl_eigen_example.exe!tds::MultiBodyConstraintSolver<tds::EigenAlgebraT<double>>::resolve_collision(const std::vector<tds::MultiBodyContactPoint<tds::EigenAlgebraT<double>>,std::allocator<tds::MultiBodyContactPoint<tds::EigenAlgebraT<double>>>> & cps, const double & dt) Line 201  C++
    laikago_opengl_eigen_example.exe!tds::World<tds::EigenAlgebraT<double>>::step(const double & dt) Line 357   C++
    laikago_opengl_eigen_example.exe!LaikagoSimulation<tds::EigenAlgebraT<double>>::operator()(const std::vector<double,std::allocator<double>> & v) Line 180   C++
    laikago_opengl_eigen_example.exe!main(int argc, char * * argv) Line 364 C++

Tiny (has known issues with spherical, so best to get Eigen version fixed first) tiny_urdf_parser_opengl_example.zip

callstack:

    tiny_urdf_parser_opengl_example.exe!TINY::DoubleUtils::FullAssert(bool a) Line 91   C++
    tiny_urdf_parser_opengl_example.exe!TINY::TinyMatrixXxX_<double,TINY::DoubleUtils,TINY::TinyVectorX>::operator()(int row, int col) Line 84  C++
    tiny_urdf_parser_opengl_example.exe!TinyAlgebra<double,TINY::DoubleUtils>::assign_block<TINY::TinyVectorX>(TINY::TinyMatrixXxX_<double,TINY::DoubleUtils,TINY::TinyVectorX> & output, const TINY::TinyMatrix6x3<double,TINY::DoubleUtils> & input, int i, int j, int m, int n, int input_i, int input_j) Line 365   C++
>   tiny_urdf_parser_opengl_example.exe!tds::mass_matrix<TinyAlgebra<double,TINY::DoubleUtils>>(tds::MultiBody<TinyAlgebra<double,TINY::DoubleUtils>> & mb, const TINY::TinyVectorX<double,TINY::DoubleUtils> & q, TINY::TinyMatrixXxX_<double,TINY::DoubleUtils,TINY::TinyVectorX> * M) Line 86    C++
    tiny_urdf_parser_opengl_example.exe!tds::mass_matrix<TinyAlgebra<double,TINY::DoubleUtils>>(tds::MultiBody<TinyAlgebra<double,TINY::DoubleUtils>> & mb, TINY::TinyMatrixXxX_<double,TINY::DoubleUtils,TINY::TinyVectorX> * M) Line 132  C++

[tiny_urdf_parser_opengl_example.zip](https://github.com/google-research/tiny-differentiable-simulator/files/5936022/tiny_urdf_parser_opengl_example.zip)
[tiny_urdf_parser_opengl_example.zip](https://github.com/google-research/tiny-differentiable-simulator/files/5936023/tiny_urdf_parser_opengl_example.zip)
[tiny_urdf_parser_opengl_example.zip](https://github.com/google-research/tiny-differentiable-simulator/files/5936024/tiny_urdf_parser_opengl_example.zip)

tiny_urdf_parser_opengl_example.exe!tds::MultiBodyConstraintSolver<TinyAlgebra<double,TINY::DoubleUtils>>::resolve_collision(const std::vector<tds::MultiBodyContactPoint<TinyAlgebra<double,TINY::DoubleUtils>>,std::allocator<tds::MultiBodyContactPoint<TinyAlgebra<double,TINY::DoubleUtils>>>> & cps, const double & dt) Line 201  C++
    tiny_urdf_parser_opengl_example.exe!tds::World<TinyAlgebra<double,TINY::DoubleUtils>>::step(const double & dt) Line 357 C++
    tiny_urdf_parser_opengl_example.exe!ContactSimulation<TinyAlgebra<double,TINY::DoubleUtils>>::operator()(const std::vector<double,std::allocator<double>> & v) Line 156 C++
    tiny_urdf_parser_opengl_example.exe!main(int argc, char * * argv) Line 335  C++

[erwincoumans]

Swapped the 6,3 to 3,6 indices for the transposed case in mass_matrix.hpp:

if (mb.is_floating()) {
        Fi = mb[j].X_parent.apply(Fi, true);
        Algebra::assign_block(*M, Fi, 0, qd_i, 6, 3);
        Algebra::assign_block(*M,Algebra::transpose(Fi), qd_i, 0, 3, 6);
      }

then some NaN appears here (NaN was introduced somewhere inside mb_constraintsolver->resolve_collision) apparently, the mass matrix cannot be inverted (leading to NaN).

>   laikago_opengl_eigen_example.exe!tds::forward_kinematics<tds::EigenAlgebraT<double>>(tds::MultiBody<tds::EigenAlgebraT<double>> & mb, const Eigen::Matrix<double,-1,1,0,-1,1> & q, const Eigen::Matrix<double,-1,1,0,-1,1> & qd, const Eigen::Matrix<double,-1,1,0,-1,1> & qdd) Line 37 C++
    laikago_opengl_eigen_example.exe!tds::forward_dynamics<tds::EigenAlgebraT<double>>(tds::MultiBody<tds::EigenAlgebraT<double>> & mb, const Eigen::Matrix<double,-1,1,0,-1,1> & q, const Eigen::Matrix<double,-1,1,0,-1,1> & qd, const Eigen::Matrix<double,-1,1,0,-1,1> & tau, const Eigen::Matrix<double,3,1,0,3,1> & gravity, Eigen::Matrix<double,-1,1,0,-1,1> & qdd) Line 47 C++
    laikago_opengl_eigen_example.exe!tds::forward_dynamics<tds::EigenAlgebraT<double>>(tds::MultiBody<tds::EigenAlgebraT<double>> & mb, const Eigen::Matrix<double,3,1,0,3,1> & gravity) Line 304   C++
    laikago_opengl_eigen_example.exe!LaikagoSimulation<tds::EigenAlgebraT<double>>::operator()(const std::vector<double,std::allocator<double>> & v) Line 174   C++
    laikago_opengl_eigen_example.exe!main(int argc, char * * argv) Line 364 C++

[PaulSchreiner]

Hi Erwin,

Sorry it took so long but I finally got to looking at it. It was a copy/paste error in lines 101/102. I changed (*M)(qd_i + ii, qd_j) = Hij[ii]; (*M)(qd_j, qd_i + ii) = Hij[ii]; to

(*M)(qd_i, qd_j + ii) = Hij[ii]; (*M)(qd_j + ii, qd_i) = Hij[ii]; and that, together with the change you wrote in the comment fixed it. I'll make a new pull request for the fix.

[erwincoumans]

Thanks a lot for spending time on this Paul, very much appreciate it!