Program Listing for File StatePropagator.h

↰ Return to documentation for file (include/Karana/SOADyn/StatePropagator.h)

/*
*Copyright(c)2024-2026KaranaDynamicsPtyLtd.Allrightsreserved.
*
*NOTICETOUSER:
*
*Thissourcecodeand/ordocumentation(the"LicensedMaterials")is
*theconfidentialandproprietaryinformationofKaranaDynamicsInc.
*UseoftheseLicensedMaterialsisgovernedbythetermsandconditions
*ofaseparatesoftwarelicenseagreementbetweenKaranaDynamicsandthe
*Licensee("LicenseAgreement").Unlessexpresslypermittedunderthat
*agreement,anyreproduction,modification,distribution,ordisclosure
*oftheLicensedMaterials,inwholeorinpart,toanythirdparty
*withoutthepriorwrittenconsentofKaranaDynamicsisstrictlyprohibited.
*
*THELICENSEDMATERIALSAREPROVIDED"ASIS"WITHOUTWARRANTYOFANYKIND.
*KARANADYNAMICSDISCLAIMSALLWARRANTIES,EXPRESSORIMPLIED,INCLUDING
*BUTNOTLIMITEDTOWARRANTIESOFMERCHANTABILITY,NON-INFRINGEMENT,AND
*FITNESSFORAPARTICULARPURPOSE.
*
*INNOEVENTSHALLKARANADYNAMICSBELIABLEFORANYDAMAGESWHATSOEVER,
*INCLUDINGBUTNOTLIMITEDTOLOSSOFPROFITS,DATA,ORUSE,EVENIF
*ADVISEDOFTHEPOSSIBILITYOFSUCHDAMAGES,WHETHERINCONTRACT,TORT,
*OROTHERWISEARISINGOUTOFORINCONNECTIONWITHTHELICENSEDMATERIALS.
*
*U.S.GovernmentEndUsers:TheLicensedMaterialsarea"commercialitem"
*asdefinedat48C.F.R.2.101,andareprovidedtotheU.S.Government
*onlyasacommercialenditemunderthetermsofthislicense.
*
*AnyuseoftheLicensedMaterialsinindividualorcommercialsoftwaremust
*include,intheuserdocumentationandinternalsourcecodecomments,
*thisNotice,Disclaimer,andU.S.GovernmentUseProvision.
*/


#pragmaonce

#include"Karana/Integrators/Integrator.h"
#include"Karana/KCore/CallbackRegistry.h"
#include"Karana/SOADyn/BodyBase.h"
#include"Karana/SOADyn/Scheduler.h"
#include"Karana/SOADyn/TimeKeeper.h"

namespaceKarana::Models{
//Forwarddelcareclassestoavoidcirculardependencies
classBaseKModel;
template<classT,classP,classSc,classS,classC>classKModel;
}//namespaceKarana::Models

namespaceKarana::Dynamics{

namespacekc=Karana::Core;
namespacekmo=Karana::Models;

//Forwarddelcareclassestoavoidcirculardependencies
classSubGraph;

classModelManager:publickc::LockingBase{

//Foraccessto_curr_timeand_curr_Xwithoutcopying.
template<classT,classP,classSc,classS,classC>friendclasskmo::KModel;

public:
structMMFunctions{

friendclassModelManager;

kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>pre_deriv_fns=
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();

kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>post_deriv_fns=
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();

kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>pre_hop_fns=
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();

kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>post_hop_fns=
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();

std::function<bool(constkm::Ktime&,constkm::Vec&)>step_validate_fn=nullptr;

kc::CallbackRegistry<bool, const km::Vec &, bool>zero_crossing_fns=
kc::CallbackRegistry<bool, const km::Vec &, bool>();

kc::CallbackRegistry<bool, const km::Ktime &, const km::Vec &>
terminate_advance_to_fns=
kc::CallbackRegistry<bool, const km::Ktime &, const km::Vec &>();
};

enumclassMMSolverType{
TREE_DYNAMICS,
TREE_AUGMENTED_DYNAMICS,
BAUMGARTE_DYNAMICS,
KINEMATICS,
INVERSE_DYNAMICS,
UNDEFINED,
};

classMMOptions:publicstd::enable_shared_from_this<MMOptions>{
public:
doublebaumgarte_stiffness=-1;
doublebaumgarte_damping=-1;

MMOptions();

virtual~MMOptions();

statickc::ks_ptr<MMOptions>create();
};

public:
statickc::ks_ptr<ModelManager>create(constkc::ks_ptr<SubTree>&st,
kc::ks_ptr<MMOptions>mm_opts=nullptr,
MMSolverTypesolver_type=MMSolverType::UNDEFINED);

ModelManager(constkc::ks_ptr<SubTree>&st,
kc::ks_ptr<MMOptions>mm_opts=nullptr,
MMSolverTypesolver_type=MMSolverType::UNDEFINED);

~ModelManager();

MMSolverTypesolverType()const{return_solver_type;}

voidsetSolverType(MMSolverTypesolver_type);

virtualvoidsetState(constkm::Vec&x);

km::VecassembleState()const;

km::VecassembleStateDeriv()const;

size_tnstates()const{return_nstates;}

virtualvoidderivFunction(constkm::Ktime&t,constkm::Vec&x,Eigen::Ref<km::Vec>dx);

virtualvoidsetTime(constkm::Ktime&t);

constkm::Ktime&getTime()const{return_time_keeper->getTime();}

km::MatcomputeJacobian();

voidregisterModel(constkc::ks_ptr<kmo::BaseKModel>&model);

voidunregisterModel(constkc::ks_ptr<kmo::BaseKModel>&model);

conststd::vector<kc::ks_ptr<kmo::BaseKModel>>&getRegisteredModels()const;

kc::ks_ptr<kmo::BaseKModel>getRegisteredModel(std::string_viewname)const;

constkc::ks_ptr<SubTree>&getSubTree()const;

booltrace_state_propagator=false;

MMFunctionsfns;

virtualvoidhardReset();

constkc::ks_ptr<TimeKeeper>&getTimeKeeper()const;

std::stringdumpString(std::string_viewprefix="",
constBase::DumpOptions*options=nullptr)constoverride;

protected:
void_setStateWithModels(constkm::Vec&x,boolglobal=false);

void_discard(kc::ks_ptr<kc::Base>&base)override;

virtualvoid_hardSetState(constkm::Ktime&t,constkm::Vec&x);

void_stdTraceMsg(std::string_viewmsg);

Eigen::Index_requiredStateSize()const;

void_checkConstraintErrors(doublethreshold)const;

protected:
kc::ks_ptr<SubTree>_st=nullptr;

kc::ks_ptr<SubGraph>_sg=nullptr;

Eigen::Index_nstates;

km::Vec_curr_X;

kc::ks_ptr<TimeKeeper>_time_keeper=nullptr;

kc::RegistryList<kmo::BaseKModel>_registered_models;

std::vector<kc::ks_ptr<kmo::BaseKModel>>_continuous_models;

Eigen::Array<Eigen::Index,Eigen::Dynamic,1>_continuous_models_sizes;

kc::ks_ptr<MMOptions>_options=nullptr;

Eigen::Index_num_st_states;

virtualvoidregisterTimedEvent(constkc::ks_ptr<TimedEvent>&timed_event);//NOLINT

virtualvoidunregisterTimedEvent(std::string_viewname,//NOLINT
boolpre_hop,
boolokay_not_exists=false);

virtualboolhasRegisteredTimedEvent(std::string_viewname,boolpre_hop)const;//NOLINT

private:
MMSolverType_solver_type;
};

classStatePropagator:publicModelManager{

//Foraccessto_curr_timeand_curr_Xwithoutcopying.
//template<classT,classP,classSc,classS,classC>friendclasskmo::KModel;

public:
classSpOptions:publicModelManager::MMOptions{
public:
km::Ktimemax_step_size=km::Ktime(0);

boolupdate_state_derivatives_hop_end=false;

SpOptions();

//Addingsuppression,asCodeCheckercomplainsthismethodhas
//thesamenameasamethodinthebaseversion.Thisisjusta
//CodeCheckerfalsepositive.
//codechecker_suppress[cppcheck-duplInheritedMember]
statickc::ks_ptr<SpOptions>create();
};

enumclassSpStatusEnum{
UNKNOWN,
//REACHED_HOP_END_TIME,///<reachednormalterminationofthecurrenthop
REACHED_END_TIME,
REACHED_TERMINATION_CONDITION,
REACHED_ZERO_CROSSING,
FAILED_STEP_VALIDATION
};

public:
statickc::ks_ptr<StatePropagator>
create(constkc::ks_ptr<SubTree>&st,
constkm::IntegratorType&integrator_type,
km::Integrator::IntegratorOptions*integ_opts=nullptr,
kc::ks_ptr<SpOptions>sp_opts=nullptr,
MMSolverTypesolver_type=MMSolverType::UNDEFINED);

StatePropagator(constkc::ks_ptr<SubTree>&st,
constkm::IntegratorType&integrator_type,
km::Integrator::IntegratorOptions*integ_opts=nullptr,
kc::ks_ptr<SpOptions>sp_opts=nullptr,
MMSolverTypesolver_type=MMSolverType::UNDEFINED);

~StatePropagator();

SpStatusEnumadvanceTo(constkm::Ktime&to_time);

SpStatusEnumadvanceBy(constkm::Ktime&delta_time);

constkc::ks_ptr<km::Integrator>&getIntegrator()const{return_integrator;}

voidsetIntegrator(kc::ks_ptr<km::Integrator>integrator);

voidsetTime(constkm::Ktime&t)override;

voidsetState(constkm::Vec&x)override;

voidderivFunction(constkm::Ktime&t,constkm::Vec&x,Eigen::Ref<km::Vec>dx)override;

voidhardReset()override;

constkm::Ktime&getMaxStepSize();

voidsetMaxStepSize(constkm::Ktime&max_step_size);

boolgetUpdateStateDerivativesHopEnd();

voidsetUpdateStateDerivativesHopEnd(boolupdate_state_derivatives_hop_end);

std::stringdumpString(std::string_viewprefix="",
constBase::DumpOptions*options=nullptr)constoverride;

voidregisterTimedEvent(constkc::ks_ptr<TimedEvent>&timed_event)override;

voidunregisterTimedEvent(std::string_viewname,
boolpre_hop,
boolokay_not_exists=false)override;

boolhasRegisteredTimedEvent(std::string_viewname,boolpre_hop)constoverride;

structCounters{
size_thops=0;
size_tderivs=0;
size_tzero_crossings=0;
size_tintegration_steps=0;
};

constCounters&counters()const{return_counters;}

protected:
Scheduler_scheduler;

SpStatusEnum_hopStep(constkm::Ktime&to_time);

SpStatusEnum_integrationStep(constkm::Ktime&to_time);

void_discard(kc::ks_ptr<kc::Base>&base)override;

protected:
kc::ks_ptr<km::Integrator>_integrator=nullptr;

km::Ktime_tolerance=km::Ktime(100);

private:
void_integratorToSp(constkm::Ktime&time,boolsanitize);

void_hardSetState(constkm::Ktime&t,constkm::Vec&x)override;

private:
km::Ktime_zero_crossing_time_upper_bound=km::Ktime(0);

Counters_counters;
};

}//namespaceKarana::Dynamics