Resolve "Complete state vector new data structure?"

Closes #287 (closed)

src/problem/problem.cpp

@@ -426,6 +426,13 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
                     state.insert(pair_key_vec);
             }
         }
+        // check for empty blocks and fill them with zeros
+        for (const auto& ckey : frame_structure_)
+        {
+            const auto& key = string(1,ckey);
+            if (state.count(key) == 0)
+                state.emplace(key, stateZero(key).at(key));
+        }
     }
 
     return state;
@@ -532,7 +539,7 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
         if (key == "O")
         {
             if (dim_ == 2) vec = VectorXd::Zero(1);
-            if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
+            else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
         }
         else
         {