Program Listing for File SubhingeForceLimits.cc

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

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

/**
 * @file
 * @brief SubhingeForceLimits implementation.
 */

namespace Karana::Models {

    namespace kd = Karana::Dynamics;

    SubhingeForceLimits::SubhingeForceLimits(std::string_view name,
                                             const kc::ks_ptr<kd::ModelManager> &mm,
                                             const kc::ks_ptr<kd::PhysicalSubhinge> &sh)
        : KModel<SubhingeForceLimits, SubhingeForceLimitsParams>(name, mm)
        , _sh(sh) {
        params = std::allocate_shared<SubhingeForceLimitsParams>(
            kc::Allocator<SubhingeForceLimitsParams>{}, std::format("{}_params", name));
    };

    kc::ks_ptr<SubhingeForceLimits>
    SubhingeForceLimits::create(std::string_view name,
                                const kc::ks_ptr<kd::ModelManager> &mm,
                                const kc::ks_ptr<kd::PhysicalSubhinge> &sh) {
        kc::ks_ptr<SubhingeForceLimits> sfl = std::allocate_shared<SubhingeForceLimits>(
            kc::Allocator<SubhingeForceLimits>{}, name, mm, sh);
        mm->registerModel(sfl);
        return sfl;
    }

    void SubhingeForceLimits::preDeriv(const km::Ktime &, const km::Vec &) {
        km::Vec T = _sh->getT();
        for (Eigen::Index k = 0; k < T.size(); k++) {
            T[k] = std::clamp(T[k], params->lower_limits[k], params->upper_limits[k]);
        }
        _sh->setT(T);
    }

    SubhingeForceLimitsParams::SubhingeForceLimitsParams(std::string_view name)
        : KModelParams(name) {
        lower_limits.resize(1);
        km::makeNotReady(lower_limits);
        upper_limits.resize(1);
        km::makeNotReady(upper_limits);
    }

    bool SubhingeForceLimitsParams::isReady() const {
        bool flag = true;
        if (not km::isReady(lower_limits)) {
            kc::warn("Parameter lower_limits is not ready.");
            flag = false;
        }
        if (not km::isReady(upper_limits)) {
            kc::warn("Parameter upper_limits is not ready.");
            flag = false;
        }
        if (lower_limits.size() != upper_limits.size()) {
            kc::warn("The sizes of upper_limits and lower_limits don't match.");
            flag = false;
        }
        return flag;
    }

    bool SubhingeForceLimits::isReady() const {
        bool flag = true;
        if (_sh->nQ() != size_t(params->lower_limits.size())) {
            kc::warn("The subhinge size {} is not equal to the size of the limits {}.",
                     _sh->nQ(),
                     params->lower_limits.size());
            flag = false;
        }
        return flag;
    };

    // Destructor included for MacOS builds. Must have a key-function out-of-line to avoid dulpicate
    // symbols.
    SubhingeForceLimits::~SubhingeForceLimits(){};

} // namespace Karana::Models