diff --git a/robotiq_description/urdf/2f_140.ros2_control.xacro b/robotiq_description/urdf/2f_140.ros2_control.xacro index 80c4294..ad52307 100644 --- a/robotiq_description/urdf/2f_140.ros2_control.xacro +++ b/robotiq_description/urdf/2f_140.ros2_control.xacro @@ -10,7 +10,12 @@ isaac_joint_states:=/isaac_joint_states use_fake_hardware:=false fake_sensor_commands:=false - com_port:=/dev/ttyUSB0"> + com_port:=/dev/ttyUSB0 + gripper_speed:=0.150 + gripper_force:=235.0 + gripper_max_speed:=0.150 + gripper_max_force:=235.0 + gripper_closed_position:=0.695"> @@ -30,10 +35,12 @@ robotiq_driver/RobotiqGripperHardwareInterface - 0.695 + ${gripper_closed_position} ${com_port} - 1.0 - 0.5 + ${gripper_speed} + ${gripper_force} + ${gripper_max_speed} + ${gripper_max_force} @@ -42,7 +49,7 @@ - 0.695 + ${gripper_closed_position} diff --git a/robotiq_description/urdf/2f_85.ros2_control.xacro b/robotiq_description/urdf/2f_85.ros2_control.xacro index 96d3969..67934ad 100644 --- a/robotiq_description/urdf/2f_85.ros2_control.xacro +++ b/robotiq_description/urdf/2f_85.ros2_control.xacro @@ -10,7 +10,12 @@ isaac_joint_states:=/isaac_joint_states use_fake_hardware:=false fake_sensor_commands:=false - com_port:=/dev/ttyUSB0"> + com_port:=/dev/ttyUSB0 + gripper_speed:=0.150 + gripper_force:=235.0 + gripper_max_speed:=0.150 + gripper_max_force:=235.0 + gripper_closed_position:=0.7929"> @@ -35,10 +40,12 @@ robotiq_driver/RobotiqGripperHardwareInterface - 0.7929 + ${gripper_closed_position} ${com_port} - 1.0 - 0.5 + ${gripper_speed} + ${gripper_force} + ${gripper_max_speed} + ${gripper_max_force} @@ -47,7 +54,7 @@ - 0.7929 + ${gripper_closed_position} diff --git a/robotiq_description/urdf/robotiq_2f_140_macro.urdf.xacro b/robotiq_description/urdf/robotiq_2f_140_macro.urdf.xacro index 9a2cc7d..5a70923 100644 --- a/robotiq_description/urdf/robotiq_2f_140_macro.urdf.xacro +++ b/robotiq_description/urdf/robotiq_2f_140_macro.urdf.xacro @@ -12,7 +12,12 @@ use_fake_hardware:=false fake_sensor_commands:=false include_ros2_control:=true - com_port:=/dev/ttyUSB0"> + com_port:=/dev/ttyUSB0 + gripper_speed:=0.150 + gripper_force:=235.0 + gripper_max_speed:=0.150 + gripper_max_force:=235.0 + gripper_closed_position:=0.695"> @@ -26,7 +31,12 @@ isaac_joint_states="${isaac_joint_states}" use_fake_hardware="${use_fake_hardware}" fake_sensor_commands="${fake_sensor_commands}" - com_port="${com_port}"/> + com_port="${com_port}" + gripper_speed="${gripper_speed}" + gripper_force="${gripper_force}" + gripper_max_speed="${gripper_max_speed}" + gripper_max_force="${gripper_max_force}" + gripper_closed_position="${gripper_closed_position}"/> diff --git a/robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro b/robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro index 7ec0918..8c2589d 100644 --- a/robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro +++ b/robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro @@ -12,7 +12,13 @@ use_fake_hardware:=false fake_sensor_commands:=false include_ros2_control:=true - com_port:=/dev/ttyUSB0"> + com_port:=/dev/ttyUSB0 + gripper_speed:=0.150 + gripper_force:=235.0 + gripper_max_speed:=0.150 + gripper_max_force:=235.0 + gripper_closed_position:=0.7929"> + @@ -26,7 +32,12 @@ isaac_joint_states="${isaac_joint_states}" use_fake_hardware="${use_fake_hardware}" fake_sensor_commands="${fake_sensor_commands}" - com_port="${com_port}"/> + com_port="${com_port}" + gripper_speed="${gripper_speed}" + gripper_force="${gripper_force}" + gripper_max_speed="${gripper_max_speed}" + gripper_max_force="${gripper_max_force}" + gripper_closed_position="${gripper_closed_position}"/> diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 15030a3..517249a 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -141,6 +141,8 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa void background_task(); double gripper_closed_pos_ = 0.0; + double gripper_max_speed_ = 0.0; + double gripper_max_force_ = 0.0; static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN(); diff --git a/robotiq_driver/src/default_driver_factory.cpp b/robotiq_driver/src/default_driver_factory.cpp index 05d66b5..aa15906 100644 --- a/robotiq_driver/src/default_driver_factory.cpp +++ b/robotiq_driver/src/default_driver_factory.cpp @@ -43,10 +43,10 @@ const auto kLogger = rclcpp::get_logger("DefaultDriverFactory"); constexpr auto kSlaveAddressParamName = "slave_address"; constexpr uint8_t kSlaveAddressParamDefault = 0x09; -constexpr auto kGripperSpeedMultiplierParamName = "gripper_speed_multiplier"; +constexpr auto kGripperSpeedMultiplierParamName = "gripper_speed"; constexpr double kGripperSpeedMultiplierParamDefault = 1.0; -constexpr auto kGripperForceMultiplierParamName = "gripper_force_multiplier"; +constexpr auto kGripperForceMultiplierParamName = "gripper_force"; constexpr double kGripperForceMultiplierParamDefault = 1.0; constexpr auto kUseDummyParamName = "use_dummy"; diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 07a3af0..cb9f3be 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -42,10 +42,11 @@ const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); +constexpr double kDefaultGripperClosedPos = 0.695; constexpr uint8_t kGripperMinPos = 3; constexpr uint8_t kGripperMaxPos = 230; -constexpr double kGripperMaxSpeed = 0.150; // mm/s -constexpr double kGripperMaxforce = 235; // N +constexpr double kDefaultGripperMaxSpeed = 0.150; // mm/s +constexpr double kDefaultGripperMaxforce = 235.0; // N constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; constexpr auto kGripperCommsLoopPeriod = std::chrono::milliseconds{ 10 }; @@ -82,8 +83,15 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons } // Read parameters. - gripper_closed_pos_ = stod(info_.hardware_parameters["gripper_closed_position"]); - + gripper_closed_pos_ = info_.hardware_parameters.count("gripper_closed_position") ? + stod(info_.hardware_parameters["gripper_closed_position"]) : + kDefaultGripperClosedPos; + gripper_max_speed_ = info_.hardware_parameters.count("gripper_max_speed") ? + stod(info_.hardware_parameters["gripper_max_speed"]) : + kDefaultGripperMaxSpeed; + gripper_max_force_ = info_.hardware_parameters.count("gripper_max_force") ? + stod(info_.hardware_parameters["gripper_max_force"]) : + kDefaultGripperMaxforce; gripper_position_ = std::numeric_limits::quiet_NaN(); gripper_velocity_ = std::numeric_limits::quiet_NaN(); gripper_position_command_ = std::numeric_limits::quiet_NaN(); @@ -192,15 +200,14 @@ std::vector RobotiqGripperHardwareInterfac 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; - + gripper_speed_ = info_.hardware_parameters.count("gripper_speed") ? + stod(info_.hardware_parameters["gripper_speed"]) : + gripper_max_speed_; 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; + gripper_force_ = info_.hardware_parameters.count("gripper_force") ? + stod(info_.hardware_parameters["gripper_force"]) : + gripper_max_force_; command_interfaces.emplace_back( hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); @@ -294,10 +301,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)); + double gripper_speed_multiplier = std::clamp(fabs(gripper_speed_) / gripper_max_speed_, 0.0, 1.0); + write_speed_.store(uint8_t(gripper_speed_multiplier * 0xFF)); + double gripper_force_multiplier = std::clamp(fabs(gripper_force_) / gripper_max_force_, 0.0, 1.0); + write_force_.store(uint8_t(gripper_force_multiplier * 0xFF)); return hardware_interface::return_type::OK; } diff --git a/robotiq_driver/tests/test_default_driver_factory.cpp b/robotiq_driver/tests/test_default_driver_factory.cpp index b0a9913..e4e44c2 100644 --- a/robotiq_driver/tests/test_default_driver_factory.cpp +++ b/robotiq_driver/tests/test_default_driver_factory.cpp @@ -86,8 +86,8 @@ TEST(TestDefaultDriverFactory, create_with_given_parameters) hardware_interface::HardwareInfo info; info.hardware_parameters.emplace("slave_address", "1"); - info.hardware_parameters.emplace("gripper_speed_multiplier", "0.5"); - info.hardware_parameters.emplace("gripper_force_multiplier", "0.5"); + info.hardware_parameters.emplace("gripper_speed", "0.5"); + info.hardware_parameters.emplace("gripper_force", "0.5"); auto driver = std::make_unique(); diff --git a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp index 7b058fd..7a5da50 100644 --- a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp +++ b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp @@ -59,8 +59,8 @@ TEST(TestRobotiqGripperHardwareInterface, load_urdf) robotiq_driver/RobotiqGripperHardwareInterface - 1.0 - 0.5 + 1.0 + 0.5 /dev/ttyUSB0 0.7929