Program Listing for File PinJointLimits.cc

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

#include"Karana/GeneralKModels/PinJointLimits.h"
#include"Karana/KCore/Allocator.h"


namespaceKarana::Models{

namespacekd=Karana::Dynamics;

PinJointLimits::PinJointLimits(std::string_viewname,
constkc::ks_ptr<kd::ModelManager>&mm,
constkc::ks_ptr<kd::PinSubhinge>&pin)
:KModel<PinJointLimits,PinJointLimitsParams>(name,mm)
,_pin(pin){
params=std::allocate_shared<PinJointLimitsParams>(kc::Allocator<PinJointLimitsParams>{},
std::format("{}_params",name));
};

kc::ks_ptr<PinJointLimits>PinJointLimits::create(std::string_viewname,
constkc::ks_ptr<kd::ModelManager>&mm,
constkc::ks_ptr<kd::PinSubhinge>&pin){
kc::ks_ptr<PinJointLimits>pjl=
std::allocate_shared<PinJointLimits>(kc::Allocator<PinJointLimits>{},name,mm,pin);
mm->registerModel(pjl);
returnpjl;
}

voidPinJointLimits::preDeriv(constkm::Ktime&,constkm::Vec&){
doubleq=_pin->getQ()[0];
if(q>params->upper_limit){
km::VecT(1);
T[0]=(params->upper_limit-q)*params->k;
if(doubleu=_pin->getU()[0];u>0){
T[0]-=u*params->d;
}
_pin->setT(T);
}
if(q<params->lower_limit){
km::VecT(1);
T[0]=(params->lower_limit-q)*params->k;
if(doubleu=_pin->getU()[0];u<0){
T[0]-=u*params->d;
}
_pin->setT(T);
}
}

PinJointLimitsParams::PinJointLimitsParams(std::string_viewname)
:KModelParams(name){
upper_limit=km::notReadyNaN;
lower_limit=km::notReadyNaN;
k=km::notReadyNaN;
d=km::notReadyNaN;
}

boolPinJointLimitsParams::isReady()const{
boolflag=true;
if(km::isNotReadyNaN(k)){
kc::warn("Parameterkisnotready.");
flag=false;
}
if(km::isNotReadyNaN(d)){
kc::warn("Parameterdisnotready.");
flag=false;
}
if(km::isNotReadyNaN(upper_limit)){
kc::warn("Parameterupper_limitisnotready.");
flag=false;
}
if(km::isNotReadyNaN(lower_limit)){
kc::warn("Parameterlower_limitisnotready.");
flag=false;
}
returnflag;
}

//DestructorincludedforMacOSbuilds.Musthaveakey-functionout-of-linetoavoiddulpicate
//symbols.
PinJointLimits::~PinJointLimits(){};
}//namespaceKarana::Models