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::StatePropagator>&sp,
constkc::ks_ptr<kd::PinSubhinge>&pin)
:KModel<PinJointLimits,PinJointLimitsParams>(name,sp)
,_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::StatePropagator>&sp,
constkc::ks_ptr<kd::PinSubhinge>&pin){
kc::ks_ptr<PinJointLimits>pjl=
std::allocate_shared<PinJointLimits>(kc::Allocator<PinJointLimits>{},name,sp,pin);
sp->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::uninitializedNaN;
lower_limit=km::uninitializedNaN;
k=km::uninitializedNaN;
d=km::uninitializedNaN;
}

boolPinJointLimitsParams::isFinalized()const{
boolflag=true;
if(km::isUninitializedNaN(k)){
kc::warn("Parameterkisuninitialized.");
flag=false;
}
if(km::isUninitializedNaN(d)){
kc::warn("Parameterdisuninitialized.");
flag=false;
}
if(km::isUninitializedNaN(upper_limit)){
kc::warn("Parameterupper_limitisuninitialized.");
flag=false;
}
if(km::isUninitializedNaN(lower_limit)){
kc::warn("Parameterlower_limitisuninitialized.");
flag=false;
}
returnflag;
}

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