Program Listing for File SpringDamper.cc

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

#include "Karana/GeneralKModels/SpringDamper.h"
#include "Karana/KCore/Allocator.h"
#include "Karana/SOADyn/Multibody.h"

/**
 * @file
 * @brief SpringDamper implementation.
 */

namespace Karana::Models {

    namespace kd = Karana::Dynamics;

    SpringDamper::SpringDamper(std::string_view name,
                               const kc::ks_ptr<kd::ModelManager> &mm,
                               const kc::ks_ptr<kd::Node> nd1,
                               const kc::ks_ptr<kd::Node> nd2)
        : KModel<SpringDamper, SpringDamperParams, SpringDamperScratch>(name, mm)
        , _nd1(nd1)
        , _nd2(nd2) {
        params = std::allocate_shared<SpringDamperParams>(kc::Allocator<SpringDamperParams>{},
                                                          std::format("{}_params", name));
        scratch = std::allocate_shared<SpringDamperScratch>(kc::Allocator<SpringDamperScratch>{},
                                                            std::format("{}_scratch", name));

        if (not _nd1->isExternalForceNode()) {
            throw std::invalid_argument("Node1 is not a force node.");
        }
        if (not _nd2->isExternalForceNode()) {
            throw std::invalid_argument("Node2 is not a force node.");
        }

        _newton = _nd1->parentBody()->multibody()->getNewtonianFrame();
        _f2f1 = _newton->frameToFrame(_nd1);
        _f2f2 = _newton->frameToFrame(_nd2);
    };

    kc::ks_ptr<SpringDamper> SpringDamper::create(std::string_view name,
                                                  const kc::ks_ptr<kd::ModelManager> &mm,
                                                  const kc::ks_ptr<kd::Node> nd1,
                                                  const kc::ks_ptr<kd::Node> nd2) {
        kc::ks_ptr<SpringDamper> sd =
            std::allocate_shared<SpringDamper>(kc::Allocator<SpringDamper>{}, name, mm, nd1, nd2);
        mm->registerModel(sd);
        return sd;
    }

    void SpringDamper::preDeriv(const km::Ktime &, const km::Vec &) {
        // The vector from node1 to node2 as measured in the newtonian frame
        scratch->offset =
            _f2f2->relTransform().getTranslation() - _f2f1->relTransform().getTranslation();

        // The velocity of node2 relative to node1 as expressed in the inertial frame.
        scratch->velocity_error = _f2f2->relSpVel().getv() - _f2f1->relSpVel().getv();

        // The current length between the nodes
        double l = scratch->offset.norm();
        km::Vec3 dir = scratch->offset / l;

        // Signed scalar length error
        scratch->position_error = l - params->unsprung_length;

        // Force on node1 toward node2 due to spring stiffness, as expressed in
        // the inertial frame.
        scratch->stiffness_force = params->k * scratch->position_error;

        /* Force on node1 due to samping as expressed in the inertial
           frame. Based on using the axial/radial velocity component of
           the relative velocity:
           https://nexus.hexagon.com/documentationcenter/tr-TR/bundle/Adams_2022.4_Adams_View_Function_Builder_User_Guide/resource/Adams_2022.4_Adams_View_Function_Builder_User_Guide.pdf
         */
        // scratch->damping_force = params->d * scratch->velocity_error;
        scratch->damping_force = (params->d * scratch->velocity_error.dot(dir)) * dir;

        // Force on node1 due to the spring as expressed in the inertial frame.
        scratch->total_force = scratch->stiffness_force * dir + scratch->damping_force;
        // km::Vec3 f = (l - params->unsprung_length) * params->k * t / l - params->d * v;

        // Apply the equal and opposing forces to the nodes
        _nd1->accumExternalSpForce(km::SpatialVector(km::Vec3{0, 0, 0}, scratch->total_force),
                                   _newton);
        _nd2->accumExternalSpForce(km::SpatialVector(km::Vec3{0, 0, 0}, -scratch->total_force),
                                   _newton);
    }

    const kc::ks_ptr<kd::Node> &SpringDamper::sourceNode() const { return _nd1; }

    const kc::ks_ptr<kd::Node> &SpringDamper::targetNode() const { return _nd2; }

    SpringDamperParams::SpringDamperParams(std::string_view name)
        : KModelParams(name) {
        unsprung_length = km::notReadyNaN;
        k = km::notReadyNaN;
        d = km::notReadyNaN;
    }

    bool SpringDamperParams::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(unsprung_length)) {
            kc::warn("Parameter unsprung_length is not ready.");
            flag = false;
        }
        return flag;
    }

    std::string
    SpringDamperParams::dumpString(std::string_view prefix,
                                   const Karana::Core::Base::DumpOptions * /*options*/) const {
        // std::string result;
        // std::cout << "JJJJJ<" << prefix << ">KKK\n";
        auto result =
            std::format("{} k={}, d={}, unsprung_length={}\n", prefix, k, d, unsprung_length);
        return result;
    }

    std::string
    SpringDamperScratch::dumpString(std::string_view prefix,
                                    const Karana::Core::Base::DumpOptions * /*options*/) const {
        // std::string result;
        // std::cout << "JJJJJ<" << prefix << ">KKK\n";
        auto result = std::format("{} offset:{}\n", prefix, km::dumpString(offset));
        result += std::format("{} position_error:{}\n", prefix, position_error);

        result += std::format("{} velocity_error:{}\n", prefix, km::dumpString(velocity_error));
        result += std::format("{} stiffness_force:{}\n", prefix, stiffness_force);
        result += std::format("{} damping_force:{}\n", prefix, km::dumpString(damping_force));
        result += std::format("{} total_force:{}\n", prefix, km::dumpString(total_force));
        return result;
    }

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

    SpringDamperScratch::SpringDamperScratch(std::string_view name)
        : KModelScratch(name) {
        km::makeNotReady(offset);
        km::makeNotReady(velocity_error);
        km::makeNotReady(damping_force);
        km::makeNotReady(total_force);
    }

} // namespace Karana::Models