Program Listing for File StatePropagator.h

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

/*
*Copyright(c)2024-2025KaranaDynamicsPtyLtd.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;

classStatePropagator:publickc::LockingBase{

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

public:
structSpOptions{
km::Ktimemax_step_size=km::Ktime(0);

boolupdate_state_derivatives_hop_end=false;

//km::Integrator::IntegratorOptionsinteg_opts;

doublebaumgarte_stiffness=-1;
doublebaumgarte_damping=-1;
};

structSpFunctions{

friendclassStatePropagator;

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 &>();
};

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

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

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

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

~StatePropagator();

SpSolverTypesolverType()const{return_solver_type;}

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);

voidsetState(constkm::Vec&x);

km::VecassembleState()const;

km::VecassembleStateDeriv()const;

size_tnstates()const{return_nstates;}

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

constkm::Ktime&getMaxStepSize();

voidsetMaxStepSize(constkm::Ktime&max_step_size);

boolgetUpdateStateDerivativesHopEnd();

voidsetUpdateStateDerivativesHopEnd(boolupdate_state_derivatives_hop_end);

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

km::MatcomputeJacobian();

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

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

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

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

void
unregisterTimedEvent(std::string_viewname,boolbefore_hop,boolokay_not_exists=false);

boolhasRegisteredTimedEvent(std::string_viewname,boolbefore_hop)const;

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

constCounters&counters()const{return_counters;}

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

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

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

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

booltrace_state_propagator=false;

SpFunctionsfns;

voidhardReset();

protected:
Scheduler_scheduler;

SpStatusEnum_hopStep(constkm::Ktime&to_time);

SpStatusEnum_integrationStep(constkm::Ktime&to_time);

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

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

kc::ks_ptr<SubGraph>_sg=nullptr;

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

SpOptions_options;

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

Eigen::Index_nstates;

kc::ks_ptr<TimeKeeper>_time_keeper=nullptr;

km::Vec_curr_X;

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

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

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

void_stdTraceMsg(std::string_viewmsg);

protected:
/*forSubGraphs,thismethodwillverifythattheQandU
coordinatescurrentlyintheSubGraphsatisfytheconstraints,
i.e.theerrornormissmallerthanthespecifiedthreshold*/
void_checkConstraintErrors(doublethreshold)const;

private:
Eigen::Index_requiredStateSize()const;

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

Counters_counters;

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;

Eigen::Index_num_st_states;

SpSolverType_solver_type;
};

}//namespaceKarana::Dynamics