Program Listing for File PID.cc

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

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

/**
 * @file
 * @brief PID implementation.
 */

namespace Karana::Models {

    namespace kd = Karana::Dynamics;

    PID::PID(std::string_view name,
             const kc::ks_ptr<kd::ModelManager> &mm,
             const kc::ks_ptr<kd::SubhingeBase> &sh,
             const kc::ks_ptr<ProfileGenerator<km::ArrayVec>> &traj)
        : KModel<PID, PIDParams>(name, mm)
        , traj(traj)
        , _sh(sh)
        , _is_attitude(sh->subhingeType() == kd::SubhingeType::SPHERICAL) {
        params = std::allocate_shared<PIDParams>(kc::Allocator<PIDParams>{},
                                                 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<PID> PID::create(std::string_view name,
                                const kc::ks_ptr<kd::ModelManager> &mm,
                                const kc::ks_ptr<kd::SubhingeBase> &sh,
                                const kc::ks_ptr<ProfileGenerator<km::ArrayVec>> &traj) {
        kc::ks_ptr<PID> pid = std::allocate_shared<PID>(kc::Allocator<PID>{}, name, mm, sh, traj);
        mm->registerModel(pid);
        return pid;
    }

    void PID::preDeriv(const km::Ktime &t, const km::Vec &) {
        if (_is_attitude) {
            // Attitude error
            km::Vec3 dark = (km::UnitQuaternion(km::RotationVector(traj->getQ(t))) *
                             km::UnitQuaternion(km::RotationVector(-_sh->getQ())))
                                .toRotationVector()
                                .toVec3();
            _t = params->kp * (dark.array()) + params->kd * (traj->getU(t) - _sh->getU().array()) +
                 _integral_contrib;

            if (debug_model) [[unlikely]] {
                stdDebugMsg(std::format("angle error {}", km::dumpString(dark)));
                stdDebugMsg(std::format("angular velocity error {}",
                                        km::dumpString(traj->getU(t) - _sh->getU().array())));
                stdDebugMsg(std::format("output torque {}", km::dumpString(_t)));
            }
        } else {
            _t = params->kp * (traj->getQ(t) - _sh->getQ().array()) +
                 params->kd * (traj->getU(t) - _sh->getU().array()) + _integral_contrib;

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

    void PID::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
        if (_is_attitude) {
            _e_prev = (km::UnitQuaternion(km::RotationVector(traj->getQ(t))) *
                       km::UnitQuaternion(km::RotationVector(-_sh->getQ())))
                          .toRotationVector()
                          .toVec3();
        } else {
            _e_prev = traj->getQ(t) - _sh->getQ().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)));
        }
    };

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

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

    bool PIDParams::isReady() const {
        bool flag = true;
        if (km::isNotReadyNaN(kp)) {
            kc::warn("Parameter kp is not ready.");
            flag = false;
        }
        if (km::isNotReadyNaN(kd)) {
            kc::warn("Parameter kd 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> PIDParams::_getVars() const {
        return PIDParamsVars::create(
            kc::static_pointer_cast<const PIDParams>(kc::ks_ptr<const Base>(shared_from_this())));
    };

    PIDParamsVars::PIDParamsVars(const kc::ks_ptr<const PIDParams> &model)
        : Karana::Core::BaseVars(model) {

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

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

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

    PIDParamsVars::~PIDParamsVars(){};

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

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

        return out;
    }

} // namespace Karana::Models