From d1fabfaea55a84743716b3f3f8ff5fa202ec58e5 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 2 Jun 2020 09:29:49 +0200
Subject: [PATCH] P::getCurrentState returns prior if !applied & using bool
isM::getState
---
src/problem/problem.cpp | 13 ++++++++++---
1 file changed, 10 insertions(+), 3 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c5d21f25e..1936420a1 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()){
--
GitLab