Program Listing for File ProjectConstraintError.cc

↰ Return to documentation for file (doxygen_docs/GeneralKModels/ProjectConstraintError.cc)

#include "Karana/GeneralKModels/ProjectConstraintError.h"
#include "Karana/KCore/Allocator.h"

/**
 * @file
 * @brief ProjectConstraintError implementation.
 */

namespace Karana::Models {

    namespace km = Karana::Math;
    namespace kd = Karana::Dynamics;

    ProjectConstraintError::ProjectConstraintError(
        std::string_view name,
        const kc::ks_ptr<kd::ModelManager> &mm,
        const kc::ks_ptr<kd::SubGraph> &sg,
        const kc::ks_ptr<km::Integrator> &integrator,
        const kc::ks_ptr<kd::ConstraintKinematicsSolver> &cks)
        : KModel<ProjectConstraintError, ProjectConstraintErrorParams>(name, mm)
        , _sg(sg)
        , _integ(integrator)
        , _cks(cks) {
        params = std::allocate_shared<ProjectConstraintErrorParams>(
            kc::Allocator<ProjectConstraintErrorParams>{}, std::format("{}_params", name));
    };

    kc::ks_ptr<ProjectConstraintError>
    ProjectConstraintError::create(std::string_view name,
                                   const kc::ks_ptr<kd::ModelManager> &mm,
                                   const kc::ks_ptr<kd::SubGraph> &sg,
                                   const kc::ks_ptr<km::Integrator> &integrator,
                                   const kc::ks_ptr<kd::ConstraintKinematicsSolver> &cks) {
        kc::ks_ptr<ProjectConstraintError> pcm = std::allocate_shared<ProjectConstraintError>(
            kc::Allocator<ProjectConstraintError>{}, name, mm, sg, integrator, cks);
        mm->registerModel(pcm);
        return pcm;
    }

    void ProjectConstraintError::postModelStep(const km::Ktime & /*t*/, const km::Vec & /*x*/) {
        if (_sg->poseError().norm() > params->tol) {
            // std::cout << "PROJECTING" << std::endl;
            //  use the CK solver to correct the errors
            auto err = _cks->solveQ();
            // auto err = 0;
            if (err > 1e-10) {
                throw std::invalid_argument(std::format(
                    "`{} (ProjectConstraintError)` - large {} Q pose error", name(), err));
            }

            err = _cks->solveU();
            if (err > 1e-10) {
                throw std::invalid_argument(std::format(
                    "`{} (ProjectConstraintError)` - large {} U velocity error", name(), err));
            }

            // do a soft reset on the integrtor
            model_manager->setState(model_manager->assembleState());
        }
    }

    ProjectConstraintErrorParams::ProjectConstraintErrorParams(std::string_view name)
        : KModelParams(name) {}

    bool ProjectConstraintErrorParams::isReady() const {
        bool flag = true;
        if (km::isNotReadyNaN(tol)) {
            kc::warn("Parameter tol is not ready.");
            flag = false;
        }
        return flag;
    }
    // Destructor included for MacOS builds. Must have a key-function out-of-line to avoid dulpicate
    // symbols.
    ProjectConstraintError::~ProjectConstraintError(){};
} // namespace Karana::Models