Class StatePropagator

Class StatePropagator#

Nested Relationships#

Nested Types#

Inheritance Relationships#

Base Type#

Class Documentation#

class StatePropagator : public Karana::Core::LockingBase#

Class to integrate and propagate system state.

See System level models section for more information on the StatePropagator class.

Public Types

enum class SpStatusEnum#

Enum with values to classify the reason for the termination of a step.

Values:

enumerator UNKNOWN#

unknown

enumerator REACHED_END_TIME#

normal termination, reached requested end time

enumerator REACHED_TERMINATION_CONDITION#

premature termination since the termination condition was met

enumerator REACHED_ZERO_CROSSING#

premature end on reaching zero-crossing condition

enumerator FAILED_STEP_VALIDATION#

rolled back due to failing step validation check

enum class SpSolverType#

Enum with values to classify the reason for the termination of a step.

Values:

enumerator TREE_DYNAMICS#

tree ATBI forward dynamics for a tree system

enumerator TREE_AUGMENTED_DYNAMICS#

tree-augmented ATBI constrained forward dynamics

enumerator BAUMGARTE_DYNAMICS#

Baumgarte constrained forward dynamics.

enumerator KINEMATICS#

kinematics simulation mode

enumerator UNDEFINED#

undefined solver type

Public Functions

StatePropagator(const kc::ks_ptr<SubTree> &st, const km::IntegratorType &integrator_type, km::Integrator::IntegratorOptions *integ_opts = nullptr, SpOptions *sp_opts = nullptr, SpSolverType solver_type = SpSolverType::UNDEFINED)#

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 explcitly specified since there is no good way to infer one.

Parameters:

  • st – the SubTree instance

  • integrator_type – the numerical integrator type

  • integ_opts – options for the numerical integrator

  • sp_opts – options for the state propagator

  • solver_type – the solver type

~StatePropagator()#

Destructor.

inline SpSolverType solverType() const#

Return the solver type for the propgator.

Returns:

the solver type

SpStatusEnum advanceTo(const km::Ktime &to_time)#

Method to advance the system state to a new time in the future.

Parameters:

to_time – the desired future time

Returns:

advancement status value

SpStatusEnum advanceBy(const km::Ktime &delta_time)#

Method to advance the system state by a time interval.

Parameters:

delta_time – the time to advance by

Returns:

advancement status value

inline const kc::ks_ptr<km::Integrator> &getIntegrator() const#

Return the integrator instance.

See Numerical integration section for more information on numerical integrators.

Returns:

the Integrator instance

void setIntegrator(kc::ks_ptr<km::Integrator> integrator)#

Change to the specified integrator.

If there was a previous integrator, the time and state values are transferred as well.

See Numerical integration section for more information on numerical integrators.

Parameters:

integrator – the new integrator

void setTime(const km::Ktime &t)#

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

System state section for more on this topic.

Parameters:

t – the new time

void setState(const km::Vec &x)#

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 also

assembleState

Parameters:

x – the new state value

km::Vec assembleState() const#

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_propgator->setState(state_propagator->assembleStaate()).

Returns:

A state vector representing the Multibody and KModels’ continuous states of this StatePropagator.

km::Vec assembleStateDeriv() const#

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 StatePropagator.

inline size_t nstates() const#

Return the state vector size.

Returns:

the state vector size

inline const km::Ktime &getTime() const#

Return the current state propagation time value.

Returns:

the Ktime time value

const km::Ktime &getMaxStepSize()#

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.

void setMaxStepSize(const km::Ktime &max_step_size)#

Set the step size that advanceTo() will use to break up the overall time advancement into sub-steps.

See getMaxStepSize for more details.

Parameters:

max_step_size – The maximum step size that advanceTo will use.

bool getUpdateStateDerivativesHopEnd()#

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.

Returns:

The the setting for updating the derivatives at hop end.

void setUpdateStateDerivativesHopEnd(bool update_state_derivatives_hop_end)#

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.

Parameters:

update_state_derivatives_hop_end – The setting for updating the derivatives at hop end.

void derivFunction(const km::Ktime &t, const km::Vec &x, Eigen::Ref<km::Vec> dx)#

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.

Parameters:

  • t – the current time

  • x – the current state

  • dx – the computed state derivative

km::Mat computeJacobian()#

Use numerical differentiation and the derivative function to compute the Jacobian (experimental)

Returns:

the Jacobian matrix

void registerModel(const kc::ks_ptr<kmo::BaseKModel> &model)#

Register the provided BaseKModel model instance with the state propagator.

See KModel component models section for more discussion on component models and the Registering and unregistering models section on registering models.

Registering a model is a way to activate a model so that it is used in the physics computations. See KModel component models section for more discussion on component models.

Parameters:

model – - The model to register.

void unregisterModel(const kc::ks_ptr<kmo::BaseKModel> &model)#

Unregister the provided BaseKModel model from the state propagator.

Unregistering a model is a way to deactivate a model so that it is no longer used in the physics computations. See the

KModel component models section for more discussion on component models and the Registering and unregistering models section on registering models.

Parameters:

model – - The model to unregister.

std::string dumpString(std::string_view prefix = "", const Base::DumpOptions *options = nullptr) const override#
void registerTimedEvent(const kc::ks_ptr<TimedEvent> &timed_event)#

Register a TimedEvent. See.

Timed events section for more on using the TimedEvent class.

Parameters:

timed_event – The TimedEvent to register.

void unregisterTimedEvent(std::string_view name, bool before_hop, bool okay_not_exists = false)#

Unregister a TimedEvent. See.

Timed events section for more on using the TimedEvent class.

Parameters:

  • name – - The name of the timed event to remove.

  • before_hop – - Whether the event to remove is a before_hop or after_hop timed event.

  • okay_not_exists – - If true, then do not error out if a timed event by this name does not exist with the given before_hop setting, otherwise, error our if the given event is not found.

bool hasRegisteredTimedEvent(std::string_view name, bool before_hop) const#

Determine whether a TimedEvent with the given name exists.

Parameters:

  • name – - The name of the TimedEvent.

  • before_hop – - Whether this is a before_hop timed event or not.

Returns:

true if a TimedEvent with the given name and before_hop setting exists, false otherwise.

inline const Counters &counters() const#

Return the Counters struct that tracks propagation statistics.

Returns:

the Counters counters struct

const std::vector<kc::ks_ptr<kmo::BaseKModel>> &getRegisteredModels() const#

Return a vector or registered models.

Returns:

A vector of registered models.

kc::ks_ptr<kmo::BaseKModel> getRegisteredModel(std::string_view name) const#

Return the registered model with the specified name.

Parameters:

name – the name of the model being sought

Returns:

The registered model or null if there is no model with matching name

const kc::ks_ptr<TimeKeeper> &getTimeKeeper() const#

Return the TimeKeeper used by this StatePropagator.

Returns:

The TimeKeeper used by this StatePropagator.

const kc::ks_ptr<SubTree> &getSubTree() const#

Return the SubTree used by this StatePropagator.

Returns:

The SubTree used by this StatePropagator.

Public Members

bool trace_state_propagator = false#

Enable/disable state propagator debug message.

SpFunctions fns#

User defined functions to tailor the behavior of the state propagator

Public Static Functions

static kc::ks_ptr<StatePropagator> create(const kc::ks_ptr<SubTree> &st, const km::IntegratorType &integrator_type, km::Integrator::IntegratorOptions *integ_opts = nullptr, SpOptions *sp_opts = nullptr, SpSolverType solver_type = SpSolverType::UNDEFINED)#

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 explcitly specified since there is no good way to infer one.

Parameters:

  • st – the SubTree instance

  • integrator_type – the numerical integrator type

  • integ_opts – options for the numerical integrator

  • sp_opts – options for the state propagator

  • solver_type – the solver type

Returns:

An instance of the StatePropagator.

Protected Functions

SpStatusEnum _hopStep(const km::Ktime &to_time)#

Helper method to execute a hop sub-step.

SpStatusEnum _integrationStep(const km::Ktime &to_time)#

Helper method to execute a numerical integration sub-step.

This will also sanitize the coordinates at the end of the integration sub-step.

Parameters:

to_time – The time to integrate to.

Returns:

The status of the integration sub step.

void _setStateWithModels(const km::Vec &x, bool global = false)#

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 getStateWithModels() method.

Parameters:

  • x – the state vector

  • global – if true, the state coord values are for the global charts

void _discard(kc::ks_ptr<kc::Base> &base) override#

Discard the StatePropagator.

Parameters:

base – A base pointer to the StatePropagator to discard.

virtual void _makeCurrent() override#

Make the StatePropagator current.

void _checkConstraintErrors(double threshold) const#

Protected Attributes

Scheduler _scheduler#

Access to the scheduler for the state propagator.

kc::ks_ptr<SubTree> _st = nullptr#

the sub-tree

kc::ks_ptr<SubGraph> _sg = nullptr#

dynamic cast the subtree to a subgraph. Non-null only if the subtree is in fact a subgraph

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

the integrator

SpOptions _options#

the state propagator options

km::Ktime _tolerance = km::Ktime(100)#

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.

Eigen::Index _nstates#
kc::ks_ptr<TimeKeeper> _time_keeper = nullptr#

The TimeKeeper keeps track of the time.

km::Vec _curr_X#

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.

Friends

friend class kmo::KModel
struct Counters#

Struct to track state propgation statistics.

Public Members

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

struct SpFunctions#

Struct with optional user-defined functions to be used by the state propagator.

Public Members

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

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&> post_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&> pre_hop_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 (eg. 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&> post_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.

std::function<bool(const km::Ktime&, const km::Vec&)> step_validate_fn = nullptr#

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.

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

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 condtion occurs instead of continuing till the specified hop end time. See the

Zero-crossings 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 fucntion is a boolean. This will be true when we have reached the zero, and false otherwise.

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

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.

Friends

friend class StatePropagator
struct SpOptions#

Options for the state propagator.

Public Members

km::Ktime max_step_size = km::Ktime(0)#

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.

bool update_state_derivatives_hop_end = false#

If true, then update the multibody state derivatives with the integrator states at the end of each hop.

double baumgarte_stiffness = -1#

the Baumgarte stiffness parameter

Options specific for the selected integrator Parameters for the Baumgarte method for loop constraint stabilization

double baumgarte_damping = -1#

the Baumgarte damping parameter

Contents