P::getCurrentState returns prior if !applied & using bool isM::getState

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()){