From b759d88e13cae29ac8eb3e0869153f6513a755a7 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Thu, 5 Sep 2024 22:26:46 +0900 Subject: [PATCH] [Ninja][Navigation] fix bug in FK in assembled state --- robots/ninja/src/ninja_navigation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp index a16e6f720..68ffc64c2 100644 --- 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;