使用Pinocchio库测试模型时触发断言失败问题求助

在使用Pinocchio库进行3D动画开发时，通过自定义骨骼结构创建模型后，调用测试代码触发了断言失败，同时randomConfiguration生成的配置全为inf。

问题代码 #

using namespace glm;

struct Skeleton
{
    ~Skeleton();

    vec3 * translation;
    quat * rotation;
    int * parent; // guaranteed: parent[i] < i
    size_t size;
};

pinocchio::Model MakeModel(Skeleton const& skeleton)
{
    pinocchio::Model model;

    for(auto i = 0u; i < skeleton.size; ++i)
    {
        vec3 const& t = skeleton.translation[i];
        quat const& r = skeleton.rotation[i];

        auto translation = pinocchio::SE3::LinearType(t.x, t.y, t.z);
        auto rotation    = pinocchio::SE3::Quaternion(r.w, r.x, r.y, r.z);
    
//-1 is no parent; so i guess it should be 0 because thats the root parent in pinocchio?
        model.addJoint  (skeleton.parent[i]+1,
            pinocchio::JointModelRX(), // i dunno what any of these are yet
            pinocchio::SE3(rotation, translation),
            std::to_string(i));
    }

    TestModel(model);
    return model; // 修正：原代码缺少分号
}

void TestModel(pinocchio::Model & model)
{
// test the model!
    pinocchio::Data data(model);

    Eigen::VectorXd q = pinocchio::randomConfiguration(model);
// everything is inf
    std::cout << "q: " << q.transpose() << std::endl;

//assertion failure (this data doesn't belong to the model)
    pinocchio::forwardKinematics(model,data,q); // 修正：添加命名空间

  // Print out the placement of each joint of the kinematic tree
    for(int joint_id = 0; joint_id < model.njoints; ++joint_id)
        std::cout << std::setw(24) << std::left
              << model.names[joint_id] << ": "
              << std::fixed << std::setprecision(2)
              << data.oMi[joint_id].translation().transpose()
              << std::endl;

}

程序输出 #

q: inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf
Engine: void pinocchio::forwardKinematics(const pinocchio::Model&, pinocchio::Data&, const Eigen::Matrix&): Assertion `model.check(data) && "data is not consistent with model."' failed.

问题原因与解决方法 #

1. 核心问题：缺少模型收尾初始化

Pinocchio的Model在添加完所有关节后，必须调用**model.build()**完成初始化。这个函数会：

• 整理关节层级结构，建立合法的运动树
• 计算配置空间（q）的维度与边界
• 初始化模型内部的索引、参数等核心数据
• 确保模型状态合法，能被Data对象正确关联

在MakeModel函数的循环结束后、调用TestModel之前添加该步骤：

// ... 添加完所有关节后
model.build();
TestModel(model);
return model;

2. 根关节处理验证

Pinocchio默认会创建一个根关节（ID为0），所以当骨骼中parent[i] = -1时，将父关节设为0是正确的，但需要确保第一个添加的关节父ID为0，后续关节父ID正确对应已添加的关节。

3. 关节类型匹配修正

当前代码中所有关节都使用JointModelRX（单自由度绕X轴旋转），但Mixamo骨骼的关节自由度存在差异（如球关节、多轴旋转），后续需根据实际骨骼类型选择对应的JointModel（如JointModelSpherical、JointModelFreeFlyer等），否则会导致配置空间维度错误。

4. 其他小问题修正

• MakeModel函数末尾的return model缺少分号，需补全
• forwardKinematics需添加pinocchio::命名空间，避免全局命名冲突

内容的提问来源于stack exchange，提问作者SpehleonLLC