Skip to content

Commit

Permalink
Fix gripper speed and force parameter initialization and scaling (#77)
Browse files Browse the repository at this point in the history 
* Expose gripper_speed_multiplier, gripper_force_multiplier, gripper_max_speed, gripper_max_force, and gripper_closed_position parameters to xacro

---------

Co-authored-by: Ashwin Varghese Kuruttukulam <ashwinvk@nvidia.com>
  • Loading branch information
@JafarAbdi @ashwinvkNV
JafarAbdi and ashwinvkNV authored Mar 5, 2025
1 parent bb68ae5 commit d41285d
Show file tree
Hide file tree
Showing 6 changed files with 67 additions and 25 deletions.
17 changes: 12 additions & 5 deletions robotiq_description/urdf/2f_140.ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,12 @@
isaac_joint_states:=/isaac_joint_states
use_fake_hardware:=false
mock_sensor_commands:=false
com_port:=/dev/ttyUSB0">
com_port:=/dev/ttyUSB0
gripper_speed_multiplier:=1.0
gripper_force_multiplier:=0.5
gripper_max_speed:=0.250
gripper_max_force:=125.0
gripper_closed_position:=0.695">

<ros2_control name="${name}" type="system">
<!-- Plugins -->
Expand All @@ -30,10 +35,12 @@
</xacro:if>
<xacro:unless value="${use_fake_hardware or sim_gazebo or sim_isaac}">
<plugin>robotiq_driver/RobotiqGripperHardwareInterface</plugin>
<param name="gripper_closed_position">0.695</param>
<param name="gripper_closed_position">${gripper_closed_position}</param>
<param name="COM_port">${com_port}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="gripper_speed_multiplier">${gripper_speed_multiplier}</param>
<param name="gripper_force_multiplier">${gripper_force_multiplier}</param>
<param name="gripper_max_speed">${gripper_max_speed}</param>
<param name="gripper_max_force">${gripper_max_force}</param>
</xacro:unless>
</hardware>

Expand All @@ -42,7 +49,7 @@
<joint name="${prefix}finger_joint">
<command_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.695</param>
<param name="initial_value">${gripper_closed_position}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
Expand Down
17 changes: 12 additions & 5 deletions robotiq_description/urdf/2f_85.ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,12 @@
isaac_joint_states:=/isaac_joint_states
use_fake_hardware:=false
mock_sensor_commands:=false
com_port:=/dev/ttyUSB0">
com_port:=/dev/ttyUSB0
gripper_speed_multiplier:=1.0
gripper_force_multiplier:=0.5
gripper_max_speed:=0.150
gripper_max_force:=235.0
gripper_closed_position:=0.7929">

<ros2_control name="${name}" type="system">
<!-- Plugins -->
Expand All @@ -35,10 +40,12 @@
</xacro:if>
<xacro:unless value="${use_fake_hardware or sim_gazebo or sim_isaac}">
<plugin>robotiq_driver/RobotiqGripperHardwareInterface</plugin>
<param name="gripper_closed_position">0.7929</param>
<param name="gripper_closed_position">${gripper_closed_position}</param>
<param name="COM_port">${com_port}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="gripper_speed_multiplier">${gripper_speed_multiplier}</param>
<param name="gripper_force_multiplier">${gripper_force_multiplier}</param>
<param name="gripper_max_speed">${gripper_max_speed}</param>
<param name="gripper_max_force">${gripper_max_force}</param>
</xacro:unless>
</hardware>

Expand All @@ -47,7 +54,7 @@
<joint name="${prefix}robotiq_85_left_knuckle_joint">
<command_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.7929</param>
<param name="initial_value">${gripper_closed_position}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
Expand Down
14 changes: 12 additions & 2 deletions robotiq_description/urdf/robotiq_2f_140_macro.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,12 @@
use_fake_hardware:=false
mock_sensor_commands:=false
include_ros2_control:=true
com_port:=/dev/ttyUSB0">
com_port:=/dev/ttyUSB0
gripper_speed_multiplier:=1.0
gripper_force_multiplier:=0.5
gripper_max_speed:=0.250
gripper_max_force:=125.0
gripper_closed_position:=0.695">

<!-- ros2 control include -->
<xacro:include filename="$(find robotiq_description)/urdf/2f_140.ros2_control.xacro" />
Expand All @@ -26,7 +31,12 @@
isaac_joint_states="${isaac_joint_states}"
use_fake_hardware="${use_fake_hardware}"
mock_sensor_commands="${mock_sensor_commands}"
com_port="${com_port}"/>
com_port="${com_port}"
gripper_speed_multiplier="${gripper_speed_multiplier}"
gripper_force_multiplier="${gripper_force_multiplier}"
gripper_max_speed="${gripper_max_speed}"
gripper_max_force="${gripper_max_force}"
gripper_closed_position="${gripper_closed_position}"/>
</xacro:if>

<!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
Expand Down
15 changes: 13 additions & 2 deletions robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,13 @@
use_fake_hardware:=false
mock_sensor_commands:=false
include_ros2_control:=true
com_port:=/dev/ttyUSB0">
com_port:=/dev/ttyUSB0
gripper_speed_multiplier:=1.0
gripper_force_multiplier:=0.5
gripper_max_speed:=0.150
gripper_max_force:=235.0
gripper_closed_position:=0.7929">


<!-- ros2 control include -->
<xacro:include filename="$(find robotiq_description)/urdf/2f_85.ros2_control.xacro" />
Expand All @@ -26,7 +32,12 @@
isaac_joint_states="${isaac_joint_states}"
use_fake_hardware="${use_fake_hardware}"
mock_sensor_commands="${mock_sensor_commands}"
com_port="${com_port}"/>
com_port="${com_port}"
gripper_speed_multiplier="${gripper_speed_multiplier}"
gripper_force_multiplier="${gripper_force_multiplier}"
gripper_max_speed="${gripper_max_speed}"
gripper_max_force="${gripper_max_force}"
gripper_closed_position="${gripper_closed_position}"/>
</xacro:if>

<link name="${prefix}robotiq_85_base_link">
Expand Down
2 changes: 2 additions & 0 deletions robotiq_driver/include/robotiq_driver/hardware_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::quiet_NaN();

Expand Down
27 changes: 16 additions & 11 deletions robotiq_driver/src/hardware_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,12 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons

// Read parameters.
gripper_closed_pos_ = stod(info_.hardware_parameters["gripper_closed_position"]);

gripper_max_speed_ = info_.hardware_parameters.count("gripper_max_speed") ?
stod(info_.hardware_parameters["gripper_max_speed"]) :
kGripperMaxSpeed;
gripper_max_force_ = info_.hardware_parameters.count("gripper_max_force") ?
stod(info_.hardware_parameters["gripper_max_force"]) :
kGripperMaxforce;
gripper_position_ = std::numeric_limits<double>::quiet_NaN();
gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
Expand Down Expand Up @@ -192,15 +197,15 @@ std::vector<hardware_interface::CommandInterface> 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_ = kGripperMaxSpeed * (info_.hardware_parameters.count("gripper_speed_multiplier") ?
std::stod(info_.hardware_parameters.at("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;
gripper_force_ = kGripperMaxforce * (info_.hardware_parameters.count("gripper_force_multiplier") ?
std::stod(info_.hardware_parameters.at("gripper_force_multiplier")) :
1.0);

command_interfaces.emplace_back(
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
Expand Down Expand Up @@ -294,10 +299,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));
const auto gripper_speed_multiplier = std::clamp(fabs(gripper_speed_) / gripper_max_speed_, 0.0, 1.0);
write_speed_.store(uint8_t(gripper_speed_multiplier * 0xFF));
const auto 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;
}
Expand Down

0 comments on commit d41285d

Please # to comment.