Merge pull request #2535 from JafarAbdi/fix_unknown_size_vector

mjcf parser: Fix unknown size vector parsing
stack-of-tasks · Jan 7, 2025 · aa816e9 · aa816e9
2 parents 600b2ac + f3cf7e5
commit aa816e9
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 4 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -11,6 +11,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Fix aba explicit template instantiation ([#2541](https://github.com/stack-of-tasks/pinocchio/pull/2541))
 - Add parsing meshes with vertices for MJCF format ([#2537](https://github.com/stack-of-tasks/pinocchio/pull/2537))
 - CMake: fix RPATH on macos ([#2546](https://github.com/stack-of-tasks/pinocchio/pull/2546))
+- Fix aba explicit template instantiation ([#2541](https://github.com/stack-of-tasks/pinocchio/pull/2541))
+- Fix mjcf parsing of keyframe qpos with newlines ([#2535](https://github.com/stack-of-tasks/pinocchio/pull/2535))
 
 ## [3.3.1] - 2024-12-13
 

diff --git 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
@@ -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>)");
 
@@ -1398,4 +1400,32 @@ BOOST_AUTO_TEST_CASE(parse_mesh_with_vertices)
   }
 }
 
+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()