diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index c5d21f25e12ac3ced6c77862b2c608f30636b51a..1936420a1ad0b160cde194c5e67b2f8f6b159565 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -367,7 +367,12 @@ void Problem::getCurrentState(Eigen::VectorXd& _state) const void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const { - if (!processor_is_motion_list_.empty()) + if (!isPriorSet() and prior_options_->mode != "nothing") + { + _state = prior_options_->state; + _ts = 0; + } + else if (!processor_is_motion_list_.empty()) { // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state) std::list<TimeStamp> proc_is_motion_current_ts; @@ -389,7 +394,7 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c } -// Problem of this implmentation: if more state +// Problem of this implementation: if more state void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const { // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp @@ -413,7 +418,9 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const for (auto proc: processor_is_motion_list_) { - Eigen::VectorXd proc_state = proc->getState(_ts); + Eigen::VectorXd proc_state; + if (!proc->getState(_ts, proc_state)) + continue; int idx = 0; for (char sb_name: proc->getStateStructure()){