Allow updating robot gravity

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -50,6 +50,7 @@
 #include "ur_msgs/msg/tool_data_msg.hpp"
 #include "ur_dashboard_msgs/msg/robot_mode.hpp"
 #include "ur_dashboard_msgs/msg/safety_mode.hpp"
+#include "ur_msgs/srv/set_gravity.hpp"
 #include "ur_msgs/srv/set_io.hpp"
 #include "ur_msgs/srv/set_analog_output.hpp"
 #include "ur_msgs/srv/set_speed_slider_fraction.hpp"
@@ -81,6 +82,10 @@ enum CommandInterfaces
   HAND_BACK_CONTROL_CMD = 33,
   HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
   ANALOG_OUTPUTS_DOMAIN = 35,
+  GRAVITY_X = 36,
+  GRAVITY_Y = 37,
+  GRAVITY_Z = 38,
+  GRAVITY_ASYNC_SUCCESS = 39,
 };
 
 enum StateInterfaces
@@ -139,6 +144,9 @@ class GPIOController : public controller_interface::ControllerInterface
   bool setPayload(const ur_msgs::srv::SetPayload::Request::SharedPtr req,
                   ur_msgs::srv::SetPayload::Response::SharedPtr resp);
 
+  bool setGravity(const ur_msgs::srv::SetGravity::Request::SharedPtr req,
+                  ur_msgs::srv::SetGravity::Response::SharedPtr resp);
+
   bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
 
   void publishIO();
@@ -168,6 +176,7 @@ class GPIOController : public controller_interface::ControllerInterface
   rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
   rclcpp::Service<ur_msgs::srv::SetAnalogOutput>::SharedPtr set_analog_output_srv_;
   rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
+  rclcpp::Service<ur_msgs::srv::SetGravity>::SharedPtr set_gravity_srv_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
 
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;

ur_controllers/src/gpio_controller.cpp

@@ -99,6 +99,12 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
 
   config.names.emplace_back(tf_prefix + "gpio/analog_output_domain_cmd");
 
+  // Gravity stuff
+  config.names.emplace_back(tf_prefix + "gravity/x");
+  config.names.emplace_back(tf_prefix + "gravity/y");
+  config.names.emplace_back(tf_prefix + "gravity/z");
+  config.names.emplace_back(tf_prefix + "gravity/gravity_async_success");
+
   return config;
 }
 
@@ -318,6 +324,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
     set_payload_srv_ = get_node()->create_service<ur_msgs::srv::SetPayload>(
         "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2));
 
+    set_gravity_srv_ = get_node()->create_service<ur_msgs::srv::SetGravity>(
+        "~/set_gravity", std::bind(&GPIOController::setGravity, this, std::placeholders::_1, std::placeholders::_2));
+
     tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
         "~/zero_ftsensor",
         std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
@@ -564,6 +573,36 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
   return true;
 }
 
+bool GPIOController::setGravity(const ur_msgs::srv::SetGravity::Request::SharedPtr req,
+                                ur_msgs::srv::SetGravity::Response::SharedPtr resp)
+{
+  // reset success flag
+  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+
+  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_X].set_value(req->gravity.x);
+  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_Y].set_value(req->gravity.y);
+  std::ignore = command_interfaces_[CommandInterfaces::GRAVITY_Z].set_value(req->gravity.z);
+
+  if (!waitForAsyncCommand([&]() {
+        return command_interfaces_[CommandInterfaces::GRAVITY_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
+      })) {
+    RCLCPP_WARN(get_node()->get_logger(), "Could not verify that gravity was set. (This might happen when using the "
+                                          "mocked interface)");
+  }
+
+  resp->success = static_cast<bool>(
+      command_interfaces_[CommandInterfaces::GRAVITY_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
+
+  if (resp->success) {
+    RCLCPP_INFO_ONCE(get_node()->get_logger(), "Gravity has been set successfully");
+  } else {
+    RCLCPP_ERROR_ONCE(get_node()->get_logger(), "Could not set the gravity");
+    return false;
+  }
+
+  return true;
+}
+
 bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
                                   std_srvs::srv::Trigger::Response::SharedPtr resp)
 {

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -291,6 +291,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   double force_mode_damping_;
   double force_mode_gain_scaling_;
 
+  // Gravity stuff
+  urcl::vector3d_t gravity_vector_;
+  double gravity_async_success_;
+
   //*************** Motion primitives stuff ***************
   std::shared_ptr<urcl::InstructionExecutor> instruction_executor_;
 

ur_robot_driver/src/hardware_interface.cpp

@@ -456,6 +456,15 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
   command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "damping", &force_mode_damping_);
   command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "gain_scaling", &force_mode_gain_scaling_);
 
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + "gravity", "x", &gravity_vector_[0]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + "gravity", "y", &gravity_vector_[1]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + "gravity", "z", &gravity_vector_[2]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + "gravity", "gravity_async_success", &gravity_async_success_));
+
   for (size_t i = 0; i < 18; ++i) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
         tf_prefix + "gpio", "standard_digital_output_cmd_" + std::to_string(i), &standard_dig_out_bits_cmd_[i]));
@@ -1036,6 +1045,8 @@ void URPositionHardwareInterface::initAsyncIO()
 
   payload_mass_ = NO_NEW_CMD_;
   payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
+
+  gravity_vector_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
 }
 
 void URPositionHardwareInterface::checkAsyncIO()
@@ -1105,6 +1116,12 @@ void URPositionHardwareInterface::checkAsyncIO()
     payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
   }
 
+  if (!std::isnan(gravity_vector_[0]) && !std::isnan(gravity_vector_[1]) && !std::isnan(gravity_vector_[2]) &&
+      ur_driver_ != nullptr) {
+    gravity_async_success_ = ur_driver_->setGravity(gravity_vector_);
+    gravity_vector_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
+  }
+
   if (!std::isnan(zero_ftsensor_cmd_) && ur_driver_ != nullptr) {
     zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor();
     zero_ftsensor_cmd_ = NO_NEW_CMD_;

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -210,6 +210,13 @@
         <command_interface name="payload_async_success"/>
       </gpio>
 
+      <gpio name="${tf_prefix}gravity">
+        <command_interface name="x"/>
+        <command_interface name="y"/>
+        <command_interface name="z"/>
+        <command_interface name="gravity_async_success"/>
+      </gpio>
+
       <gpio name="${tf_prefix}resend_robot_program">
         <command_interface name="resend_robot_program_cmd"/>
         <command_interface name="resend_robot_program_async_success"/>