diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index 502254a9a..bb2dd21ee 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/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::SharedPtr set_io_srv_; rclcpp::Service::SharedPtr set_analog_output_srv_; rclcpp::Service::SharedPtr set_payload_srv_; + rclcpp::Service::SharedPtr set_gravity_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; std::shared_ptr> io_pub_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index ba16fb4b1..89cea6fe4 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/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( "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2)); + set_gravity_srv_ = get_node()->create_service( + "~/set_gravity", std::bind(&GPIOController::setGravity, this, std::placeholders::_1, std::placeholders::_2)); + tare_sensor_srv_ = get_node()->create_service( "~/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( + 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) { diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 3ba603f29..19e66e113 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/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 instruction_executor_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 376f20409..fd4c65999 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -456,6 +456,15 @@ std::vector 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_; diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 6394a5af7..6c44315b4 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -210,6 +210,13 @@ + + + + + + +