[Ninja][Navigation] fix bug in FK in assembled state

sugihara-16 · Sep 5, 2024 · b759d88 · b759d88
1 parent 52848ff
commit b759d88
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp
@@ -293,8 +293,8 @@ void NinjaNavigator::calcCenterOfMoving()
                     KDL::ChainFkSolverPos_recursive fk_solver(chain);
                     KDL::Frame frame;
                     KDL::JntArray joint_positions(2);
-                    joint_positions(0) = data.joint_pos_(YAW);
-                    joint_positions(1) = data.joint_pos_(PITCH);
+                    joint_positions(0) = data.joint_pos_(PITCH);
+                    joint_positions(1) = data.joint_pos_(YAW);
                     if (fk_solver.JntToCart(joint_positions, frame) < 0) {
                       ROS_ERROR_STREAM("Failed to compute FK for module" << it.first);
                       return;