fix quaterion bindings for change_fixed_frame

ami-iit · Jun 23, 2024 · cedbfe8 · cedbfe8
1 parent 0a36ffb
commit cedbfe8
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 4 deletions.
diff --git a/bindings/python/FloatingBaseEstimators/src/LeggedOdometry.cpp b/bindings/python/FloatingBaseEstimators/src/LeggedOdometry.cpp
@@ -91,7 +91,7 @@ void CreateLeggedOdometry(pybind11::module& module)
              py::arg("frame_index"))
         .def("change_fixed_frame",
              py::overload_cast<const std::ptrdiff_t&,
-                               const Eigen::Quaterniond&,
+                               const Eigen::Vector4d&,
                                const Eigen::Vector3d&>(&LeggedOdometry::changeFixedFrame),
              py::arg("frame_index"),
              py::arg("frame_orientation_in_world"),

diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/LeggedOdometry.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/LeggedOdometry.h
@@ -93,7 +93,7 @@ class LeggedOdometry : public FloatingBaseEstimator
      * @note ensure to pass unit quaternion
      */
     bool changeFixedFrame(const std::ptrdiff_t& frameIndex,
-                          const Eigen::Quaterniond& frameOrientationInWorld,
+                          const Eigen::Vector4d& frameOrientationInWorld,
                           const Eigen::Vector3d& framePositionInWorld);
 
     /**

diff --git a/src/Estimators/src/LeggedOdometry.cpp b/src/Estimators/src/LeggedOdometry.cpp
@@ -917,7 +917,7 @@ bool LeggedOdometry::Impl::changeFixedFrame(const iDynTree::FrameIndex& newIdx,
 
 
 bool LeggedOdometry::changeFixedFrame(const std::ptrdiff_t& newIdx,
-                                      const Eigen::Quaterniond& frameOrientationInWorld,
+                                      const Eigen::Vector4d& frameOrientationInWorld,
                                       const Eigen::Vector3d& framePositionInWorld)
 {
     const std::string_view printPrefix = "[LeggedOdometry::changeFixedFrame] ";
@@ -929,7 +929,10 @@ bool LeggedOdometry::changeFixedFrame(const std::ptrdiff_t& newIdx,
             return false;
     }
 
-    m_pimpl->m_world_H_fixedFrame.quat(frameOrientationInWorld);
+    Eigen::Quaterniond frameOrientationInWorldQ(frameOrientationInWorld);
+
+    m_pimpl->m_world_H_fixedFrame.quat(frameOrientationInWorldQ);
+
     m_pimpl->m_world_H_fixedFrame.translation(framePositionInWorld);
     m_pimpl->m_currentFixedFrameIdx = newIdx;