fix(panda_control_server): fix gasping behavoir

This commit ensures that the gripper always ignores the gripper_width
when grasping. This was done since we currently don't use the
`frank_gripper/grasp` action service to grasp objects based on their
width.

BREAKING CHANGE: brute_force_grasping is depricated

\#33

panda_gazebo/launch/put_robot_in_world.launch

@@ -29,7 +29,6 @@
   <!--Control arguments-->
   <!--  The control type used for controlling the robot (Options: Trajectory, position, effort)-->
   <arg name="control_type" default="trajectory" doc="The type of control used for controlling the arm. Options: trajectory, position, effort"/>
-  <arg name="brute_force_grasping" default="false" doc="Disable the gripper width reached check when grasping."/>
 
   <!--Start Gazebo and spawn the robot-->
   <include file="$(find franka_gazebo)/launch/panda.launch">
@@ -90,7 +89,6 @@
   <arg unless="$(eval arg('control_type') == 'trajectory')" name="load_set_joint_commands_service" value="true"/>
   <arg unless="$(eval arg('control_type') == 'trajectory')" name="load_arm_follow_joint_trajectory_action" value="false"/>
   <node pkg="panda_gazebo" type="panda_control_server.py" name="panda_control_server" required="true" output="$(arg output)">
-    <param name="brute_force_grasping" value="$(arg brute_force_grasping)"/>
     <param name="load_gripper" value="$(arg load_gripper)"/>
     <param name="load_set_joint_commands_service" value="$(arg load_set_joint_commands_service)"/>
     <param name="load_arm_follow_joint_trajectory_action" value="$(arg load_arm_follow_joint_trajectory_action)"/>

panda_gazebo/nodes/panda_control_server.py

@@ -42,10 +42,6 @@
         load_extra_services = rospy.get_param("~load_extra_services")
     except KeyError:
         load_extra_services = False
-    try:  # Disables the gripper width reached check when grasping.
-        brute_force_grasping = rospy.get_param("~brute_force_grasping")
-    except KeyError:
-        brute_force_grasping = False
     try:  # The rate with which we check for the used controllers to be active.
         controllers_check_rate = rospy.get_param("~controllers_check_rate")
     except KeyError:
@@ -58,7 +54,6 @@
         load_set_joint_commands_service=load_set_joint_commands_service,
         load_arm_follow_joint_trajectory_action=load_arm_follow_joint_trajectory_action,
         load_extra_services=load_extra_services,
-        brute_force_grasping=brute_force_grasping,
         controllers_check_rate=controllers_check_rate,
     )
     rospy.spin()  # Maintain the service open.

panda_gazebo/src/panda_gazebo/core/control_server.py

@@ -113,7 +113,6 @@ def __init__(  # noqa: C901
         load_set_joint_commands_service=True,
         load_arm_follow_joint_trajectory_action=False,
         load_extra_services=False,
-        brute_force_grasping=False,
         controllers_check_rate=CONTROLLER_INFO_RATE,
     ):
         """Initialise PandaControlServer object.
@@ -135,8 +134,6 @@ def __init__(  # noqa: C901
             load_extra_services (bool, optional): Whether to load extra services that
                 are not used by the :ros-gazebo-gym:`ros_gazebo_gym <>` package.
                 Defaults to ``False``.
-            brute_force_grasping (bool, optional): Disable the gripper width reached
-                check when grasping. Defaults to ``False``.
             controllers_check_rate (float, optional): Rate at which the availability of
                 the used controllers is checked. Setting this to ``-1`` will check at
                 every time step. Defaults to ``0.1`` Hz.
@@ -158,10 +155,9 @@ def __init__(  # noqa: C901
         self.__controlled_joints = {}
         self.__joint_controllers = {}
 
-        # Disable the gripper width reached check used in the.
-        # franka_gripper/gripper_action` when the `max_effort` is bigger than 0.0.
-        if brute_force_grasping:
-            rospy.set_param("/franka_gripper/gripper_action/width_tolerance", 0.1)
+        # Disable the `gripper_action` gripper width reached check.
+        # NOTE: Done to allow grasping with the `gripper_action` service (see #33).
+        rospy.set_param("/franka_gripper/gripper_action/width_tolerance", 0.1)
 
         ########################################
         # Create Panda control services ########
@@ -1640,26 +1636,26 @@ def _set_gripper_width_cb(self, set_gripper_width_req):
         """
         resp = SetGripperWidthResponse()
 
-        # Check if gripper width is within boundaries.
-        set_gripper_width_req.width = (
-            set_gripper_width_req.width / 2
-        )  # NOTE: Done because the gripper command action takes in the joints positions
-        if set_gripper_width_req.width < 0.0 or set_gripper_width_req.width > 0.08:
+        # Check if gripper width is within boundaries and convert to finger position.
+        set_gripper_width_req.width /= 2
+        gripper_width = np.clip(set_gripper_width_req.width, 0, 0.08)
+        if gripper_width != set_gripper_width_req.width:
             rospy.logwarn(
-                "Gripper width was clipped as it was not within bounds [0, 0.8]."
+                "Gripper width was clipped as it was not within bounds [0, 0.08]."
             )
-            gripper_width = np.clip(set_gripper_width_req.width, 0, 0.08)
-        else:
-            gripper_width = set_gripper_width_req.width
 
-        # Create gripper command message.
+        # Create gripper command action message.
+        # NOTE: The max_effort has to be 0 for the gripper to move (see #33).
         req = GripperCommandGoal()
         req.command.position = gripper_width
         req.command.max_effort = (
-            set_gripper_width_req.max_effort
-            if set_gripper_width_req.max_effort != 0.0
-            and set_gripper_width_req.grasping
-            else (GRASP_FORCE if set_gripper_width_req.grasping else 0.0)
+            (
+                set_gripper_width_req.max_effort
+                if set_gripper_width_req.max_effort != 0.0
+                else GRASP_FORCE
+            )
+            if set_gripper_width_req.grasping
+            else 0.0
         )
 
         # Invoke 'franka_gripper' action service
@@ -1668,10 +1664,9 @@ def _set_gripper_width_cb(self, set_gripper_width_req):
 
             # Wait for result.
             if set_gripper_width_req.wait:
-                gripper_resp = self._gripper_command_client.wait_for_result(
+                resp.success = self._gripper_command_client.wait_for_result(
                     timeout=rospy.Duration.from_sec(WAIT_TILL_DONE_TIMEOUT)
                 )
-                resp.success = gripper_resp
             else:
                 resp.success = True
         else:
@@ -1705,7 +1700,9 @@ def _get_controlled_joints_cb(self, get_controlled_joints_req):
         resp.success = True
         resp.message = "Everything went OK"
         try:
-            self.__controllers = {}  # Make sure latest controller info is retrieved.
+            self.__controllers = (
+                {}
+            )  # Make sure latest controller info is retrieved. # TODO: Doesn't work!
             controlled_joints = self.controlled_joints[
                 get_controlled_joints_req.control_type
             ]

panda_gazebo/srv/SetGripperWidth.srv

@@ -2,10 +2,10 @@
 # NOTE: It serves as a small wrapper around the 'franka_gripper/move` action but automatically
 # sets the speed to the maximum speed. It further clips gripper width such that it is within
 # the set max/min boundaries.
-float64 width
-bool grasping # The gripper simply moves if this is `false`.
-float64 max_effort # The max effort used by the gripper.
+float64 width       # Gripper width - ignored when the gripper is grasping.
+bool grasping       # The gripper simply moves if this is `false`.
+float64 max_effort  # The max effort used by the gripper.
 bool wait
 ---
 bool success
-string message
+string message

panda_gazebo/tests/manual/panda_control_server_test.py

@@ -207,7 +207,7 @@
 
     # Without grasp.
     req = SetGripperWidthRequest()
-    req.width = 0.04
+    req.width = 0.08
     req.wait = True
     set_gripper_width_srv = rospy.ServiceProxy(
         "/panda_control_server/panda_hand/set_gripper_width",
@@ -224,10 +224,19 @@
     )
     resp = set_gripper_width_srv.call(req)
     print(resp.message)
+    req = SetGripperWidthRequest()
+    req.width = 0.04
+    req.wait = True
+    set_gripper_width_srv = rospy.ServiceProxy(
+        "/panda_control_server/panda_hand/set_gripper_width",
+        SetGripperWidth,
+    )
+    resp = set_gripper_width_srv.call(req)
+    print(resp.message)
 
     # With grasp.
     req = SetGripperWidthRequest()
-    req.width = 0.04
+    req.width = 0.08
     req.wait = True
     req.grasping = True
     set_gripper_width_srv = rospy.ServiceProxy(