[robots] Fix init attitude for fixed based robots

jrl-umi3218 · Jan 18, 2024 · 384a9bd · 384a9bd
1 parent a7b5322
commit 384a9bd
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp
@@ -293,6 +293,13 @@ Robot::Robot(NewRobotToken,
       const auto & attitude = module_.default_attitude();
       initQ[0] = {std::begin(attitude), std::end(attitude)};
     }
+    else
+    {
+      const auto & attitude = module_.default_attitude();
+      posW(sva::PTransformd{
+          Eigen::Quaterniond(attitude[0], attitude[1], attitude[2], attitude[3]).toRotationMatrix().inverse(),
+          Eigen::Vector3d(attitude[4], attitude[5], attitude[6])});
+    }
     mbc().q = initQ;
     forwardKinematics();
     forwardVelocity();