3. Expose max_speed and max_force parameters to xacro

Author: JafarAbdi

robotiq_description/urdf/2f_140.ros2_control.xacro

@@ -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 -->
@@ -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>
 
@@ -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>

robotiq_description/urdf/2f_85.ros2_control.xacro

@@ -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 -->
@@ -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>
 
@@ -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>

robotiq_description/urdf/robotiq_2f_140_macro.urdf.xacro

@@ -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" />
@@ -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 -->

robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro

@@ -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" />
@@ -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">

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<double>::quiet_NaN();
 

robotiq_driver/src/hardware_interface.cpp

@@ -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();