From 5a60333090a7b33e69bd7e870e71aab4adf2f51b Mon Sep 17 00:00:00 2001
From: JafarAbdi <jafar.uruc@gmail.com>
Date: Tue, 31 Dec 2024 00:33:25 +0000
Subject: [PATCH] Improves MJCF unknown size vector parsing robustness

---
 include/pinocchio/parsers/mjcf/mjcf-graph.hpp |  5 ++--
 unittest/mjcf.cpp                             | 30 ++++++++++++++++++-
 2 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp
index 081a0cf35..4960a24a6 100644
--- a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp
+++ b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp
@@ -549,7 +549,7 @@ namespace pinocchio
         inline std::istringstream getConfiguredStringStream(const std::string & str)
         {
           std::istringstream posStream(str);
-          posStream.exceptions(std::ios::failbit);
+          posStream.exceptions(std::ios::badbit);
           return posStream;
         }
 
@@ -569,9 +569,8 @@ namespace pinocchio
           std::istringstream stream = getConfiguredStringStream(str);
           std::vector<double> vector;
           double elem;
-          while (!stream.eof())
+          while (stream >> elem)
           {
-            stream >> elem;
             vector.push_back(elem);
           }
 
diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp
index 25c016acc..56e0d60ca 100644
--- a/unittest/mjcf.cpp
+++ b/unittest/mjcf.cpp
@@ -8,6 +8,7 @@
 #include "pinocchio/multibody/model.hpp"
 
 #include "pinocchio/parsers/mjcf.hpp"
+#include "pinocchio/parsers/mjcf/mjcf-graph.hpp"
 #include "pinocchio/parsers/urdf.hpp"
 
 #include "pinocchio/algorithm/joint-configuration.hpp"
@@ -923,7 +924,8 @@ BOOST_AUTO_TEST_CASE(adding_keyframes)
                     <key name="test"
                     qpos="0 0 0.596
                         0.988015 0 0.154359 0
-                        0.988015 0 0.154359 0"/>
+                        0.988015 0 0.154359 0
+                        "/>
                 </keyframe>
                 </mujoco>)");
 
@@ -1396,6 +1398,32 @@ BOOST_AUTO_TEST_CASE(parse_mesh_with_vertices)
   {
     BOOST_CHECK(mesh.vertices.row(i) == vertices.row(i));
   }
+BOOST_AUTO_TEST_CASE(test_get_unknown_size_vector_from_stream)
+{
+  const auto v = pinocchio::mjcf::details::internal::getUnknownSizeVectorFromStream("");
+  BOOST_CHECK(v.size() == 0);
+
+  const auto v1 = pinocchio::mjcf::details::internal::getUnknownSizeVectorFromStream("1 2 3");
+  BOOST_CHECK(v1.size() == 3);
+  Eigen::VectorXd expected(3);
+  expected << 1, 2, 3;
+  BOOST_CHECK(v1 == expected);
+
+  const auto v2 = pinocchio::mjcf::details::internal::getUnknownSizeVectorFromStream(R"(1 2 3
+                                                                                        4 5 6)");
+  BOOST_CHECK(v2.size() == 6);
+  Eigen::VectorXd expected2(6);
+  expected2 << 1, 2, 3, 4, 5, 6;
+  BOOST_CHECK(v2 == expected2);
+
+  const auto v3 = pinocchio::mjcf::details::internal::getUnknownSizeVectorFromStream(R"(1 2 3
+                                                                                        4 5 6
+                                                                                        7 8 9
+                                                                                        )");
+  BOOST_CHECK(v3.size() == 9);
+  Eigen::VectorXd expected3(9);
+  expected3 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  BOOST_CHECK(v3 == expected3);
 }
 
 BOOST_AUTO_TEST_SUITE_END()