torque_residual is not equal to M*ddq

[longwoo]

The source code pinocchio/multibody/model.hpp line 499 explain that residual torque is Temporary corresponding to the residual torque tau - b.

1. According to the unconstrained rigid-body system equation M*ddq + b = tau. So I compute M*ddq alone, but it is not equal to residual torque.

2. M*ddq + b is not equal to tau

Test code is like this

    using namespace se3;
    Model model;
    buildModels::humanoidSimple(model);
    Data data(model);

    Eigen::VectorXd lb(model.lowerPositionLimit);
    lb.head<3>().fill(-10.);
    lb.segment<4>(3).fill(-1.);

    Eigen::VectorXd ub(model.upperPositionLimit);
    ub.head<3>().fill(10.);
    ub.segment<4>(3).fill(1.);

    Eigen::VectorXd q = randomConfiguration(model,lb,ub);
    Eigen::VectorXd v = randomConfiguration(model,lb,ub);
    Eigen::MatrixXd M = crba(model,data,q);

    // Symmetrize M
    M.triangularView<Eigen::StrictlyLower>() = M.transpose().triangularView<Eigen::StrictlyLower>();
    std::cout << "q:\n" << q << std::endl;
    std::cout << "v:\n" << v << std::endl;
    std::cout << "Joint space inertia matrix:\n" << M << std::endl;
    se3::computeAllTerms(model, data, q, v);

    std::cout <<"===================== data.ddq ===================" << std::endl;
    std::cout << data.ddq<< std::endl;
    std::cout <<"===================== data.torque_residual ===================" << std::endl;
    std::cout << data.torque_residual << std::endl;
    std::cout <<"===================== error1 ===================" << std::endl;
    std::cout << M*data.ddq-data.torque_residual << std::endl;
    std::cout <<"===================== error2 ===================" << std::endl;
    std::cout << M*data.ddq-data.nle-data.tau << std::endl;

In mujoco, equation is almost zero, is that because I missed the constrained force JTF in pinocchio?

    std::cout <<"===================== mujoco ===================" << std::endl;
    std::cout << m_M*qdd+m_H-tao-JTF << std::endl;

[jcarpent]

Hi @longwoo. It appears that you have several mistakes in your code:

In the line Eigen::VectorXd v = randomConfiguration(model,lb,ub);, randomConfiguration returns a configuration vector. What you want is v a velocity vector, not a configuration vector. In other words, v belongs to the tangent space of the configuration manifold at q. Which means that q and v might not have the same dimension. In this case, you should use directly your own linear random function between two bounds to sample v adequately.

Also, ddq is only filled via a call to aba or forwardDynamics, but you never call such a function in your test code. Then data.ddq appears to be filled with random values when data is built.

[longwoo]

@jcarpent Ha ha ha. It is a stupid question for me. It has already been checked in pinocchio/unittest/aba.cpp. What interest me is the meaning of velocity vector you mentioned. According to your description, the input v of computeAllTerms or nonLinearEffects is a velocity vector, not a configuration vector. The point is , for a floating base model , velocity vector is w.r.t. World Frame or Other Frame?

Second, the order of velocity is the same as Featherstone's Book(p12) (w_x,w_y,w_z, v_x,v_y,v_z) or (v_x,v_y,v_z, w_x,w_y,w_z)?

[jcarpent]

We are currently following the convention of Roy Featherstone who represents the velocity of each inside its local frame to retrieve a sparsity pattern in the computations. So for the free flyer, the velocity are expressed in the local frame of the joint. On the other side, we have chose to use the linear then the angular velocity inside their vectorial representation. If you look into the spatial classes like Motion, you have a enum structure with LINEAR and ANGULAR info. LINEAR is set to 0 and ANGULAR to 3.

[jcarpent]

@longwoo Do not hesitate to reopen this issue if needed.