[KinematicInertial] Fix use of state-observation

jrl-umi3218 · Sep 11, 2023 · 9ce7fa3 · 9ce7fa3
1 parent d4de8d3
commit 9ce7fa3
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 36 deletions.
diff --git a/include/mc_observers/KinematicInertialPoseObserver.h b/include/mc_observers/KinematicInertialPoseObserver.h
@@ -101,41 +101,6 @@ struct MC_OBSERVER_DLLAPI KinematicInertialPoseObserver : public Observer
    */
   void estimatePosition(const mc_control::MCController & ctl);
 
-protected:
-  /**
-   * @brief Merge roll and pitch orientation from a rotation matrix R1 with yaw (rotation around gravity) from another
-   * rotation matrix R2
-   *
-   * This function was adpated from https://github.com/mehdi-benallegue/state-observation and modified to follow
-   * SpaceVecAlg conventions for rotation. It computes:
-   *
-   *
-   * \f[
-   *  R=\left(
-   *  \begin{array}{ccc}
-   *   \frac{m\times e_{z}}{\left\Vert m\times e_{z}\right\Vert } & \frac{e_{z}\times m\times e_{z}}{\left\Vert m\times
-   * e_{z}\right\Vert } & e_{z}\end{array}\right)\left(\begin{array}{ccc} \frac{m_{l}\times v_{1}}{\left\Vert
-   * m_{l}\times v_{1}\right\Vert } & \frac{v_{1}\times m_{l}\times v_{1}}{\left\Vert m_{l}\times v_{1}\right\Vert } &
-   * v_{1}\end{array}
-   *   \right)^{T}\\
-   *   v_{1}=R_{1}e_{z}\qquad m_{l}=R_{2}m\\
-   *   m = \left\{
-   *   \begin{array}{c}
-   *   e_x \mbox{ if } ||R_2e_x \times v_1||^2 < \epsilon^2\\
-   *   e_y \mbox{ otherwise }
-   *   \end{array}
-   *   \right.
-   *  \f]
-   *
-   * @param R1 First rotation matrix (for roll and pitch)
-   * @param R2 Second rotation matrix (for yaw)
-   *
-   * @return a rotation matrix composed of roll and pitch from R1, yaw from R2
-   */
-  inline Eigen::Matrix3d mergeRoll1Pitch1WithYaw2(const Eigen::Matrix3d & R1,
-                                                  const Eigen::Matrix3d & R2,
-                                                  double epsilonAngle = 1e-16);
-
 protected:
   std::string robot_; /**< Robot to observe (default main robot) */
   std::string realRobot_; /**< Corresponding real robot (default main real robot) */

diff --git a/src/mc_control/samples/LIPMStabilizer/etc/LIPMStabilizer.in.yaml b/src/mc_control/samples/LIPMStabilizer/etc/LIPMStabilizer.in.yaml
@@ -398,6 +398,7 @@ ObserverPipelines:
 - name: "MainPipeline"
   observers:
     - type: Encoder
+      #- type: Attitude
       # Floating base kinematic-inertial state observation
     - type: KinematicInertial
       # Floating base Groundtruth

diff --git a/src/mc_observers/KinematicInertialPoseObserver.cpp b/src/mc_observers/KinematicInertialPoseObserver.cpp
@@ -7,6 +7,7 @@
 
 #include <mc_observers/KinematicInertialPoseObserver.h>
 #include <mc_observers/ObserverMacros.h>
+#include <state-observation/tools/rigid-body-kinematics.hpp>
 
 #include <mc_control/MCController.h>
 #include <mc_rbdyn/rpy_utils.h>
@@ -110,7 +111,8 @@ void KinematicInertialPoseObserver::estimateOrientation(const mc_rbdyn::Robot &
   Eigen::Matrix3d E_0_mIMU = imuSensor.X_b_s().rotation().transpose() * imuSensor.orientation().toRotationMatrix();
   const Eigen::Matrix3d & E_0_cIMU = X_0_cIMU.rotation();
   // Estimate IMU orientation: merges roll+pitch from measurement with yaw from control
-  Eigen::Matrix3d E_0_eIMU = mergeRoll1Pitch1WithYaw2(E_0_mIMU, E_0_cIMU);
+  Eigen::Matrix3d E_0_eIMU =
+      stateObservation::kine::mergeRoll1Pitch1WithYaw2AxisAgnostic(E_0_mIMU.transpose(), E_0_cIMU.transpose());
   pose_.rotation() = X_rIMU_rBase.rotation() * E_0_eIMU.transpose();
 }