Program Listing for File ProjectConstraintError.cc

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

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


namespaceKarana::Models{

namespacekm=Karana::Math;
namespacekd=Karana::Dynamics;

ProjectConstraintError::ProjectConstraintError(
std::string_viewname,
constkc::ks_ptr<kd::ModelManager>&mm,
constkc::ks_ptr<kd::SubGraph>&sg,
constkc::ks_ptr<km::Integrator>&integrator,
constkc::ks_ptr<kd::ConstraintKinematicsSolver>&cks)
:KModel<ProjectConstraintError,ProjectConstraintErrorParams>(name,mm)
,_sg(sg)
,_integ(integrator)
,_cks(cks){
params=std::allocate_shared<ProjectConstraintErrorParams>(
kc::Allocator<ProjectConstraintErrorParams>{},std::format("{}_params",name));
};

kc::ks_ptr<ProjectConstraintError>
ProjectConstraintError::create(std::string_viewname,
constkc::ks_ptr<kd::ModelManager>&mm,
constkc::ks_ptr<kd::SubGraph>&sg,
constkc::ks_ptr<km::Integrator>&integrator,
constkc::ks_ptr<kd::ConstraintKinematicsSolver>&cks){
kc::ks_ptr<ProjectConstraintError>pcm=std::allocate_shared<ProjectConstraintError>(
kc::Allocator<ProjectConstraintError>{},name,mm,sg,integrator,cks);
mm->registerModel(pcm);
returnpcm;
}

voidProjectConstraintError::postModelStep(constkm::Ktime&/*t*/,constkm::Vec&/*x*/){
if(_sg->poseError().norm()>params->tol){
//usetheCKsolvertocorrecttheerrors
_cks->solveQ();
_cks->solveU();

//doasoftresetontheintegrtor
model_manager->setState(model_manager->assembleState());
}
}

ProjectConstraintErrorParams::ProjectConstraintErrorParams(std::string_viewname)
:KModelParams(name){}

boolProjectConstraintErrorParams::isReady()const{
boolflag=true;
if(km::isNotReadyNaN(tol)){
kc::warn("Parametertolisnotready.");
flag=false;
}
returnflag;
}
//DestructorincludedforMacOSbuilds.Musthaveakey-functionout-of-linetoavoiddulpicate
//symbols.
ProjectConstraintError::~ProjectConstraintError(){};
}//namespaceKarana::Models