fix quaternion vector to match FRI standard

kroshu · Oct 15, 2024 · f8cf452 · f8cf452
1 parent 419c591
commit f8cf452
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 10 deletions.
diff --git a/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml b/kuka_sunrise_fri_driver/config/cartesian_pose_controller_config.yaml
@@ -4,10 +4,11 @@ cartesian_pose_controller:
     - dummy_cart_joint/x
     - dummy_cart_joint/y
     - dummy_cart_joint/z
+    - dummy_cart_joint/qw
     - dummy_cart_joint/qx
     - dummy_cart_joint/qy
     - dummy_cart_joint/qz
-    - dummy_cart_joint/qw 
+
 
     interface_names:
     - position
diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp
@@ -302,11 +302,6 @@ hardware_interface::return_type KukaFRIHardwareInterface::read(
     robot_state_.operation_mode_ = robotState().getOperationMode();
     robot_state_.drive_state_ = robotState().getDriveState();
     robot_state_.overlay_type_ = robotState().getOverlayType();
-    RCLCPP_DEBUG(
-        rclcpp::get_logger("KukaFRIHardwareInterface"),
-        "cartesianPoseQuaternion state: %f %f %f %f %f %f %f",cart_pose[0],cart_pose[1],
-        cart_pose[2],cart_pose[3],cart_pose[4],cart_pose[5]
-        ,cart_pose[6]);
     for (auto & output : gpio_outputs_)
     {
       output.getValue();
@@ -462,8 +457,8 @@ std::vector<hardware_interface::StateInterface> KukaFRIHardwareInterface::export
   }
   std::vector<std::string> cart_joints_list = {
     hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z,
-    hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ,
-    hardware_interface::HW_IF_QW};
+    hardware_interface::HW_IF_QW,hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY,
+    hardware_interface::HW_IF_QZ};
   for (size_t i = 0; i < cart_joints_list.size(); ++i)
   {
     state_interfaces.emplace_back(
@@ -509,8 +504,9 @@ KukaFRIHardwareInterface::export_command_interfaces()
   }
   std::vector<std::string> cart_joints_list = {
     hardware_interface::HW_IF_X, hardware_interface::HW_IF_Y, hardware_interface::HW_IF_Z,
-    hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY, hardware_interface::HW_IF_QZ,
-    hardware_interface::HW_IF_QW};
+    hardware_interface::HW_IF_QW, hardware_interface::HW_IF_QX, hardware_interface::HW_IF_QY,
+    hardware_interface::HW_IF_QZ,
+    };
   for (size_t i = 0; i < cart_joints_list.size(); ++i)
   {
     command_interfaces.emplace_back(