refactor: improve default grasping behavoir and codebase

This commit ensures that the `max_effort` is used when it is set in in
the gripper_width service. It also improves the panda_moveit_server
error messages and cleansup the codebase.

panda_gazebo/launch/put_robot_in_world.launch

@@ -6,7 +6,7 @@
   <arg name="debug" default="false" doc="Add gdb debug flag"/>
   <arg name="output" default="screen" doc="output channel of main nodes. options: screen, log"/>
   <arg name="disable_franka_gazebo_logs" default="false" doc="prevents franka gazebo from logging to the stdout"/>
-  <!--Moveit arguments-->
+  <!--MoveIt arguments-->
   <arg name="moveit" default="true" doc="Start MoveIt"/>
   <arg name="rviz_file" default="" doc="Path to the Rviz configuration file"/>
   <arg name="pipeline" default="ompl" doc="MoveIt planning pipeline"/>

panda_gazebo/src/panda_gazebo/common/helpers.py

@@ -8,13 +8,13 @@
 import numpy as np
 import rospy
 from actionlib_msgs.msg import GoalStatusArray
+from controller_manager_msgs.srv import ListControllersResponse
+from geometry_msgs.msg import Quaternion
+from moveit_msgs.msg import MoveItErrorCodes
 from numpy import linalg, nan
 from rospy.exceptions import ROSException
 from sensor_msgs.msg import JointState
 from trajectory_msgs.msg import JointTrajectoryPoint
-from controller_manager_msgs.srv import ListControllersResponse
-from moveit_msgs.msg import MoveItErrorCodes
-from geometry_msgs.msg import Quaternion
 
 from panda_gazebo.msg import FollowJointTrajectoryGoal
 
@@ -237,7 +237,7 @@ def translate_moveit_error_code(moveit_error_code):
         if attr[0] != "_" and all(map(str.isupper, attr.replace("_", "")))
     }
     return (
-        error_dict[moveit_error_code.val].lower().capitalize().replace("_", " ") + "."
+        error_dict[moveit_error_code.val].lower().capitalize().replace("_", " ")
         if error_dict[moveit_error_code.val] != "SUCCESSFUL"
         else ""
     )

panda_gazebo/src/panda_gazebo/core/control_server.py

@@ -1155,6 +1155,11 @@ def _joint_states_cb(self, data):
     def _set_joint_commands_cb(self, set_joint_commands_req):
         """Request arm and hand joint command control.
 
+        .. note::
+            The gripper `max_effort` is determined based on the `grasping` field if it's
+            set to `0`. Specifically, if `grasping` is `True`, `max_effort` is set to
+            10N. Otherwise, it remains at `0`.
+
         Args:
             set_joint_commands_req (:obj:`panda_gazebo.srv.SetJointPositionsRequest`):
                 Service request message specifying the joint position/effort commands
@@ -1444,6 +1449,11 @@ def _arm_set_joint_efforts_cb(self, set_joint_efforts_req):  # noqa: C901
     def _set_gripper_width_cb(self, set_gripper_width_req):
         """Request gripper width.
 
+        .. note::
+            The gripper `max_effort` is determined based on the `grasping` field if it's
+            set to `0`. Specifically, if `grasping` is `True`, `max_effort` is set to
+            10N. Otherwise, it remains at `0`.
+
         Args:
             set_gripper_width_req (:obj:`panda_gazebo.srv.SetGripperWidth`):
                 Service request message specifying the gripper width for the robot hand.
@@ -1462,15 +1472,13 @@ def _set_gripper_width_cb(self, set_gripper_width_req):
             )
 
         # Create gripper command action message.
-        # NOTE: The max_effort has to be 0 for the gripper to move (see #33).
+        # NOTE: The max_effort has to be 0 for the gripper to be able 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
-                else GRASP_FORCE
-            )
+            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
         )

panda_gazebo/src/panda_gazebo/core/moveit_server.py

@@ -47,13 +47,13 @@
     get_duplicate_list,
     get_unique_list,
     joint_state_dict_2_joint_state_msg,
+    load_panda_joint_limits,
     lower_first_char,
     normalize_quaternion,
+    normalize_vector,
     quaternion_norm,
     ros_exit_gracefully,
     translate_moveit_error_code,
-    normalize_vector,
-    load_panda_joint_limits,
 )
 from panda_gazebo.exceptions import InputMessageInvalidError, JointLimitsInvalidError
 from panda_gazebo.srv import (
@@ -120,7 +120,7 @@ def __init__(  # noqa: C901
         Args:
             arm_move_group (str, optional): The name of the move group you want to use
                 for controlling the Panda arm. Defaults to ``panda_arm``.
-            arm_ee_link (str, optional): The end effector you want moveit to use when
+            arm_ee_link (str, optional): The end effector you want MoveIt to use when
                 controlling the Panda arm. Defaults to ``panda_link8``.
             hand_move_group (str, optional): The name of the move group you want to use
                 for controlling the Panda hand. Defaults to ``panda_hand``.
@@ -472,8 +472,11 @@ def _execute(self, control_group="both", wait=True):  # noqa: C901
             wait (boolean, optional): Whether to wait on the control to be executed.
 
         Returns:
-            list: List specifying whether the arm and/or hand execution was successful.
+        (tuple): tuple containing:
+
+            - list: List specifying whether the arm and/or hand execution was successful.
                 If ``control_group == "both"`` then ``["arm_success", "hand_success"]``.
+            - str: The error message, or an empty string if successful.
         """
         control_group = control_group.lower()
         if control_group not in ["arm", "hand", "both"]:
@@ -484,7 +487,7 @@ def _execute(self, control_group="both", wait=True):  # noqa: C901
             return [False]
 
         # Plan and execute trajectory.
-        retval, groups = [], []
+        retval, groups, err_msg = [], [], None
         if control_group in ["arm", "both"]:
             groups.append(self.move_group_arm)
         if control_group in ["hand", "both"]:
@@ -501,14 +504,15 @@ def _execute(self, control_group="both", wait=True):  # noqa: C901
                 if wait:
                     group.stop()
             else:
+                err_msg = translate_moveit_error_code(error_code)
                 rospy.logwarn(
                     f"No plan found for the current {group.get_name()} setpoints "
-                    f"since '{translate_moveit_error_code(error_code)}'."
+                    f"since '{err_msg}'."
                 )
                 group_retval = False
             retval.append(group_retval)
 
-        return retval
+        return retval, err_msg
 
     def _create_positions_moveit_setpoints(  # noqa: C901
         self, set_joint_positions_req, control_group
@@ -1043,7 +1047,7 @@ def _arm_get_ee_pose_joint_config(self, get_ee_pose_joint_configuration):
         # Make sure quaternion is normalized.
         if quaternion_norm(get_ee_pose_joint_configuration.pose.orientation) != 1.0:
             rospy.logwarn(
-                "The quaternion in the set ee pose was normalized since moveit expects "
+                "The quaternion in the set ee pose was normalized since MoveIt expects "
                 "normalized quaternions."
             )
             get_ee_pose_joint_configuration.pose.orientation = normalize_quaternion(
@@ -1101,7 +1105,7 @@ def _arm_set_ee_pose_callback(self, set_ee_pose_req):
         # Make sure quaternion is normalized.
         if quaternion_norm(set_ee_pose_req.pose.orientation) != 1.0:
             rospy.logwarn(
-                "The quaternion in the set ee pose was normalized since moveit expects "
+                "The quaternion in the set ee pose was normalized since MoveIt expects "
                 "normalized quaternions."
             )
             set_ee_pose_req.pose.orientation = normalize_quaternion(
@@ -1116,13 +1120,16 @@ def _arm_set_ee_pose_callback(self, set_ee_pose_req):
         # Send trajectory message and return response.
         try:
             self.move_group_arm.set_pose_target(self.ee_pose_target)
-            retval = self._execute(control_group="arm")
+            retval, err_msg = self._execute(control_group="arm")
             self.move_group_arm.clear_pose_targets()
 
             # Check if setpoint execution was successful.
             if not all(retval):
                 resp.success = False
-                resp.message = "Ee pose could not be set"
+                resp.message = (
+                    "Ee pose could not be set since MoveIt "
+                    f"{lower_first_char(err_msg)}."
+                )
             else:
                 resp.success = True
                 resp.message = "Everything went OK"
@@ -1192,10 +1199,12 @@ def _set_joint_positions_callback(self, set_joint_positions_req):  # noqa: C901
         # Execute setpoints.
         rospy.logdebug("Executing joint positions setpoint.")
         try:
-            retval = self._execute()
+            retval, err_msg = self._execute()
             if not all(retval):
                 resp.success = False
-                resp.message = "Joint position setpoint could not executed"
+                resp.message = (
+                    f"Joint position setpoint could not executed since {err_msg}."
+                )
             else:
                 resp.success = True
                 resp.message = "Everything went OK"
@@ -1254,10 +1263,12 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req):
         # Execute setpoint.
         rospy.logdebug("Executing joint positions setpoint.")
         try:
-            retval = self._execute(control_group="arm")
+            retval, err_msg = self._execute(control_group="arm")
             if not all(retval):
                 resp.success = False
-                resp.message = "Arm joint position setpoint could not executed"
+                resp.message = (
+                    f"Arm joint position setpoint could not executed since {err_msg}."
+                )
             else:
                 resp.success = True
                 resp.message = "Everything went OK"
@@ -1314,10 +1325,13 @@ def _hand_set_joint_positions_callback(self, set_joint_positions_req):
         # Execute setpoints.
         rospy.logdebug("Executing joint positions setpoint.")
         try:
-            retval = self._execute(control_group="hand")
+            retval, err_msg = self._execute(control_group="hand")
             if not all(retval):
                 resp.success = False
-                resp.message = "Hand joint position setpoint could not be executed"
+                resp.message = (
+                    "Hand joint position setpoint could not be executed since "
+                    f"{err_msg}."
+                )
             else:
                 resp.success = True
                 resp.message = "Everything went OK"
@@ -1660,7 +1674,7 @@ def _get_random_ee_pose_callback(self, get_random_ee_pose_req):
         return resp
 
     def _get_controlled_joints_cb(self, _):
-        """Returns the joints that are currently being controlled by moveit.
+        """Returns the joints that are currently being controlled by MoveIt.
 
         Args:
             get_controlled_joints_req (:obj:`panda_gazebo.srv.GetControlledJointsRequest`):

panda_gazebo/srv/AddBox.srv

@@ -5,4 +5,4 @@ geometry_msgs/Pose pose
 float64[3] size
 ---
 bool success
-string message
+string message

panda_gazebo/srv/AddPlane.srv

@@ -6,4 +6,4 @@ float64[3] normal
 float64 offset  # In the direction of the normal
 ---
 bool success
-string message
+string message

panda_gazebo/srv/GetControlledJoints.srv

@@ -6,4 +6,4 @@ string[] controlled_joints
 string[] controlled_joints_arm
 string[] controlled_joints_hand
 bool success
-string message
+string message

panda_gazebo/srv/GetEe.srv

@@ -1,6 +1,6 @@
-# Service that can be used to get the current EE used in moveit.
+# Service that can be used to get the current EE used in MoveIt.
 
 ---
 string ee_name
 bool success
-string message
+string message

panda_gazebo/srv/GetEePose.srv

@@ -1,6 +1,6 @@
-# Service that can be used to get pose information about the EE using moveit.
+# Service that can be used to get pose information about the EE using MoveIt.
 
 ---
 geometry_msgs/Pose pose
 bool success
-string message
+string message

panda_gazebo/srv/GetEePoseJointConfig.srv

@@ -1,9 +1,9 @@
 # Service that can be used to get a set of posible joint configuration for a given EE
-# pose using moveit.
+# pose using MoveIt.
 geometry_msgs/Pose pose
 int32 attempts
 ---
 string[] joint_names
 float64[] joint_positions
 bool success
-string message
+string message

panda_gazebo/srv/GetEeRpy.srv

@@ -1,8 +1,8 @@
-# Service that can be used to get orientation information about the EE using moveit.
+# Service that can be used to get orientation information about the EE using MoveIt.
 
 ---
 float32 r
 float32 p
 float32 y
 bool success
-string message
+string message

panda_gazebo/srv/GetMoveItControlledJoints.srv

@@ -4,4 +4,4 @@ string[] controlled_joints
 string[] controlled_joints_arm
 string[] controlled_joints_hand
 bool success
-string message
+string message

panda_gazebo/srv/GetRandomEePose.srv

@@ -6,4 +6,4 @@ geometry_msgs/Pose ee_pose
 string[] joint_names
 float64[] joint_positions
 bool success
-string message
+string message

panda_gazebo/srv/GetRandomJointPositions.srv

@@ -6,4 +6,4 @@ int32 attempts
 string[] joint_names
 float64[] joint_positions
 bool success
-string message
+string message

panda_gazebo/srv/SetEe.srv

@@ -1,5 +1,5 @@
-# Service that can be used to set the current EE used in moveit.
+# Service that can be used to set the current EE used in MoveIt.
 string ee_name
 ---
 bool success
-string message
+string message

panda_gazebo/srv/SetEePose.srv

@@ -1,6 +1,6 @@
 # Service that can be used to set the current EE pose and control the robot using
-# moveit.
+# MoveIt.
 geometry_msgs/Pose pose
 ---
 bool success
-string message
+string message

panda_gazebo/srv/SetGripperWidth.srv

@@ -1,5 +1,5 @@
 # Service that can be used to control the robot hand gripper width using the panda_control_server.
-# NOTE: It serves as a small wrapper around the 'franka_gripper/move` action but automatically
+# 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       # Gripper width - ignored when the gripper is grasping.

panda_gazebo/srv/SetJointCommands.srv

@@ -4,10 +4,10 @@
 # the gripper  width to the 'franka_gripper/move' action service.
 string[] joint_names
 float64[] joint_commands
-string control_type # The control type you want to use for the robot arm. Options are "effort" and "position".
-bool grasping # The gripper simply moves if this is `false`.
+string control_type # The control type you want to use for the robot arm. Options are `effort` and `position`.
+bool grasping # If set to true, the gripper 'max_effort' will be set to 10N when not supplied. This will simplify grasping.
 bool arm_wait # Wait till the arm control has completed
 bool hand_wait # Wait till the hand control has completed
 ---
 bool success
-string message
+string message

panda_gazebo/srv/SetJointEfforts.srv

@@ -5,4 +5,4 @@ float64[] joint_efforts
 bool wait
 ---
 bool success
-string message
+string message

panda_gazebo/srv/SetJointPositions.srv

@@ -6,4 +6,4 @@ float64[] joint_positions
 bool wait
 ---
 bool success
-string message
+string message

panda_gazebo/tests/manual/test_effort_joint_trajectory_controller.py

@@ -7,7 +7,6 @@
 from std_msgs.msg import Header
 from trajectory_msgs.msg import JointTrajectoryPoint
 
-
 if __name__ == "__main__":
     rospy.init_node("position_joint_trajectory_controller_test")