Program Listing for File StatePropagator.h#
↰ Return to documentation for file (include/Karana/SOADyn/StatePropagator.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains the declarations for the ModelManager and StatePropagator classes.
*/
#pragma once
#include "Karana/Integrators/Integrator.h"
#include "Karana/KCore/CallbackRegistry.h"
#include "Karana/SOADyn/BodyBase.h"
#include "Karana/SOADyn/Defs.h"
#include "Karana/SOADyn/Scheduler.h"
#include "Karana/SOADyn/TimeKeeper.h"
namespace Karana::Models {
// Forward delcare classes to avoid circular dependencies
class BaseKModel;
template <class T, class P, class Sc, class S, class C> class KModel;
} // namespace Karana::Models
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace kmo = Karana::Models;
// Forward delcare classes to avoid circular dependencies
class SubGraph;
/**
* @class ModelManager
* @brief Class to manage the multibody dynamics and KModels
*
* See \sref{system_level_models_sec} section for more information on
the ModelManager class.
*/
class ModelManager : public Karana::Core::LockingBase {
// For access to _curr_time and _curr_X without copying.
template <class T, class P, class Sc, class S, class C> friend class kmo::KModel;
public:
/** @brief Options for the model manager */
class MMOptions : public std::enable_shared_from_this<MMOptions> {
public:
/** Parameters for the Baumgarte method for loop constraint
stabilization for the error dynamics \f$\ddot e +
2\alpha \dot e + \beta^2 e = 0\f$. Baumgarte
stabilization is enabled in TA closed-chain dynamics
when the stiffness and damping coefficients are both
positive. */
double baumgarte_alpha = -1; ///< the Baumgarte damping coefficient
double baumgarte_beta = -1; ///< the Baumgarte stiffness coefficient
/**
* @brief MMOptions constructor. The constructor is not meant to be called
* directly. Please use the create(...) method instead to create an instance.
*/
MMOptions();
/**
* @brief Destructor.
*/
virtual ~MMOptions();
/**
* @brief Create an instance of MMOptions.
*
* @returns A pointer to the newly created MMOptions.
*/
static kc::ks_ptr<MMOptions> create();
};
/** @brief Struct with optional user-defined functions to be used by the
model manager */
struct MMFunctions {
friend class ModelManager;
/** Optional function called before evaluating system derivative. Useful for
setting gravity accel and other forces on the system. */
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &> pre_deriv_fns =
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();
/** Optional function called after evaluating system derivative. Useful for
passing state derivative values to models that needs them, and
to perform data logging. */
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &> post_deriv_fns =
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();
/** Optional function called at the start of a hop
step. Useful for propagating discrete states, sanitizing
the input state (e.g., for changing coordinate chart
origin), calling FSM step functions, processing external
models to apply force inputs from closed-loop
controllers etc. */
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &> pre_hop_fns =
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();
/** Optional function called at the end of a hop
step. Useful for processing external models that use the
new state to set sensor inputs for closed-loop
controllers, data logging, updating visualization,
cleaning up the state to null out constraint errors
etc. */
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &> post_hop_fns =
kc::CallbackRegistry<void, const km::Ktime &, const km::Vec &>();
/** Optional function called at the end of a integration
step to decide whether to accept the new state, or to
roll back to the state at the start of the hop. Useful
for repeating a the time step when the new state has
invalid values. */
std::function<bool(const km::Ktime &, const km::Vec &)> step_validate_fn = nullptr;
/** Optional function used for terminating a hop when a
zero-crossing condition has been met. Useful for getting
a simulation hop to terminate precisely when the
implicitly defined zero-crossing condition occurs instead
of continuing till the specified hop end time. See the
\sref{zero_crossings_sec} section for more discussion on zero
crossing detection.
The zero crossing functions return a bool. This should be true if the
zero has been crossed. The second argument to the function is a boolean.
This will be true when we have reached the zero, and false otherwise. */
kc::CallbackRegistry<bool, const km::Vec &, bool> zero_crossing_fns =
kc::CallbackRegistry<bool, const km::Vec &, bool>();
/** Optional function called to check whether to terminate the
state propagation prematurely before the state advancement
end time has been reached. Handy when the simulation has
already reached an end state (possibly for an FSM) and
there is no value in continuing on with the state
propagation. This check is only done at the end of a hop,
so there may be some timing slop in the check for an end
state. This is in contrast with the zero-crossing check
which is done within a hop to end the hop precisely when
the zero-crossing condition is met. */
kc::CallbackRegistry<bool, const km::Ktime &, const km::Vec &>
terminate_advance_to_fns =
kc::CallbackRegistry<bool, const km::Ktime &, const km::Vec &>();
};
public:
/**
* @brief Create an instance of the ModelManager.
*
* If the solver type is not specified, the constructor will use
* TREE_DYNAMICS if the specified st argument is a pure subtree
* (and not SubGraph), or if is the multibody instance itself
* with no bilateral constraints. For all other st, an error
* will be raised - and a legal solver type must be explicitly
* specified since there is no good way to infer one.
*
* @param st the SubTree instance
* @param mm_opts options for the model manager
* @param solver_type the solver type
* @return An instance of the ModelManager.
*/
static kc::ks_ptr<ModelManager> create(const kc::ks_ptr<SubTree> &st,
kc::ks_ptr<MMOptions> mm_opts = nullptr,
MMSolverType solver_type = MMSolverType::UNDEFINED);
/**
* @brief ModelManager constructor. The constructor is not meant to be called directly.
* Please use the create(...) method instead to create an instance.
*
* If the solver type is not specified, the constructor will use
* TREE_DYNAMICS if the specified st argument is a pure subtree
* (and not SubGraph), or if is the multibody instance itself
* with no bilateral constraints. For all other st, an error
* will be raised - and a legal solver type must be explicitly
* specified since there is no good way to infer one.
*
* @param st the SubTree instance
* @param mm_opts options for the model manager
* @param solver_type the solver type
*/
ModelManager(const kc::ks_ptr<SubTree> &st,
kc::ks_ptr<MMOptions> mm_opts = nullptr,
MMSolverType solver_type = MMSolverType::UNDEFINED);
/** @brief Destructor */
~ModelManager();
/**
* @brief Return the solver type for the propagator
*
* @return the solver type
*/
MMSolverType solverType() const { return _solver_type; }
/**
* @brief Set the solver type for the propagator
*
* @param solver_type the new solver type
*/
void setSolverType(MMSolverType solver_type);
/**
* @brief Reset the integrator to the new state values
*
* This is also referred to as a *soft reset*. This method
* assumes that only the state values have changed, and there
* have been no configuration changes causing changes to the
* size or structure of the state vector. The input state vector
* elements for subhinges should contain *global* coordinate
* values. This method resets the chart offsets for all regular
* and constraint subhinges for the sub graph.
*
* The state itself is often created using the assembleState method.
* @see assembleState
*
* @param x the new state value
*/
virtual void setState(const km::Vec &x);
/**
* @brief Combine the current associated SubTree state and continuous KModel states
* into one vector.
*
* This state vector reflects the current state of the Multibody and KModels' continuous
* states, which may be different than the state vector in the integrator.
*
* The packing order is the same as the integrator. This can be
* used to create an initial state for the integrator as
* follows:
* state_propagator->setState(state_propagator->assembleState()).
*
* @returns A state vector representing the Multibody and KModels' continuous states of this
* ModelManager.
*/
km::Vec assembleState() const;
/**
* @brief Combine the current associated SubTree state derivative and continuous KModel
* state derivatives into one vector.
*
* This state derivative vector reflects the current state of the Multibody and KModels'
* continuous states' derivatives, which may be different than the state vector in the
* integrator.
*
* The packing order is the same as the integrator. *
*
* @returns A state vector derivative representing the Multibody and KModels' continuous
* states' derivatives of this ModelManager.
*/
km::Vec assembleStateDeriv() const;
/**
* @brief Return the state vector size
* @return the state vector size
*/
size_t nstates() const { return _nstates; }
/**
* @brief The derivative function for use by the integrator
*
* This method is used to compute the state derivative by the
* numerical integrator for propagating system state.
*
* @param t the current time
* @param x the current state
* @param dx the computed state derivative
*/
virtual void derivFunction(const km::Ktime &t, const km::Vec &x, Eigen::Ref<km::Vec> dx);
/**
* @brief Reset the integrator to the new time value
*
* This is also referred to as a *soft reset*. If the time set
* is different than the current time, this will attempt to
* rescheduled all TimedEvents appropriately; however, since
* TimedEvents can have arbitrary reschedule_fns, it is
* impossible to meet all corner cases. Use this with caution
* when you have non-uniform TimedEvents. See
* \sref{system_state_sec} section for more on this topic.
*
* @param t the new time
*/
virtual void setTime(const km::Ktime &t);
/**
* @brief Return the current state propagation time value
* @return the Ktime time value
*/
const km::Ktime &getTime() const { return _time_keeper->getTime(); }
/**
* @brief Use numerical differentiation and the derivative function to compute the Jacobian
* (experimental)
*
* @return the Jacobian matrix
*/
km::Mat computeJacobian();
/**
* @brief Register the provided BaseKModel model instance with the model manager.
*
* See \sref{kmodels_sec} section for more discussion on component
* models and the \sref{model_register_sec} section on registering
* models.
*
* Registering a model is a way to activate a model so that it
* is used in the physics computations. See \sref{kmodels_sec}
* section for more discussion on component models.
*
* @param model - The model to register.
*/
void registerModel(const kc::ks_ptr<kmo::BaseKModel> &model);
/**
* @brief Unregister the provided BaseKModel model from the model manager.
*
* Unregistering a model is a way to deactivate a model so that
* it is no longer used in the physics computations. See the
* \sref{kmodels_sec} section for more discussion on component models
* and the \sref{model_register_sec} section on registering
* models.
*
* @param model - The model to unregister.
*/
void unregisterModel(const kc::ks_ptr<kmo::BaseKModel> &model);
/**
* @brief Return a vector or registered models.
*
* @return A vector of registered models.
*/
const std::vector<kc::ks_ptr<kmo::BaseKModel>> &getRegisteredModels() const;
/**
* @brief Return the registered model with the specified name
*
* @param name the name of the model being sought
* @return The registered model or null if there is no model with matching name
*/
kc::ks_ptr<kmo::BaseKModel> getRegisteredModel(std::string_view name) const;
/**
* @brief Return the SubTree used by this ModelManager.
*
* @return The SubTree used by this ModelManager.
*/
const kc::ks_ptr<SubTree> &getSubTree() const;
/// Enable/disable model manager debug message.
bool trace_state_propagator = false;
/** User defined functions to tailor the behavior of the state
propagator */
MMFunctions fns;
/**
* @brief Reset the size of the internal state vector to match the size of the SubTree state
* + the size of all the continuous model states.
*
* This method also makeNotReadys the state, so setState must be called afterwards.
*/
virtual void hardReset();
/**
* @brief Return the TimeKeeper used by this ModelManager.
*
* @return The TimeKeeper used by this ModelManager.
*/
const kc::ks_ptr<TimeKeeper> &getTimeKeeper() const;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
/**
* @brief Pushes the combined state vector values into the SubTree state and continuous
* KModel states
*
* The packing order is the same as the integrator. This method
* is the converse of the assembleState() method.
*
* @param x the state vector
* @param global if true, the state coord values are for the global charts
*/
void _setStateWithModels(const km::Vec &x, bool global = false);
/**
* @brief Discard the ModelManager.
*
* @param base A base pointer to the ModelManager to discard.
*/
void _discard(kc::ks_ptr<kc::Base> &base) override;
/**
* @brief Resize and reset the new time and state values.
*
* This is also referred to as a *hard reset*. This is like a *soft reset*, except
* it also allows the state vector size to change. The input state vector
* elements for subhinges should contain *local* chart
* coordinate values (in contrast with setState() which works
* with global coordinate values). The packing order should reflect the packing order
* of the ModelManager. See \sref{system_state_sec}
* section for more on this topic.
*
* @param t the new time
* @param x the new state value
*/
virtual void _hardSetState(const km::Ktime &t, const km::Vec &x);
/**
* @brief Log a standard trace message
*
* @param msg The message to log.
*/
void _stdTraceMsg(std::string_view msg);
/**
* @brief Return the size that the ModelManager _curr_X should be.
*
* This is the size of the SubTree's states plus the size of the continuous model states.
* @return the size value
*/
Eigen::Index _requiredStateSize() const;
/** @brief For SubGraphs, this method will verify that the Q and U
coordinates currently in the SubGraph satisfy the constraints,
i.e. the error norm is smaller than the specified threshold
@param threshold the threshold to use
*/
void _checkConstraintErrors(double threshold) const;
protected:
/** the sub-tree */
kc::ks_ptr<SubTree> _st = nullptr;
/** dynamic cast the subtree to a SubGraph. Non-null only if the
subtree is in fact a SubGraph */
kc::ks_ptr<SubGraph> _sg = nullptr;
/** the size of the state vector */
Eigen::Index _nstates;
/** the current state value. Normally identical to the value in
the integrator, but we have it here as well for cases where we
do not have an integrator. This value is updated at the end of
each integration sub-step and hop. */
km::Vec _curr_X;
/// The TimeKeeper keeps track of the time.
kc::ks_ptr<TimeKeeper> _time_keeper = nullptr;
/** List of registered models.*/
kc::RegistryList<kmo::BaseKModel> _registered_models;
/** List of models with continuous states.*/
std::vector<kc::ks_ptr<kmo::BaseKModel>> _continuous_models;
/** Map with size of the continuous states for the models with continuous states.*/
Eigen::Array<Eigen::Index, Eigen::Dynamic, 1> _continuous_models_sizes;
/** the model manager options */
kc::ks_ptr<MMOptions> _options = nullptr;
/** The number of states in the SubTree. */
Eigen::Index _num_st_states;
/**
* @brief Register a TimedEvent.
* This is a no-op. Only used so we can call registerTimedEvent for KModels.
* @param timed_event The TimedEvent to register.
*/
virtual void registerTimedEvent(const kc::ks_ptr<TimedEvent> &timed_event); // NOLINT
/**
* @brief Unregister a TimedEvent.
*
* This is a no-op. Only used so we can call registerTimedEvent for KModels.
*
* @param name - The name of the timed event to remove.
* @param pre_hop - Whether the event to remove is a pre_hop or post_hop timed event.
* @param okay_not_exists - If true, then do not error out if a timed event by this name
* does not exist with the given pre_hop setting, otherwise, error our if the given event
* is not found.
*/
virtual void unregisterTimedEvent(std::string_view name, // NOLINT
bool pre_hop,
bool okay_not_exists = false);
/**
* @brief Determine whether a TimedEvent with the given name exists.
*
* This is a no-op. Only used so we can call registerTimedEvent for KModels.
*
* @param name - The name of the TimedEvent.
* @param pre_hop - Whether this is a pre_hop timed event or not.
* @returns true if a TimedEvent with the given name and pre_hop setting exists, false
* otherwise.
*/
virtual bool hasRegisteredTimedEvent(std::string_view name, bool pre_hop) const; // NOLINT
private:
/** the type of solver to use for state propagation */
MMSolverType _solver_type;
};
/**
* @class StatePropagator
* @brief Class to integrate and propagate system state
*
* See \sref{time_domain_sim_sec} section for more information on
the StatePropagator class.
*/
class StatePropagator : public ModelManager {
// For access to _curr_time and _curr_X without copying.
// template <class T, class P, class Sc, class S, class C> friend class kmo::KModel;
public:
/** @brief Options for the state propagator */
class SpOptions : public ModelManager::MMOptions {
public:
/** Define the step size that advanceTo() should use to break up the
overall time advancement into sub-steps. This is especially
useful for fixed-step integrators to ensure that integrator
steps are not too large. Also for cases where external
interactions and data logging needs to be done at fixed step
intervals. */
km::Ktime max_step_size = km::Ktime(0);
/// If true, then update the multibody state derivatives with the integrator states at
/// the end of each hop.
bool update_state_derivatives_hop_end = false;
/**
* @brief SpOptions constructor. The constructor is not meant to be called
* directly. Please use the create(...) method instead to create an instance.
*/
SpOptions();
/**
* @brief Create an instance of SpOptions.
*
* @returns A pointer to the newly created SpOptions.
*/
// Adding suppression, as CodeChecker complains this method has
// the same name as a method in the base version. This is just a
// CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
static kc::ks_ptr<SpOptions> create();
};
public:
/**
* @brief Create an instance of the StatePropagator.
*
* If the solver type is not specified, the constructor will use
* TREE_DYNAMICS if the specified st argument is a pure subtree
* (and not SubGraph), or if is the multibody instance itself
* with no bilateral constraints. For all other st, an error
* will be raised - and a legal solver type must be explicitly
* specified since there is no good way to infer one.
*
* @param st the SubTree instance
* @param integrator_type the numerical integrator type
* @param integ_opts options for the numerical integrator
* @param sp_opts options for the state propagator
* @param solver_type the solver type
* @return An instance of the StatePropagator.
*/
static kc::ks_ptr<StatePropagator>
create(const kc::ks_ptr<SubTree> &st,
const km::IntegratorType &integrator_type,
km::Integrator::IntegratorOptions *integ_opts = nullptr,
kc::ks_ptr<SpOptions> sp_opts = nullptr,
MMSolverType solver_type = MMSolverType::UNDEFINED);
/**
* @brief StatePropagator constructor. The constructor is not meant to be called directly.
* Please use the create(...) method instead to create an instance.
*
* If the solver type is not specified, the constructor will use
* TREE_DYNAMICS if the specified st argument is a pure subtree
* (and not SubGraph), or if is the multibody instance itself
* with no bilateral constraints. For all other st, an error
* will be raised - and a legal solver type must be explicitly
* specified since there is no good way to infer one.
*
* @param st the SubTree instance
* @param integrator_type the numerical integrator type
* @param integ_opts options for the numerical integrator
* @param sp_opts options for the state propagator
* @param solver_type the solver type
*/
StatePropagator(const kc::ks_ptr<SubTree> &st,
const km::IntegratorType &integrator_type,
km::Integrator::IntegratorOptions *integ_opts = nullptr,
kc::ks_ptr<SpOptions> sp_opts = nullptr,
MMSolverType solver_type = MMSolverType::UNDEFINED);
/** @brief Destructor */
~StatePropagator();
/**
* @brief Method to advance the system state to a new time in the future
*
* @param to_time the desired future time
* @return advancement status value
*/
SpStatusEnum advanceTo(const km::Ktime &to_time);
/**
* @brief Method to advance the system state by a time interval
*
* @param delta_time the time to advance by
* @return advancement status value
*/
SpStatusEnum advanceBy(const km::Ktime &delta_time);
/**
* @brief Return the integrator instance
*
* See \sref{integrator_sec} section for more information on
* numerical integrators.
*
* @return the Integrator instance
*/
const kc::ks_ptr<km::Integrator> &getIntegrator() const { return _integrator; }
/**
* @brief Change to a new integrator using the provided type and options.
*
* If there was a previous integrator, the time and state values
* are transferred as well.
*
* See \sref{integrator_sec} section for more information on
* numerical integrators.
*
* @param integrator_type The new integrator type to use.
* @param integ_opts The options to us for the new integrator.
*/
void setIntegrator(const km::IntegratorType &integrator_type,
km::Integrator::IntegratorOptions *integ_opts);
/**
* @brief Change to the specified integrator.
*
* If there was a previous integrator, the time and state values
* are transferred as well.
*
* See \sref{integrator_sec} section for more information on
* numerical integrators.
*
* @param integrator the new integrator
*/
void setIntegrator(const kc::ks_ptr<km::Integrator> &integrator);
void setTime(const km::Ktime &t) override;
/**
* @brief Reset the integrator to the new state values
*
* This is also referred to as a *soft reset*. This method
* assumes that only the state values have changed, and there
* have been no configuration changes causing changes to the
* size or structure of the state vector. The input state vector
* elements for subhinges should contain *global* coordinate
* values. This method resets the chart offsets for all regular
* and constraint subhinges for the sub graph.
*
* The state itself is often created using the assembleState method.
* @see assembleState
*
* @param x the new state value
*/
void setState(const km::Vec &x) override;
void derivFunction(const km::Ktime &t, const km::Vec &x, Eigen::Ref<km::Vec> dx) override;
void hardReset() override;
/**
* @brief Get the step size that advanceTo() is using to break up the
* overall time advancement into sub-steps.
*
* This is especially useful for fixed-step integrators to ensure that integrator
* steps are not too large. Also for cases where external
* interactions and data logging needs to be done at fixed step
* intervals.
*
* @returns The maximum step size that advanceTo will use.
*/
const km::Ktime &getMaxStepSize();
/**
* @brief Set the step size that advanceTo() will use to break up the
* overall time advancement into sub-steps.
*
* See getMaxStepSize for more details.
*
* @param max_step_size The maximum step size that advanceTo will use.
*/
void setMaxStepSize(const km::Ktime &max_step_size);
/**
* @brief Get the setting for updating the derivatives at hop end.
*
* If true, then update the multibody state derivatives with the
* integrator states at the end of each hop.
*
* @return The the setting for updating the derivatives at hop end.
*/
bool getUpdateStateDerivativesHopEnd();
/**
* @brief Set whether to update the derivatives at hop end or not.
*
* If true, then update the multibody state derivatives with the
* integrator states at the end of each hop.
*
* @param update_state_derivatives_hop_end The setting for updating the derivatives at hop
* end.
*/
void setUpdateStateDerivativesHopEnd(bool update_state_derivatives_hop_end);
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/**
* @brief Register a TimedEvent.
* See \sref{timed_events_sec} section for more on using the TimedEvent class.
* @param timed_event The TimedEvent to register.
*/
void registerTimedEvent(const kc::ks_ptr<TimedEvent> &timed_event) override;
/**
* @brief Unregister a TimedEvent.
* See \sref{timed_events_sec} section for more on using the TimedEvent class.
* @param name - The name of the timed event to remove.
* @param pre_hop - Whether the event to remove is a pre_hop or post_hop timed event.
* @param okay_not_exists - If true, then do not error out if a timed event by this name
* does not exist with the given pre_hop setting, otherwise, error our if the given event
* is not found.
*/
void unregisterTimedEvent(std::string_view name,
bool pre_hop,
bool okay_not_exists = false) override;
/**
* @brief Determine whether a TimedEvent with the given name exists.
*
* @param name - The name of the TimedEvent.
* @param pre_hop - Whether this is a pre_hop timed event or not.
* @returns true if a TimedEvent with the given name and pre_hop setting exists, false
* otherwise.
*/
bool hasRegisteredTimedEvent(std::string_view name, bool pre_hop) const override;
/** @brief Struct to track state propagation statistics */
struct Counters {
size_t hops = 0; //!< the number of hops taken
size_t derivs = 0; //!< the number of deriv calls
size_t zero_crossings = 0; //!< the number of zero crossings detected
size_t integration_steps = 0; //!< the number of integration steps taken
};
/**
* @brief Return the Counters struct that tracks propagation statistics
*
* @return the Counters counters struct
*/
const Counters &counters() const { return _counters; }
protected:
/// Access to the scheduler for the state propagator.
Scheduler _scheduler;
/** @brief Helper method to execute a hop sub-step.
@param to_time the to time
@return the status value
*/
SpStatusEnum _hopStep(const km::Ktime &to_time);
/**
* @brief Helper method to execute a numerical integration sub-step.
*
* This will also sanitize the coordinates at the end of the integration sub-step.
*
* @param to_time The time to integrate to.
* @return The status of the integration sub step.
*/
SpStatusEnum _integrationStep(const km::Ktime &to_time);
/**
* @brief Discard the StatePropagator.
*
* @param base A base pointer to the StatePropagator to discard.
*/
void _discard(kc::ks_ptr<kc::Base> &base) override;
protected:
/** the integrator */
kc::ks_ptr<km::Integrator> _integrator = nullptr;
/** the time tolerance to use to conclude that we are close enough
to the zero-crossing condition, and do not need to get any
closer. */
km::Ktime _tolerance = km::Ktime(100);
private:
/**
* @brief Take what is in the integrator and push it into the StatePropagator (Multibody and
* KModels' continuous states).
*
* @param time The time to
* @param sanitize If true, then check and sanitize the coordinates of the multibody. If
* false, then do not sanitize. If sanitization is done, the integrator state will be
* updated as well.
*/
void _integratorToSp(const km::Ktime &time, bool sanitize);
void _hardSetState(const km::Ktime &t, const km::Vec &x) override;
private:
/** variable used to track the upper bound on the zero-crossing
time when in the midst of zero-crossing iterations */
km::Ktime _zero_crossing_time_upper_bound = km::Ktime(0);
/** Counters for various state propagation events */
Counters _counters;
};
} // namespace Karana::Dynamics