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::StatePropagator>&sp,
constkc::ks_ptr<kd::SubGraph>&sg,
constkc::ks_ptr<km::Integrator>&integrator,
constkc::ks_ptr<kd::ConstraintKinematicsSolver>&cks)
:KModel<ProjectConstraintError,ProjectConstraintErrorParams>(name,sp)
,_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::StatePropagator>&sp,
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,sp,sg,integrator,cks);
sp->registerModel(pcm);
returnpcm;
}

voidProjectConstraintError::postModelStep(constkm::Ktime&t,constkm::Vec&x){
if(_sg->poseError().norm()>params->tol){
//setthestateinthembody
_sg->stateToDynamics(x);

//usetheCKsolvertocorrecttheerrors
_cks->solveQ();
_cks->solveU();

//doasoftresetontheintegrtor
autonew_x=_sg->dynamicsToState();
_integ->softReset(t,new_x);
}
}

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

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