Отображение движения с использованием результатов инверсной динамики

Я совершенно новичок в OpenSim, так что это может быть тривиальная проблема, но все равно я не смог найти ответ на форуме или в интернете 🙁 . Я использую OpenSim 4.5 и хочу визуализировать движение на основе вывода обратной динамики (это означает использование сил и моментов).

Я вижу, что окно всплывает, но отображаемое движение даже не похоже на оригинальное. Мое начальное движение длится 15 секунд, но симуляция часто зависает после ~5 секунд. Более того, я получаю разные движения, когда запускаю один и тот же код несколько раз, даже не меняя файл движения.

Я опубликую свой код и надеюсь, что кто-то более опытный, чем я, сможет на него взглянуть, потому что мне кажется, что я упускаю что-то простое 🙏.

Вот краткое резюме кода: функция принимает модель, оригинальный файл движения .sto (для начальной позиции) и силы, произведенные симуляцией обратной динамики (тоже .sto).

1. Сначала недостающие актуаторы (6 сил и моментов, действующих на таз) добавляются в модель.
2. Затем я устанавливаю верхние и нижние пределы для координат.
3. Я перебираю координаты, указанные в списке “coordinateNames”, и использую сплайн GCV для каждой координаты, чтобы установить prescribeControlForActuator.
4. В конце я использую менеджер для интеграции до последнего момента движения.

void forwardForInverseResult(OpenSim::Model model, const std::string& inputSTOfn, const std::string& inputForceSTOfn) {
    std::cout << "Запуск симуляции..." << std::endl;

    // Содержит вывод обратной динамики (силы и моменты)
    auto inputSTO = std::unique_ptr<OpenSim::Storage>(new OpenSim::Storage(inputForceSTOfn));
    // Содержит начальную позицию модели, взятую из записанного оригинального движения.
    auto inputSTOposition = std::unique_ptr<OpenSim::Storage>(new OpenSim::Storage(inputSTOfn));

    if (inputSTO->isInDegrees())
        model.getSimbodyEngine().convertDegreesToRadians(*inputSTO);

    const double firstTime = inputSTO->getFirstTime();
    const double lastTime = inputSTO->getLastTime();

    /*  Добавление актуаторов контроллера */
    /* Этот раздел добавляет недостающие актуаторы, действующие на таз: наклон/крутение/подъем/длинный/вертикальный/боковой */
    auto controller = new OpenSim::PrescribedController();
    model.addController(controller);
    for (int i = 0; i < model.getNumCoordinates(); ++i) {
        const auto& c = model.getCoordinateSet().get(i);

        OpenSim::CoordinateActuator* ca;
        if (model.getActuators().contains(c.getName() + "_ca")) {
            std::cout << "Использование существующего " << c.getName() << "_ca." << std::endl;
            ca = OpenSim::CoordinateActuator::safeDownCast(
                &(model.updActuators().get(c.getName() + "_ca")));
            controller->addActuator(*ca);
        }
        else {
            std::cout << "Добавление нового " << c.getName() << "_ca." << std::endl;
            ca = new OpenSim::CoordinateActuator(c.getName());
            ca->setName(c.getName() + "_ca");
            ca->setOptimalForce(1.);
            ca->setMinControl(-1e3);
            ca->setMaxControl(1e3);
            model.addForce(ca);
            controller->addActuator(*ca);
        }
        controller->prescribeControlForActuator(ca->getName(), new OpenSim::Constant(0.));
    }
    /*  КОНЕЦ добавления актуаторов контроллера */

    model.setUseVisualizer(true);
    auto state = model.initSystem();

    /* Установка начальной позиции модели из файла координат. */
    for (int i = 0; i < model.getNumCoordinates(); ++i) {
        const auto& coord = model.getCoordinateSet().get(i);
        double initialValue = inputSTOposition->getStateVector(0)->getData()[i];
        coord.setValue(state, initialValue);
    }
    model.realizePosition(state);
    /* Конец установки начальной позиции модели. */

    /* Установка столбца времени */
    OpenSim::Array<double> times;
    inputSTO->getTimeColumn(times);
    std::vector<double> timesVec(times.getSize());
    // Установить вектор времени только один раз в начале
        for (int p = 0; p < times.getSize(); p++) {
            timesVec[p] = times[p];
        }
    /* Конец установки столбца времени */

    // Определение имен координат
    static const std::unordered_set<std::string> coordinateNames = {
        "Hip_RR_r", "Hip_DB_r", "Knee_FE_r", "Hip_EF_r",
        "Hip_EF_l", "Hip_RR_l", "Hip_BD_l", "Knee_FE_l",
        "Shoulder_BD_r", "Shoulder_EF_r", "Elbow_EF_r",
        "Shoulder_DB_l", "Shoulder_EF_l", "Shoulder_RR_l", "Elbow_EF_l",
        "Neck_LL", "Neck_RR", "Neck_FE", "Shoulder_RR_r", "SpineL_LL", "SpineL_RR", "SpineL_FE", "SpineU_FE",
    };

    state.setTime(firstTime);

    for (int i = 0; i < model.getNumCoordinates(); ++i) {
        auto& c = model.updCoordinateSet().get(i);

        // Если координата исключена, пропустите оставшуюся часть цикла
        // (Здесь, если функция "find" возвращает итератор конца, это означает, что координата не найдена)
        if (coordinateNames.find(c.getName()) != coordinateNames.end()) {
            continue;
        }

        // Перебор компонентов CoordinateLimitForce, чтобы применить ограничения
        for (const auto& x : model.getComponentList<OpenSim::CoordinateLimitForce>()) {
            if (x.get_coordinate() == c.getName()) {
                double upperLimitRad = SimTK::convertDegreesToRadians(x.getUpperLimit());
                double lowerLimitRad = SimTK::convertDegreesToRadians(x.getLowerLimit());

                // Настройка значения координаты в пределах ограничений
                double currentValue = c.getValue(state);
                if (currentValue > upperLimitRad) {
                    c.setValue(state, upperLimitRad);
                }
                else if (currentValue < lowerLimitRad) {
                    c.setValue(state, lowerLimitRad);
                }

                // Выйти из цикла после нахождения совпадения
                break;
            }
        }

    }

    // Начало симуляции
    auto manager = std::make_unique<OpenSim::Manager>(model, state);

    // Цикл по координатам
    for (int k = 0; k < model.getNumCoordinates(); ++k) {
        const auto& c = model.getCoordinateSet().get(k);
        cout << "Получение координаты на k:" << k << " которая есть: " << c.getName() << endl;

        // Пропустить исключенные координаты
        if (coordinateNames.find(c.getName()) == coordinateNames.end()) {
            cout << "Пропуск исключенной координаты: " << c.getName() << endl;
            continue;
        }

        // Безопасное преобразование к CoordinateActuator
        auto* ca = OpenSim::CoordinateActuator::safeDownCast(
            &(model.updActuators().get(c.getName() + "_ca")));
        if (!ca) continue;

        // Получение значения силы из файла входных сил
        OpenSim::Array<double> forceValues;

        auto columnName = c.getAbsolutePathString() + "/value";

        inputSTO->getDataColumn(columnName, forceValues);

        // Преобразование OpenSim::Array в std::vector для времени и значений силы
        std::vector<double> forceValuesVec(forceValues.getSize());
        for (int p = 0; p < times.getSize(); p++) {
            forceValuesVec[p]= forceValues[p];
        }

        auto newSpline = OpenSim::GCVSpline(5, forceValuesVec.size(), timesVec.data(), forceValuesVec.data(), "");

        controller->prescribeControlForActuator(
            controller->getActuatorSet().get(c.getName() + "_ca").getName(),
            new OpenSim::GCVSpline(newSpline));

    }

    // Интегрировать состояние модели во времени
    state = manager->integrate(lastTime);
}

Большое спасибо заранее!

Я даже пытался зафиксировать все координаты, кроме одной, и также установить все моменты/силы в входном файле в 0, кроме одной, но все равно все координаты двигались, и симуляция быстро зависала.