From 0d51d0f69687781d4106ac2ff39ea3c69e27b897 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 30 Jan 2024 07:37:38 -0700 Subject: [PATCH] add optional interfaces for force and velocity Signed-off-by: Paul Gesel --- .../gripper_action_controller.hpp | 4 +- .../gripper_action_controller_impl.hpp | 47 ++++++++++++++----- .../hardware_interface_adapter.hpp | 22 ++++----- .../gripper_action_controller_parameters.yaml | 22 ++++----- 4 files changed, 54 insertions(+), 41 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 99219540c8..2914a4c333 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -128,9 +128,9 @@ class GripperActionController : public controller_interface::ControllerInterface std::experimental::optional> joint_position_command_interface_; std::optional> - force_multiplier_interface_; + effort_interface_; std::optional> - speed_multiplier_interface_; + speed_interface_; std::optional> joint_position_state_interface_; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index edfa567709..a38c0636ce 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -100,8 +100,22 @@ void GripperActionController::accepted_callback( // This is the non-realtime command_struct // We use command_ for sharing command_struct_.position_ = goal_handle->get_goal()->command.position; - command_struct_.target_velocity_ = goal_handle->get_goal()->command.target_velocity; - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + if (params_.use_speed_interface) + { + command_struct_.target_velocity_ = goal_handle->get_goal()->command.target_velocity; + } + else + { + command_struct_.target_velocity_ = 0.0; + } + if (params_.use_effort_interface) + { + command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + } + else + { + command_struct_.max_effort_ = params_.max_effort; + } command_.writeFromNonRT(command_struct_); @@ -286,21 +300,19 @@ controller_interface::CallbackReturn GripperActionController: for (auto & interface : command_interfaces_) { - if (interface.get_name() == "gripper_speed_multiplier") + if (interface.get_interface_name() == "gripper_effort") { - speed_multiplier_interface_ = interface; + effort_interface_ = interface; } - else if (interface.get_name() == "gripper_force_multiplier") + else if (interface.get_interface_name() == "gripper_speed") { - force_multiplier_interface_ = interface; + speed_interface_ = interface; } } - force_multiplier_interface_.value().get().set_value(params_.force_multiplier); - speed_multiplier_interface_.value().get().set_value(params_.speed_multiplier); // Hardware interface adapter - hw_iface_adapter_.init( - joint_position_command_interface_, force_multiplier_interface_, speed_multiplier_interface_, get_node()); + + hw_iface_adapter_.init(joint_position_command_interface_, speed_interface_, effort_interface_, get_node()); // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); @@ -340,9 +352,18 @@ template controller_interface::InterfaceConfiguration GripperActionController::command_interface_configuration() const { - return { - controller_interface::interface_configuration_type::INDIVIDUAL, - {joint_name_ + "/" + hardware_interface::HW_IF_POSITION}}; + std::vector names = {params_.joint + "/" + HardwareInterface}; + if (params_.use_effort_interface) + { + names.push_back({params_.joint + "/gripper_effort"}); + } + if (params_.use_speed_interface) + { + names.push_back({params_.joint + "/gripper_speed"}); + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; + } template diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 09f5bf7c83..25172da4d3 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -72,14 +72,13 @@ class HardwareInterfaceAdapter public: bool init( std::optional> joint_handle, - std::optional> force_handle, - std::optional> - target_velocity_handle, + std::optional> speed_handle, + std::optional> effort_handle, const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) { joint_handle_ = joint_handle; - force_handle_ = force_handle; - target_velocity_handle_ = target_velocity_handle; + effort_handle_ = effort_handle; + speed_handle_ = speed_handle; return true; } @@ -92,22 +91,21 @@ class HardwareInterfaceAdapter { // Forward desired position to command joint_handle_->get().set_value(desired_position); - if (target_velocity_handle_.has_value()) + if (speed_handle_.has_value()) { - target_velocity_handle_->get().set_value(desired_velocity); + speed_handle_->get().set_value(desired_velocity); } - if (force_handle_.has_value()) + if (effort_handle_.has_value()) { - force_handle_->get().set_value(max_allowed_effort); + effort_handle_->get().set_value(max_allowed_effort); } return max_allowed_effort; } private: std::optional> joint_handle_; - std::optional> force_handle_; - std::optional> - target_velocity_handle_; + std::optional> effort_handle_; + std::optional> speed_handle_; }; /** diff --git a/gripper_controllers/src/gripper_action_controller_parameters.yaml b/gripper_controllers/src/gripper_action_controller_parameters.yaml index 3d7fb2d032..5c6fe166ac 100644 --- a/gripper_controllers/src/gripper_action_controller_parameters.yaml +++ b/gripper_controllers/src/gripper_action_controller_parameters.yaml @@ -41,19 +41,13 @@ gripper_action_controller: description: "stall timeout", default_value: 1.0, } - force_multiplier: { - type: double, - description: "Scales the maximum effort of the gripper defined in the URDF", - default_value: 1.0, - validation: { - bounds<>: [ 0.0, 1.0 ] - }, + use_effort_interface: { + type: bool, + description: "Controller will claim the {joint}/gripper_effort interface if true.", + default_value: false, } - speed_multiplier: { - type: double, - description: "Scales the default velocity of the gripper defined in the URDF", - default_value: 1.0, - validation: { - bounds<>: [ 0.0, 1.0 ] - }, + use_speed_interface: { + type: bool, + description: "Controller will claim the {joint}/gripper_speed interface if true.", + default_value: false, }