diff --git a/robotiq_description/config/robotiq_controllers.yaml b/robotiq_description/config/robotiq_controllers.yaml index 8d0b84d..b2677dc 100644 --- a/robotiq_description/config/robotiq_controllers.yaml +++ b/robotiq_description/config/robotiq_controllers.yaml @@ -4,7 +4,7 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster robotiq_gripper_controller: - type: position_controllers/GripperActionController + type: antipodal_gripper_action_controller/GripperActionController robotiq_activation_controller: type: robotiq_controllers/RobotiqActivationController @@ -12,6 +12,8 @@ robotiq_gripper_controller: ros__parameters: default: true joint: robotiq_85_left_knuckle_joint + use_effort_interface: true + use_speed_interface: true robotiq_activation_controller: ros__parameters: diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 3a80499..35dfde2 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -149,11 +149,15 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa double gripper_position_command_; std::atomic write_command_; + std::atomic write_force_; + std::atomic write_speed_; std::atomic gripper_current_state_; double reactivate_gripper_cmd_; std::atomic reactivate_gripper_async_cmd_; double reactivate_gripper_response_; + double gripper_force_; + double gripper_speed_; std::atomic> reactivate_gripper_async_response_; }; diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 2a293cb..07a3af0 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -44,6 +44,8 @@ const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); constexpr uint8_t kGripperMinPos = 3; constexpr uint8_t kGripperMaxPos = 230; +constexpr double kGripperMaxSpeed = 0.150; // mm/s +constexpr double kGripperMaxforce = 235; // N constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; constexpr auto kGripperCommsLoopPeriod = std::chrono::milliseconds{ 10 }; @@ -187,6 +189,19 @@ std::vector RobotiqGripperHardwareInterfac command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_)); + gripper_speed_ = info_.hardware_parameters.count("gripper_speed_multiplier") ? + info_.hardware_parameters.count("gripper_speed_multiplier") : + 1.0; + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_)); + gripper_force_ = info_.hardware_parameters.count("gripper_force_multiplier") ? + info_.hardware_parameters.count("gripper_force_multiplier") : + 1.0; + command_interfaces.emplace_back( hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -279,6 +294,10 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl double gripper_pos = (gripper_position_command_ / gripper_closed_pos_) * kGripperRange + kGripperMinPos; gripper_pos = std::max(std::min(gripper_pos, 255.0), 0.0); write_command_.store(uint8_t(gripper_pos)); + gripper_speed_ = kGripperMaxSpeed * std::clamp(fabs(gripper_speed_) / kGripperMaxSpeed, 0.0, 1.0); + write_speed_.store(uint8_t(gripper_speed_ * 0xFF)); + gripper_force_ = kGripperMaxforce * std::clamp(fabs(gripper_force_) / kGripperMaxforce, 0.0, 1.0); + write_force_.store(uint8_t(gripper_force_ * 0xFF)); return hardware_interface::return_type::OK; } @@ -301,6 +320,8 @@ void RobotiqGripperHardwareInterface::background_task() // Write the latest command to the gripper. this->driver_->set_gripper_position(write_command_.load()); + this->driver_->set_speed(write_speed_.load()); + this->driver_->set_force(write_force_.load()); // Read the state of the gripper. gripper_current_state_.store(this->driver_->get_gripper_position());