Explicitly just rotate gravity vector

Signed-off-by: AdamPettinger <adam.l.pettinger@gmail.com>

Author: AdamPettinger

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -58,6 +58,8 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/duration.hpp"
 #include "std_msgs/msg/bool.hpp"
+#include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_ros/buffer.hpp"
 #include "tf2_ros/transform_listener.hpp"
 #include "ur_controllers/gpio_controller_parameters.hpp"

ur_controllers/src/gpio_controller.cpp

@@ -590,16 +590,18 @@ bool GPIOController::setGravity(const ur_msgs::srv::SetGravity::Request::SharedP
     return false;
   }
 
-  // Transform the gravity vector
-  geometry_msgs::msg::Vector3 transformed_gravity;
-  tf2::doTransform(req->gravity.vector, transformed_gravity, tf_to_base_link);
+  // Rotate the gravity vector
+  tf2::Vector3 raw_gravity(req->gravity.vector.x, req->gravity.vector.y, req->gravity.vector.z);
+  tf2::Quaternion quat;
+  tf2::fromMsg(tf_to_base_link.transform.rotation, quat);
+  tf2::Vector3 transformed_gravity = tf2::quatRotate(quat, raw_gravity);
 
   // reset success flag
   std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
 
-  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_X].set_value(transformed_gravity.x);
-  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_Y].set_value(transformed_gravity.y);
-  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_Z].set_value(transformed_gravity.z);
+  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_X].set_value(transformed_gravity.x());
+  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_Y].set_value(transformed_gravity.y());
+  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_Z].set_value(transformed_gravity.z());
 
   if (!waitForAsyncCommand([&]() {
         return command_interfaces_[CommandInterfaces::GRAVITY_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);