efficiency of CentroidalMomentumMatrix::computeMatrixAndMatrixDot etc.

[wuv-ogre]

If I'm not mis-understanding, code like

jacWork[i] = proj*jac; jacVec[i].fullJacobian(mb, jacWork[i], jacFull); cmMat.block(0, 0, 6, mb.nrDof()) += jacFull;

could turn into

        auto add_in = []( const MultiBody& mb, const std::vector<int>& jointsPath, Eigen::MatrixXd& cmMat, Eigen::MatrixXd& jac )
        {
            int jacPos = 0;
            for(std::size_t index = 0; index < jointsPath.size(); ++index)
            {
                int j = jointsPath[index];
                int dof = mb.joint(j).dof();

                cmMat.block(0, mb.jointPosInDof(j), jac.rows(), dof) += jac.block(0, jacPos, jac.rows(), dof);

                jacPos += dof;
            }
        };

        const std::vector<int>& jointsPath = jacVec_[i].jointsPath();
        jacWork_[i] = proj*jac;
        add_in( mb, jointsPath, cmMat_, jacWork_[i] );

doing so bumped my frame rate from about 7 to 20 fps for a 50 something dof multibody

[gergondet]

Hi @wuv-ogre

This is indeed a nice improvement. That's pretty much the implementation of fullJacobian but the result is accumulated rather than zero-ed. I think it is relevant to add this function in the Jacobian class (something like addFullJacobian) and use it in RBDyn (although it also seems to be only useful in Momentum.cpp)

Would you have time to submit a PR?

[wuv-ogre]

done a quick check with

void Jacobian::addFullJacobian(const MultiBody& mb, const Eigen::Ref& jac, Eigen::MatrixXd& res) const { int jacPos = 0; for(std::sizet index = 0; index < jointsPath.size(); ++index) { int i = jointsPath_[index]; int dof = mb.joint(i).dof(); res.block(0, mb.jointPosInDof(i), jac.rows(), dof) += jac.block(0, jacPos, jac.rows(), dof); jacPos += dof; } }

and the momentum code going to

        jacWork_[i] = proj*jac;
        jacVec_[i].addFullJacobian(mb, jacWork_[i], cmMat_);    // uses joint paths of jacVec_[i] adds jacWork_[i] to cmMat_

        jacWork_[i] = proj*jacDot;
        jacVec_[i].addFullJacobian(mb, jacWork_[i], cmMatDot_);

        jacWork_[i] = projDot*jac;
        jacVec_[i].addFullJacobian(mb, jacWork_[i], cmMatDot_);

which gives the same results as the previous method. I'll have a go at doing pull request later, probably tomorrow, thanks gergondet

[gergondet]

Closed by #49