[mc_rbdyn] Always use default_attitude (closes #424)

jrl-umi3218 · Jan 19, 2024 · 4b63e3e · 4b63e3e
1 parent ad18f7b
commit 4b63e3e
Showing 1 changed file with 3 additions and 6 deletions.
diff --git 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();
   }