Program Listing for File PinJointLimits.cc

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

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

/**
 * @file
 * @brief PinJointLimits implementation.
 */

namespace Karana::Models {

    namespace kd = Karana::Dynamics;

    PinJointLimits::PinJointLimits(std::string_view name,
                                   const kc::ks_ptr<kd::ModelManager> &mm,
                                   const kc::ks_ptr<kd::PinSubhinge> &pin)
        : KModel<PinJointLimits, PinJointLimitsParams>(name, mm)
        , _pin(pin) {
        params = std::allocate_shared<PinJointLimitsParams>(kc::Allocator<PinJointLimitsParams>{},
                                                            std::format("{}_params", name));
    };

    kc::ks_ptr<PinJointLimits> PinJointLimits::create(std::string_view name,
                                                      const kc::ks_ptr<kd::ModelManager> &mm,
                                                      const kc::ks_ptr<kd::PinSubhinge> &pin) {
        kc::ks_ptr<PinJointLimits> pjl =
            std::allocate_shared<PinJointLimits>(kc::Allocator<PinJointLimits>{}, name, mm, pin);
        mm->registerModel(pjl);
        return pjl;
    }

    void PinJointLimits::preDeriv(const km::Ktime &, const km::Vec &) {
        double q = _pin->getQ()[0];
        if (q > params->upper_limit) {
            km::Vec T(1);
            T[0] = (params->upper_limit - q) * params->k;
            if (double u = _pin->getU()[0]; u > 0) {
                T[0] -= u * params->d;
            }
            _pin->setT(T);
        }
        if (q < params->lower_limit) {
            km::Vec T(1);
            T[0] = (params->lower_limit - q) * params->k;
            if (double u = _pin->getU()[0]; u < 0) {
                T[0] -= u * params->d;
            }
            _pin->setT(T);
        }
    }

    PinJointLimitsParams::PinJointLimitsParams(std::string_view name)
        : KModelParams(name) {
        upper_limit = km::notReadyNaN;
        lower_limit = km::notReadyNaN;
        k = km::notReadyNaN;
        d = km::notReadyNaN;
    }

    bool PinJointLimitsParams::isReady() const {
        bool flag = true;
        if (km::isNotReadyNaN(k)) {
            kc::warn("Parameter k is not ready.");
            flag = false;
        }
        if (km::isNotReadyNaN(d)) {
            kc::warn("Parameter d is not ready.");
            flag = false;
        }
        if (km::isNotReadyNaN(upper_limit)) {
            kc::warn("Parameter upper_limit is not ready.");
            flag = false;
        }
        if (km::isNotReadyNaN(lower_limit)) {
            kc::warn("Parameter lower_limit is not ready.");
            flag = false;
        }
        return flag;
    }

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