diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index ee3884ba30cdd6f434956ebde18b86b6c424839a..5a38d11f4a825c4a8320e0e6d8a6c3e6cc1e3bb4 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -461,9 +461,16 @@ VectorComposite Problem::getState(const StateStructure& _structure) const } // check for empty blocks and fill them with the last KF, or with zeros in the worst case + VectorComposite state_last; + const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); - if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure); + + if (last_kf_or_aux) + state_last = last_kf_or_aux->getState(structure); + + else if (prior_options_ and prior_options_->mode != "nothing") + state_last = prior_options_->state; for (const auto& ckey : structure) { @@ -504,9 +511,16 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ } // check for empty blocks and fill them with the closest KF to ts, or with zeros in the worst case + VectorComposite state_last; + const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); - if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure); + + if (last_kf_or_aux) + state_last = last_kf_or_aux->getState(structure); + + else if (prior_options_ and prior_options_->mode != "nothing") + state_last = prior_options_->state; for (const auto& ckey : structure) {