diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 9f1b74d3ab..99219540c8 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -65,8 +65,9 @@ class GripperActionController : public controller_interface::ControllerInterface */ struct Commands { - double position_; // Last commanded position - double max_effort_; // Max allowed effort + double position_; // Last commanded position + double target_velocity_; // Desired gripper velocity + double max_effort_; // Max allowed effort }; GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); @@ -123,9 +124,15 @@ class GripperActionController : public controller_interface::ControllerInterface bool verbose_ = false; ///< Hard coded verbose flag to help in debugging std::string name_; ///< Controller name. + std::experimental::optional> joint_position_command_interface_; - std::experimental::optional> + std::optional> + force_multiplier_interface_; + std::optional> + speed_multiplier_interface_; + std::optional> + joint_position_state_interface_; std::experimental::optional> joint_velocity_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 2cd66776b5..edfa567709 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -68,14 +68,14 @@ controller_interface::return_type GripperActionController::up const double current_velocity = joint_velocity_state_interface_->get().get_value(); const double error_position = command_struct_rt_.position_ - current_position; - const double error_velocity = -current_velocity; + const double error_velocity = command_struct_rt_.target_velocity_ - current_velocity; check_for_success(get_node()->now(), error_position, current_position, current_velocity); // Hardware interface adapter: Generate and send commands computed_command_ = hw_iface_adapter_.updateCommand( - command_struct_rt_.position_, 0.0, error_position, error_velocity, - command_struct_rt_.max_effort_); + command_struct_rt_.position_, command_struct_rt_.target_velocity_, error_position, + error_velocity, command_struct_rt_.max_effort_); return controller_interface::return_type::OK; } @@ -97,11 +97,13 @@ void GripperActionController::accepted_callback( // Accept new goal preempt_active_goal(); - // This is the non-realtime command_struct - // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position; - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; - command_.writeFromNonRT(command_struct_); + // 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; + command_.writeFromNonRT(command_struct_); + pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false; @@ -145,6 +147,7 @@ void GripperActionController::set_hold_position() { command_struct_.position_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = default_max_effort_; + command_struct_.target_velocity_ = 0.0; command_.writeFromNonRT(command_struct_); } @@ -281,12 +284,29 @@ controller_interface::CallbackReturn GripperActionController: joint_position_state_interface_ = *position_state_interface_it; joint_velocity_state_interface_ = *velocity_state_interface_it; + for (auto & interface : command_interfaces_) + { + if (interface.get_name() == "gripper_speed_multiplier") + { + speed_multiplier_interface_ = interface; + } + else if (interface.get_name() == "gripper_force_multiplier") + { + force_multiplier_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_, get_node()); + hw_iface_adapter_.init( + joint_position_command_interface_, force_multiplier_interface_, speed_multiplier_interface_, get_node()); // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = default_max_effort_; + command_struct_.target_velocity_ = 0.0; + command_.initRT(command_struct_); // Result pre_alloc_result_ = std::make_shared(); diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 40fde4442c..09f5bf7c83 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -71,11 +71,15 @@ class HardwareInterfaceAdapter { public: bool init( - std::experimental::optional> - joint_handle, + std::optional> joint_handle, + std::optional> force_handle, + std::optional> + target_velocity_handle, const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) { joint_handle_ = joint_handle; + force_handle_ = force_handle; + target_velocity_handle_ = target_velocity_handle; return true; } @@ -83,17 +87,27 @@ class HardwareInterfaceAdapter void stopping(const rclcpp::Time & /* time */) {} double updateCommand( - double desired_position, double /* desired_velocity */, double /* error_position */, + double desired_position, double desired_velocity, double /* error_position */, double /* error_velocity */, double max_allowed_effort) { // Forward desired position to command joint_handle_->get().set_value(desired_position); + if (target_velocity_handle_.has_value()) + { + target_velocity_handle_->get().set_value(desired_velocity); + } + if (force_handle_.has_value()) + { + force_handle_->get().set_value(max_allowed_effort); + } return max_allowed_effort; } private: - std::experimental::optional> - joint_handle_; + std::optional> joint_handle_; + std::optional> force_handle_; + std::optional> + target_velocity_handle_; }; /** @@ -130,8 +144,11 @@ class HardwareInterfaceAdapter } bool init( - std::experimental::optional> - joint_handle, + std::optional> joint_handle, + std::optional< + std::reference_wrapper> /* force_handle */, + std::optional> /* target_velocity_handle */, const std::shared_ptr & node) { joint_handle_ = joint_handle; diff --git a/gripper_controllers/src/gripper_action_controller_parameters.yaml b/gripper_controllers/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..3d7fb2d032 --- /dev/null +++ b/gripper_controllers/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,59 @@ +gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + validation: { + gt_eq: [ 0.1 ] + }, + } + joint: { + type: string, + default_value: "", + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + gt_eq: [ 0.0 ] + }, + } + max_effort: { + type: double, + default_value: 0.0, + description: "Max allowable effort", + validation: { + gt_eq: [ 0.0 ] + }, + } + allow_stalling: { + type: bool, + description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", + default_value: false, + } + stall_velocity_threshold: { + type: double, + description: "stall velocity threshold", + default_value: 0.001, + } + stall_timeout: { + type: double, + 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 ] + }, + } + 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 ] + }, + } diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 7aadfe5fd1..c0cccb62fa 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -15,6 +15,7 @@ effort_controllers force_torque_sensor_broadcaster forward_command_controller + gripper_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller