Update to using quaternion

Author: Felix Exner

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -80,6 +80,28 @@ enum StoppingInterface
   STOP_VELOCITY
 };
 
+// We define our own quaternion to use it as a buffer, since we need to pass pointers to the state
+// interfaces.
+struct Quaternion
+{
+  Quaternion() : x(0), y(0), z(0), w(0)
+  {
+  }
+
+  void set(const tf2::Quaternion& q)
+  {
+    x = q.x();
+    y = q.y();
+    z = q.z();
+    w = q.w();
+  }
+
+  double x;
+  double y;
+  double z;
+  double w;
+};
+
 /*!
  * \brief The HardwareInterface class handles the interface between the ROS system and the main
  * driver. It contains the read and write methods of the main control loop and registers various ROS
@@ -144,8 +166,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t urcl_joint_efforts_;
   urcl::vector6d_t urcl_ft_sensor_measurements_;
   urcl::vector6d_t urcl_tcp_pose_;
-  urcl::vector3d_t tcp_orientation_rpy_;
   tf2::Quaternion tcp_rotation_quat_;
+  Quaternion tcp_rotation_buffer;
 
   bool packet_read_;
 

ur_robot_driver/src/hardware_interface.cpp

@@ -243,11 +243,13 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
   state_interfaces.emplace_back(
       hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.z", &urcl_tcp_pose_[2]));
   state_interfaces.emplace_back(
-      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.r", &tcp_orientation_rpy_[0]));
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.x", &tcp_rotation_buffer.x));
   state_interfaces.emplace_back(
-      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.p", &tcp_orientation_rpy_[1]));
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.y", &tcp_rotation_buffer.y));
   state_interfaces.emplace_back(
-      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.y", &tcp_orientation_rpy_[2]));
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.z", &tcp_rotation_buffer.z));
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.w", &tcp_rotation_buffer.w));
 
   return state_interfaces;
 }
@@ -802,8 +804,7 @@ void URPositionHardwareInterface::extractToolPose()
   } else {
     tcp_rotation_quat_.setValue(0.0, 0.0, 0.0, 1.0);  // default Quaternion is 0,0,0,0 which is invalid
   }
-  auto rot_mat = tf2::Matrix3x3(tcp_rotation_quat_);
-  rot_mat.getRPY(tcp_orientation_rpy_[0], tcp_orientation_rpy_[1], tcp_orientation_rpy_[2]);
+  tcp_rotation_buffer.set(tcp_rotation_quat_);
 }
 
 hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch(