From 4b63e3e7107ace0597cb70864ba56c54bb36a3b8 Mon Sep 17 00:00:00 2001
From: Pierre Gergondet <pierre.gergondet@gmail.com>
Date: Fri, 19 Jan 2024 11:07:35 +0900
Subject: [PATCH] [mc_rbdyn] Always use default_attitude (closes #424)

---
 src/mc_rbdyn/Robot.cpp | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp
index b35d18b4b1..94a687ec7b 100644
--- a/src/mc_rbdyn/Robot.cpp
+++ b/src/mc_rbdyn/Robot.cpp
@@ -288,13 +288,10 @@ Robot::Robot(NewRobotToken,
         initQ[i] = jQ;
       }
     }
-    if(initQ[0].size())
-    {
-      const auto & attitude = module_.default_attitude();
-      initQ[0] = {std::begin(attitude), std::end(attitude)};
-    }
     mbc().q = initQ;
-    forwardKinematics();
+    const auto & attitude = module_.default_attitude();
+    posW({Eigen::Quaterniond(attitude[0], attitude[1], attitude[2], attitude[3]).inverse(),
+          Eigen::Vector3d(attitude[4], attitude[5], attitude[6])});
     forwardVelocity();
     forwardAcceleration();
   }