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/Math/SpatialVector.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);
// register the nodes with the mulibody objecst struct
_multibody_objs.nodes.push_back(nd1);
_multibody_objs.nodes.push_back(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::SpatialForce(km::Vec3{0, 0, 0}, scratch->total_force),
_newton);
_nd2->accumExternalSpForce(km::SpatialForce(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);
}
SpringDamperParamsVars::SpringDamperParamsVars(
const kc::ks_ptr<const SpringDamperParams> ¶ms)
: Karana::Core::BaseVars(params) {
unsprung_length = Karana::Core::Var_T<double>::create(
"unsprung_length", [params] { return params->unsprung_length; }, "Unsprung length", "");
k = Karana::Core::Var_T<double>::create(
"k", [params] { return params->k; }, "Spring constant", "");
d = Karana::Core::Var_T<double>::create(
"d", [params] { return params->d; }, "Damper constant", "");
}
SpringDamperParamsVars::~SpringDamperParamsVars(){};
kc::ks_ptr<SpringDamperParamsVars>
SpringDamperParamsVars::create(const kc::ks_ptr<const SpringDamperParams> ¶ms) {
return std::allocate_shared<SpringDamperParamsVars>(kc::Allocator<SpringDamperParamsVars>{},
params);
}
kc::NestedVars SpringDamperParamsVars::getAllVars() const {
auto out = Karana::Core::BaseVars::getAllVars();
kc::addVarsToStruct(out, unsprung_length, k, d);
return out;
}
kc::ks_ptr<kc::BaseVars> SpringDamperParams::_getVars() const {
return SpringDamperParamsVars::create(kc::static_pointer_cast<const SpringDamperParams>(
kc::ks_ptr<const Base>(shared_from_this())));
};
SpringDamperScratchVars::SpringDamperScratchVars(
const kc::ks_ptr<const SpringDamperScratch> &scratch)
: Karana::Core::BaseVars(scratch) {
offset = Karana::Core::Var_T<km::Vec3>::create(
"offset", [scratch] { return scratch->offset; }, "Translation from node1 to node2", "");
position_error = Karana::Core::Var_T<double>::create(
"position_error",
[scratch] { return scratch->position_error; },
"Signed length offset from the unsprung length",
"");
velocity_error = Karana::Core::Var_T<km::Vec3>::create(
"velocity_error",
[scratch] { return scratch->velocity_error; },
"Relative linear velocity across the spring",
"");
stiffness_force = Karana::Core::Var_T<double>::create(
"stiffness_force",
[scratch] { return scratch->stiffness_force; },
"Signed scalar force due to spring stiffness",
"");
damping_force = Karana::Core::Var_T<km::Vec3>::create(
"damping_force",
[scratch] { return scratch->damping_force; },
"Linear force due to spring damping",
"");
total_force = Karana::Core::Var_T<km::Vec3>::create(
"total_force",
[scratch] { return scratch->total_force; },
"Total linear spring force",
"");
}
SpringDamperScratchVars::~SpringDamperScratchVars(){};
kc::ks_ptr<SpringDamperScratchVars>
SpringDamperScratchVars::create(const kc::ks_ptr<const SpringDamperScratch> &scratch) {
return std::allocate_shared<SpringDamperScratchVars>(
kc::Allocator<SpringDamperScratchVars>{}, scratch);
}
kc::NestedVars SpringDamperScratchVars::getAllVars() const {
auto out = Karana::Core::BaseVars::getAllVars();
kc::addVarsToStruct(out,
offset,
position_error,
velocity_error,
stiffness_force,
damping_force,
total_force);
return out;
}
kc::ks_ptr<kc::BaseVars> SpringDamperScratch::_getVars() const {
return SpringDamperScratchVars::create(kc::static_pointer_cast<const SpringDamperScratch>(
kc::ks_ptr<const Base>(shared_from_this())));
};
} // namespace Karana::Models