diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 3f35ce96421cc137e4eec8266309aaac94de89d3..77377539ed0989398472d46a0e1916d2b673467e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -878,13 +878,12 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
             cout << endl;
             if (metric && state_blocks)
             {
-                for (auto& pair_key_sb : S->getStateBlockMap())
-//                for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++)
+                for (auto& _key : S->getStructure())
                 {
-                    auto key = pair_key_sb.first;
+                    auto key = std::string(1,_key);
                     if (key == "P") cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
                     if (key == "I") cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    auto sb = S->getStateBlock(key);
+                    auto sb = S->getStateBlockPtrStatic(key);
                     if (sb)
                     {
                         cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
@@ -908,9 +907,12 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
             else if (state_blocks)
             {
                 cout << "    sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
-                for (auto sb : S->getStateBlockVec())
+                for (auto& key : S->getStructure())
+                {
+                    auto sb = S->getStateBlockPtrStatic(key);
                     if (sb != nullptr)
                         cout << " " << (sb->isFixed() ? "Fix" : "Est");
+                }
                 cout << endl;
             }
             if (depth >= 2)
@@ -975,9 +977,9 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
             if (state_blocks)
             {
                 cout << "    sb:";
-                for (const auto& pair_key_sb : F->getStateBlockMap())
+                for (const auto& key : F->getStructure())
                 {
-                    const auto& sb = pair_key_sb.second;
+                    const auto& sb = F->getStateBlock(key);
                     cout << " " << (sb->isFixed() ? "Fix" : "Est");
                 }
                 cout << endl;
@@ -1018,7 +1020,9 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
                     cout << endl;
 
                     if (state_blocks)
-                        for(auto sb : C->getStateBlockVec())
+                        for (const auto& key : C->HasStateBlocks::getStructure())
+                        {
+                            auto sb = C->HasStateBlocks::getStateBlock(key);
                             if(sb != nullptr)
                             {
                                 cout << "      sb: ";
@@ -1027,6 +1031,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
                                     cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
                                 cout << endl;
                             }
+                        }
 
                     if (C->isMotion() )
                     {
@@ -1115,9 +1120,6 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
                     if (sb != nullptr)
                         cout << (sb->isFixed() ? " Fix" : " Est");
                 }
-//                for (auto sb : L->getStateBlockVec())
-//                    if (sb != nullptr)
-//                        cout << (sb->isFixed() ? " Fix" : " Est");
                 cout << endl;
             }
         } // for L
@@ -1147,47 +1149,18 @@ std::string Problem::printToString(int depth, bool constr_by, bool metric, bool
             result << endl;
             if (metric && state_blocks)
             {
-
-
-
-
-
-
-                for (auto& pair_key_sb : S->getStateBlockMap())
+                for (const auto& _key : S->getStructure())
                 {
-                    auto key = pair_key_sb.first;
+                    auto key = std::string(1,_key);
                     if (key == "P") cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
                     if (key == "I") cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    auto sb = S->getStateBlock(key);
+                    auto sb = S->getStateBlockPtrStatic(key);
                     if (sb)
                     {
                         cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
                     }
                     if (key == "O") cout << " ]" << endl;
                 }
-
-
-
-
-
-
-
-//                for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++)
-//                {
-//                    if (i==0) result << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-//                    if (i==2) result << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-//                    auto sb = S->getStateBlock(i);
-//                    if (sb)
-//                    {
-//                        result << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
-//                    }
-//                    if (i==1) result << " ]" << endl;
-//                }
-
-
-
-
-
                 if (S->getStateBlockVec().size() > 2) result << " ]" << endl;
             }
             else if (metric)
@@ -1729,7 +1702,6 @@ bool Problem::check(int verbose_level) const
                         // find in constrained landmark
                         if (Lo)
                             found = found || (std::find_if(Lo->getStateBlockMap().begin(), Lo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Lo->getStateBlockMap().end());
-//                            found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end());
                         if (verbose_level > 0)
                         {
                             if (found)
@@ -1765,9 +1737,6 @@ bool Problem::check(int verbose_level) const
             for (const auto& key : L->getStructure())
             {
                 const auto& sb = L->getStateBlock(key);
-//            }
-//            for (auto sb : L->getStateBlockVec())
-//            {
                 cout <<  "  sb @ " << sb.get();
                 if (sb)
                 {