Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Replace gripper_speed_multiplier/gripper_speed_multiplier with gripper_speed/gripper_force. Fix bug #58

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Replace gripper_speed_multiplier/gripper_speed_multiplier with gripper_speed/gripper_force. Fix bug #58

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 12 additions & 5 deletions robotiq_description/urdf/2f_140.ros2_control.xacro
View file
Open in desktop
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
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">

<ros2_control name="${name}" type="system">
<!-- Plugins -->
Expand All @@ -30,10 +35,12 @@
</xacro:if>
<xacro:unless value="${use_fake_hardware or sim_ignition 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">${gripper_speed}</param>
<param name="gripper_force">${gripper_force}</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
Open in desktop
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
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">

<ros2_control name="${name}" type="system">
<!-- Plugins -->
Expand All @@ -35,10 +40,12 @@
</xacro:if>
<xacro:unless value="${use_fake_hardware or sim_ignition 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">${gripper_speed}</param>
<param name="gripper_force">${gripper_force}</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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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">

<!-- 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}"
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}"/>
</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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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">


<!-- 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}"
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}"/>
</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
Open in desktop
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
4 changes: 2 additions & 2 deletions robotiq_driver/src/default_driver_factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down
37 changes: 22 additions & 15 deletions robotiq_driver/src/hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 };
Expand Down Expand Up @@ -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<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 +200,14 @@ 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_ = 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_));
Expand Down Expand Up @@ -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;
}
Expand Down
4 changes: 2 additions & 2 deletions robotiq_driver/tests/test_default_driver_factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<MockDriver>();

Expand Down
4 changes: 2 additions & 2 deletions robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,8 @@ TEST(TestRobotiqGripperHardwareInterface, load_urdf)
<ros2_control name="robotiq_driver_ros2_control" type="system">
<hardware>
<plugin>robotiq_driver/RobotiqGripperHardwareInterface</plugin>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="gripper_speed">1.0</param>
<param name="gripper_force">0.5</param>
<param name="COM_port">/dev/ttyUSB0</param>
<param name="gripper_closed_position">0.7929</param>
</hardware>
Expand Down
Loading