diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml index 987a43d..fb445f4 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml @@ -1,8 +1,9 @@ robotiq_2f_gripper: # Physical limits - min_position: 0.0 # radiant (fully open) - max_position: 0.695 # radiant (fully open) + min_position: 0.0 # Meters (fully closed) + max_position: 0.14 # Meters (fully open) + max_angle: 0.695 # radiants (closed) min_speed: 0.02 # Meters per second - max_speed: 0.15 # Meters per second + max_speed: 0.5 # Meters per second min_force: 20.0 # Newtons max_force: 235.0 # Newtons \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml index 62b551f..215d737 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml @@ -3,42 +3,14 @@ controller_manager: update_rate: 500 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - robotiq_gripper_controller: - type: position_controllers/GripperActionController - fake_gripper_controller: - type: joint_trajectory_controller/JointTrajectoryController forward_gripper_position_controller: type: robotiq_2f_controllers/RobotiqForwardController robotiq_activation_controller: type: robotiq_controllers/RobotiqActivationController -robotiq_gripper_controller: - ros__parameters: - default: true - joint: finger_joint - use_effort_interface: true - use_speed_interface: true - forward_gripper_position_controller: ros__parameters: - joint: finger_joint - -fake_gripper_controller: - ros__parameters: - joints: - - finger_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.0 - finger_joint: { trajectory: 0.2, goal: 0.1 } + joint: gripper_gap robotiq_activation_controller: ros__parameters: diff --git a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py index 4a11ebd..46f9a1a 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py +++ b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py @@ -118,13 +118,11 @@ def generate_launch_description(): ) if not use_fake_hardware_bool: - controller_spawner_names = ["robotiq_gripper_controller"] - controller_spawner_inactive_names = ["forward_gripper_position_controller", - "fake_gripper_controller"] + controller_spawner_names = [] + controller_spawner_inactive_names = ["forward_gripper_position_controller"] else: controller_spawner_names = ["forward_gripper_position_controller"] - controller_spawner_inactive_names = ["robotiq_gripper_controller", - "fake_gripper_controller"] + controller_spawner_inactive_names = [] controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [ controller_spawner(name, active=False) for name in controller_spawner_inactive_names diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro index 63f9236..06480b1 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro @@ -10,6 +10,7 @@ robot_port min_position max_position + max_angle min_speed max_speed min_force @@ -35,6 +36,7 @@ 0.5 ${min_position} ${max_position} + ${max_angle} ${min_speed} ${max_speed} ${min_force} @@ -43,12 +45,17 @@ - + + + 0.0 + + + + 0.695 - diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro index 9a15bff..9195619 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro @@ -17,6 +17,7 @@ + @@ -33,6 +34,7 @@ robot_port="${robot_port}" min_position="${min_position}" max_position="${max_position}" + max_angle="${max_angle}" min_speed="${min_speed}" max_speed="${max_speed}" min_force="${min_force}" @@ -433,5 +435,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp index 4e3e0ab..f0cafa2 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp @@ -67,8 +67,9 @@ protected: // Likely changes the access to protected from private static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN(); // Conversion Methods - double convertRawToPosition(int raw_position); - int convertPositionToRaw(double position); + double convertRawToGap(int raw_position); + double convertRawToAngle(int raw_position); + int convertGapToRaw(double gap); int convertSpeedToRaw(double speed); int convertForceToRaw(double force); @@ -80,9 +81,10 @@ protected: // Likely changes the access to protected from private // Gripper State Tracking - double gripper_position_ = 0.0; + double gripper_gap_ = 0.0; + double gripper_angle_ = 0.0; double gripper_velocity_ = 0.0; - double gripper_position_command_ = 0.0; + double gripper_gap_command_ = 0.0; rclcpp::Clock::SharedPtr clock_ = std::make_shared(RCL_ROS_TIME); rclcpp::Time last_update_time_ = clock_ -> now(); @@ -102,6 +104,7 @@ protected: // Likely changes the access to protected from private // Parameters for physical limits and configuration double min_position_; double max_position_; + double max_angle_; double min_speed_; double max_speed_; double min_force_; diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp index 048a871..0c7039f 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -18,6 +18,9 @@ const double min_position_default = 0.01; const std::string max_position_parameter_name = "max_position"; const double max_position_default = 0.0; +const std::string max_angle_parameter_name = "max_angle"; +const double max_angle_default = 0.5; + const std::string min_speed_parameter_name = "min_speed"; const double min_speed_default = 0.0; @@ -96,6 +99,14 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons } RCLCPP_INFO(logger_, "Accessing parameter: max position, Value: %f", max_position_); + if (info.hardware_parameters.count(max_angle_parameter_name) > 0) { + max_angle_ = std::stod(info.hardware_parameters.at(max_angle_parameter_name)); + } else { + RCLCPP_WARN(logger_, "max angle not found falling back to default value!"); + max_angle_ = max_angle_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: max angle , Value: %f", max_angle_); + if (info.hardware_parameters.count(min_speed_parameter_name) > 0) { min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name)); } else { @@ -130,49 +141,69 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons return CallbackReturn::ERROR; } - gripper_position_ = std::numeric_limits::quiet_NaN(); + gripper_gap_ = std::numeric_limits::quiet_NaN(); + gripper_angle_ = std::numeric_limits::quiet_NaN(); gripper_velocity_ = std::numeric_limits::quiet_NaN(); - gripper_position_command_ = std::numeric_limits::quiet_NaN(); + gripper_gap_command_ = std::numeric_limits::quiet_NaN(); reactivate_gripper_cmd_ = NO_NEW_CMD_; reactivate_gripper_async_cmd_.store(false); - const hardware_interface::ComponentInfo& joint = info_.joints[0]; + const hardware_interface::ComponentInfo& joint_gap = info_.joints[0]; + const hardware_interface::ComponentInfo& joint_angle = info_.joints[1]; // There is one command interface: position. - if (joint.command_interfaces.size() != 1) + if (joint_gap.command_interfaces.size() != 1) { - RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint_gap.name.c_str(), + joint_gap.command_interfaces.size()); return CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + if (joint_gap.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint_gap.name.c_str(), + joint_gap.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return CallbackReturn::ERROR; } - // There are two state interfaces: position and velocity. - if (joint.state_interfaces.size() != 2) + // There are two state interfaces for the gripper gap: position and velocity. + if (joint_gap.state_interfaces.size() != 2) { - RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), - joint.state_interfaces.size()); + RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint_gap.name.c_str(), + joint_gap.state_interfaces.size()); return CallbackReturn::ERROR; } for (int i = 0; i < 2; ++i) { - if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION || - joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) + if (!(joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_POSITION || + joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) { - RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(), - joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION, + RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_gap.name.c_str(), + joint_gap.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); return CallbackReturn::ERROR; } } + // There are two state interfaces for the gripper angle: position. + if (joint_angle.state_interfaces.size() != 1) + { + RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 1 expected.", joint_angle.name.c_str(), + joint_angle.state_interfaces.size()); + return CallbackReturn::ERROR; + } + + if (!(joint_angle.state_interfaces[0].name == hardware_interface::HW_IF_POSITION)) + { + RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_angle.name.c_str(), + joint_angle.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY); + return CallbackReturn::ERROR; + } + + + return CallbackReturn::SUCCESS; } @@ -207,11 +238,12 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( RCLCPP_DEBUG(logger_, "on_activate"); // set some default values for joints - if (std::isnan(gripper_position_)) + if (std::isnan(gripper_gap_)) { - gripper_position_ = 0; - gripper_velocity_ = 0; - gripper_position_command_ = 0; + gripper_gap_ = 0.0; + gripper_angle_ = 0.0; + gripper_velocity_ = 0.0; + gripper_gap_command_ = 0.0; } // Activate the gripper. @@ -272,14 +304,15 @@ std::vector RobotiqGripperHardwareInterface: std::vector state_interfaces; state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_)); + hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_gap_)); state_interfaces.emplace_back( hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); - + state_interfaces.emplace_back( + hardware_interface::StateInterface(info_.joints[1].name, hardware_interface::HW_IF_POSITION, &gripper_angle_)); for (const auto& interface : state_interfaces) { - RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s", + RCLCPP_INFO(logger_, "Exporting state interface for joint: %s type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); -} + } return state_interfaces; } @@ -292,7 +325,7 @@ std::vector RobotiqGripperHardwareInterfac command_interfaces.emplace_back( hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_ + info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_gap_command_ ) ); @@ -337,16 +370,18 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc try { int raw_position = socket_adapter_->position(); - double new_position = convertRawToPosition(raw_position); + double new_gap = convertRawToGap(raw_position); + double new_angle = convertRawToAngle(raw_position); - if (!std::isnan(gripper_position_)) { + if (!std::isnan(gripper_gap_)) { double time_difference = (current_time_ - last_update_time_).seconds(); if (time_difference > 0) { - gripper_velocity_ = (new_position - gripper_position_) / time_difference; + gripper_velocity_ = (new_gap - gripper_gap_) / time_difference; } } - gripper_position_ = new_position; + gripper_gap_ = new_gap; + gripper_angle_ = new_angle; last_update_time_ = current_time_; } catch (const std::exception& e) { @@ -375,12 +410,12 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl { try { // Use the conversion methods to scale the command values to the range expected by the hardware - int scaled_position = convertPositionToRaw(gripper_position_command_); + int scaled_gap = convertGapToRaw(gripper_gap_command_); int scaled_speed = convertSpeedToRaw(gripper_speed_); int scaled_force = convertForceToRaw(gripper_force_); // Store the scaled values into atomic variables - write_command_.store(scaled_position); + write_command_.store(scaled_gap); write_speed_.store(scaled_speed); write_force_.store(scaled_force); @@ -425,17 +460,17 @@ void RobotiqGripperHardwareInterface::performRegularOperations() { try { - int scaled_position = write_command_.load(); + int scaled_gap = write_command_.load(); int scaled_speed = write_speed_.load(); int scaled_force = write_force_.load(); - auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force); + auto result = socket_adapter_->move(scaled_gap, scaled_speed, scaled_force); if (!std::get<0>(result)) { throw std::runtime_error("Failed to send move command to Robotiq gripper."); } - int raw_position = socket_adapter_->position(); - gripper_current_state_.store(convertRawToPosition(raw_position)); + int raw_gap = socket_adapter_->position(); + gripper_current_state_.store(convertRawToGap(raw_gap)); } catch (const std::exception& e) { @@ -457,19 +492,27 @@ void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) { // Conversion methods -double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) { +double RobotiqGripperHardwareInterface::convertRawToGap(int raw_position) { if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) { throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly."); } - double scaled_position = min_position_ + (static_cast(raw_position) / 255.0) * (max_position_ - min_position_); + double scaled_position = max_position_ - (static_cast(raw_position) / 255.0) * (max_position_ - min_position_); return std::max(min_position_, std::min(scaled_position, max_position_)); } -int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) { +double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) { + if (std::isnan(max_angle_) || 0.0 >= max_angle_) { + throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly."); + } + double scaled_angle = (static_cast(raw_position) / 255.0) * max_angle_; + return std::max(0.0, std::min(scaled_angle, max_angle_)); +} + +int RobotiqGripperHardwareInterface::convertGapToRaw(double gap) { if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) { throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly."); } - double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0; + double scaled = 255.0 - (gap - min_position_) / (max_position_ - min_position_) * 255.0; return static_cast(std::clamp(scaled, 0.0, 255.0)); }