Program Listing for File PinVelControl.cc

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

#include "Karana/GeneralKModels/PinVelControl.h"
#include "Karana/KCore/Allocator.h"
#include "Karana/SOADyn/CompoundSubhinge.h"

/**
 * @file
 * @brief PinVelControl implementation.
 */

namespace Karana::Models {

    namespace kd = Karana::Dynamics;

    PinVelControl::PinVelControl(std::string_view name,
                                 const kc::ks_ptr<kd::ModelManager> &mm,
                                 const kc::ks_ptr<kd::PhysicalSubhinge> &sh,
                                 const kc::ks_ptr<ProfileGenerator<km::ArrayVec>> &traj)
        : KModel<PinVelControl, PinVelControlParams>(name, mm)
        , traj(traj)
        , _sh(sh) {
        if (sh->subhingeType() != kd::SubhingeType::PIN) {
            throw std::runtime_error(
                std::format("Subhinge for PinVelControl {} must be PIN", name));
        }
        params = std::allocate_shared<PinVelControlParams>(kc::Allocator<PinVelControlParams>{},
                                                           std::format("{}_params", name));

        // Resize the generalized force vector appropriately
        _t.resize(_sh->nU());

        // Start the internal state at zero so the integral gain can be calculated appropriately
        _e_prev.resize(sh->nQ());
        _e_prev.setZero();

        _integral_contrib.resize(sh->nQ());
        _integral_contrib.setZero();

        _prev_t = km::Ktime{0};

        // keep track of multibody objects used by this model
        if (auto psh = kc::dynamic_pointer_cast<kd::PhysicalSubhinge>(sh); psh) {
            _multibody_objs.physical_subhinges.push_back(psh);
        } else if (auto csh = kc::dynamic_pointer_cast<kd::CompoundSubhinge>(sh); csh) {
            _multibody_objs.compound_subhinges.push_back(csh);
        }
    };

    kc::ks_ptr<PinVelControl>
    PinVelControl::create(std::string_view name,
                          const kc::ks_ptr<kd::ModelManager> &mm,
                          const kc::ks_ptr<kd::PhysicalSubhinge> &sh,
                          const kc::ks_ptr<ProfileGenerator<km::ArrayVec>> &traj) {
        kc::ks_ptr<PinVelControl> pid =
            std::allocate_shared<PinVelControl>(kc::Allocator<PinVelControl>{}, name, mm, sh, traj);
        mm->registerModel(pid);
        return pid;
    }

    void PinVelControl::preDeriv(const km::Ktime &t, const km::Vec &) {
        _t = params->kp * (traj->getQ(t) - _sh->getU().array()) + _integral_contrib;

        if (debug_model) [[unlikely]] {
            stdDebugMsg(
                std::format("error {}", km::dumpString(traj->getQ(t) - _sh->getU().array())));
            stdDebugMsg(std::format("output torque {}", km::dumpString(_t)));
        }
        _sh->accumT(_t);
    }

    void PinVelControl::preModelStep(const km::Ktime &t, const km::Vec &) {
        // Calculate the integral contribution for this step
        _integral_contrib += params->ki * (_e_prev * km::ktimeToSeconds(t - _prev_t));

        // Calculate new error for next time
        _e_prev = traj->getQ(t) - _sh->getU().array();

        // Set _prev_t to t for next time
        _prev_t = t;

        if (debug_model) [[unlikely]] {
            stdDebugMsg(std::format("preModelStep integral error {}", km::dumpString(_e_prev)));
        }
    };

    const kc::ks_ptr<kd::PhysicalSubhinge> &PinVelControl::getSubhinge() const { return _sh; }

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

    PinVelControlParams::PinVelControlParams(std::string_view name)
        : KModelParams(name) {
        kp = km::notReadyNaN;
        ki = km::notReadyNaN;
    }

    bool PinVelControlParams::isReady() const {
        bool flag = true;
        if (km::isNotReadyNaN(kp)) {
            kc::warn("Parameter kp is not ready.");
            flag = false;
        }
        if (km::isNotReadyNaN(ki)) {
            kc::warn("Parameter ki is not ready.");
            flag = false;
        }
        return flag;
    }

    kc::ks_ptr<kc::BaseVars> PinVelControlParams::_getVars() const {
        return PinVelControlParamsVars::create(kc::static_pointer_cast<const PinVelControlParams>(
            kc::ks_ptr<const Base>(shared_from_this())));
    };

    PinVelControlParamsVars::PinVelControlParamsVars(
        const kc::ks_ptr<const PinVelControlParams> &model)
        : Karana::Core::BaseVars(model) {

        kp = Karana::Core::Var_T<double>::create(
            "kp", [model] { return model->kp; }, "Proportional gain", "");

        ki = Karana::Core::Var_T<double>::create(
            "ki", [model] { return model->ki; }, "Integral gain", "");
    }

    PinVelControlParamsVars::~PinVelControlParamsVars(){};

    kc::ks_ptr<PinVelControlParamsVars>
    PinVelControlParamsVars::create(const kc::ks_ptr<const PinVelControlParams> &model) {
        return std::allocate_shared<PinVelControlParamsVars>(
            kc::Allocator<PinVelControlParamsVars>{}, model);
    }

    kc::NestedVars PinVelControlParamsVars::getAllVars() const {
        auto out = Karana::Core::BaseVars::getAllVars();
        kc::addVarsToStruct(out, kp, ki);

        return out;
    }

} // namespace Karana::Models