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"

/**
 * @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::PhysicalSubhinge> &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};
    };

    kc::ks_ptr<PID> PID::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<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("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)));
        }
    };

    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;
    }

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