Upgrade API of composites

include/core/state_block/has_state_blocks.h

@@ -88,6 +88,7 @@ class HasStateBlocks
         bool            getState(VectorComposite& _state) const;
         void            setState(const VectorComposite& _state);
         bool            setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
+        bool            setState(const VectorXd& _state, const StateStructure& _structure, const std::list<int>& _sizes);
 
         // Perturb state with noise
         void            perturb(double amplitude = 0.01);
@@ -241,6 +242,23 @@ inline bool HasStateBlocks::setState (const StateStructure& _structure, const st
     return true;
 }
 
+inline bool HasStateBlocks::setState (const VectorXd& _state, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+    assert(_structure.size() == _sizes.size() && "Structure and sizes mismatch");
+
+    int index = 0;
+    auto sizes_it = _sizes.begin();
+
+    for (const auto& ckey : _structure)
+    {
+        string key(1,ckey);
+        state_block_composite_.set(key, _state.segment(index, *sizes_it));
+        index += *sizes_it;
+        sizes_it++;
+    }
+    return true;
+}
+
 } // namespace wolf
 
 #endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */

include/core/state_block/state_composite.h

@@ -47,6 +47,7 @@ class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd
          *   vec_comp["b"].transpose(); // = (3,4,5);
          */
         VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+        VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
 
         unsigned int size(const StateStructure& _structure) const;
 

src/state_block/state_composite.cpp

@@ -47,6 +47,19 @@ VectorComposite::VectorComposite (const StateStructure& _s)
     }
 }
 
+VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors)
+{
+    assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch");
+
+    auto vector_it = _vectors.begin();
+
+    for (const auto& ckey : _structure)
+    {
+        string key(1,ckey);
+        this->emplace(key, *vector_it);
+        vector_it++;
+    }
+}
 
 
 unsigned int VectorComposite::size(const StateStructure &_structure) const