🐛 Fixes initial pose execution problem #33

Bug fixes:
 - Fised the pose exeuction problem that was explained in issue #33.

Performance:
 - Fixed slow gazebo robot setup.

panda_openai_sim/cfg/env_config.yaml

@@ -112,10 +112,10 @@ training:
     # Target sampling bounds
     bounds: # (DEFAULT) - The bounds that define the region in which the target pose is sampled
       global: # The global sample bounds relative to the world frame
-        x_min: -1.3
-        x_max: 1.3
-        y_min: -1.3
-        y_max: 1.3
+        x_min: -0.7
+        x_max: 0.7
+        y_min: -0.7
+        y_max: 0.7
         z_min: 0.0
         z_max: 1.3
       local: # The target sample bounds relative to the current ee_pose.

panda_openai_sim/launch/panda_openai_sim.launch

@@ -35,6 +35,7 @@
     </node>
     <node pkg="panda_openai_sim" type="panda_control_server.py" name="panda_control_server" required="true">
         <param name="use_group_controller" value="$(arg use_group_controller)" />
+        <param name="create_all_services" value="false" />
     </node>
 
     <!--## SIMULATION ##-->
@@ -53,6 +54,11 @@
         <arg name="grip_site_rpy" value="$(arg grip_site_rpy)" />
     </include>
 
+    <!-- ## Increase Movit execution time limit (See issue #33)-->
+    <!-- # FIXME: Check if this is the right way -->
+    <param name="move_group/trajectory_execution/allowed_execution_duration_scaling" value="4.0" />
+    <!-- <param name="move_group/trajectory_execution/execution_duration_monitoring" value="false" /> -->
+
     <!-- ## LOAD RVIZ ##
          # NOTE: Only moveit was not used -->
     <include if="$(eval not arg('moveit') and arg('rviz'))" file="$(find panda_openai_sim)/launch/panda_rviz.launch">

panda_openai_sim/nodes/panda_control_server.py

@@ -34,10 +34,15 @@
         autofill_traj_positions = rospy.get_param("~autofill_traj_positions")
     except KeyError:
         autofill_traj_positions = False
+    try:  # Check required services
+        create_all_services = rospy.get_param("~services_load_type")
+    except KeyError:
+        create_all_services = False
 
     # Start control server
     control_server = PandaControlServer(
         use_group_controller=use_group_controller,
         autofill_traj_positions=autofill_traj_positions,
+        create_all_services=create_all_services,
     )
     rospy.spin()  # Maintain the service open

panda_openai_sim/nodes/panda_moveit_server.py

@@ -37,6 +37,6 @@
 
     # Start moveit planner server
     moveit_planner_server = PandaMoveitPlannerServer(
-        arm_ee_link=arm_ee_link, create_all_services=True
+        arm_ee_link=arm_ee_link, create_all_services=create_all_services
     )
     rospy.spin()  # Maintain the service open

panda_openai_sim/src/panda_openai_sim/core/control_server.py

@@ -138,6 +138,7 @@ def __init__(
         self,
         use_group_controller=False,
         autofill_traj_positions=False,
+        create_all_services=False,
         connection_timeout=10,
     ):
         """Initializes the PandaControlServer object.
@@ -149,6 +150,10 @@ def __init__(
         autofill_traj_positions : bool, optional
             Whether you want to automatically set the current states as positions when
             the positions field of the joint trajectory message is left empty.
+        create_all_services : bool, optional
+            Specifies whether we want to create all the available services or only the
+            ones that are crucial for the panda_openai_sim package, by default
+            False.
         connection_timeout : int, optional
             The timeout for connecting to the controller_manager services,
             by default 3 sec.
@@ -196,6 +201,8 @@ def __init__(
         #############################################
         # Create Panda control services #############
         #############################################
+
+        # Create main PandaControlServer services
         rospy.loginfo("Creating '%s' services." % rospy.get_name())
         rospy.logdebug("Creating '%s/set_joint_positions' service." % rospy.get_name())
         self._set_joint_positions_srv = rospy.Service(
@@ -209,38 +216,6 @@ def __init__(
             SetJointEfforts,
             self._set_joint_efforts_cb,
         )
-        rospy.logdebug(
-            "Creating '%s/panda_arm/set_joint_positions' service." % rospy.get_name()
-        )
-        self._set_arm_joint_positions_srv = rospy.Service(
-            "%s/panda_arm/set_joint_positions" % rospy.get_name()[1:],
-            SetJointPositions,
-            self._arm_set_joint_positions_cb,
-        )
-        rospy.logdebug(
-            "Creating '%s/panda_arm/set_joint_efforts' service." % rospy.get_name()
-        )
-        self._set_arm_joint_efforts_srv = rospy.Service(
-            "%s/panda_arm/set_joint_efforts" % rospy.get_name()[1:],
-            SetJointEfforts,
-            self._arm_set_joint_efforts_cb,
-        )
-        rospy.logdebug(
-            "Creating '%s/panda_hand/set_joint_positions' service." % rospy.get_name()
-        )
-        self._set_hand_joint_positions_srv = rospy.Service(
-            "%s/panda_hand/set_joint_positions" % rospy.get_name()[1:],
-            SetJointPositions,
-            self._hand_set_joint_positions_cb,
-        )
-        rospy.logdebug(
-            "Creating '%s/panda_hand/set_joint_efforts' service." % rospy.get_name()
-        )
-        self._set_hand_joint_efforts_srv = rospy.Service(
-            "%s/panda_hand/set_joint_efforts" % rospy.get_name()[1:],
-            SetJointEfforts,
-            self._hand_set_joint_efforts_cb,
-        )
         rospy.logdebug("Creating '%s/switch_control_type' service." % rospy.get_name())
         self._switch_control_type_srv = rospy.Service(
             "%s/switch_control_type" % rospy.get_name()[1:],
@@ -253,7 +228,6 @@ def __init__(
             ListControlType,
             self._list_control_type_cb,
         )
-        rospy.loginfo("'%s' services created successfully." % rospy.get_name())
         rospy.logdebug(
             "Creating '%s/get_controlled_joints' service." % rospy.get_name()
         )
@@ -262,6 +236,43 @@ def __init__(
             GetControlledJoints,
             self._get_controlled_joints_cb,
         )
+
+        # Create other services
+        if create_all_services:
+            rospy.logdebug(
+                "Creating '%s/panda_arm/set_joint_positions' service."
+                % rospy.get_name()
+            )
+            self._set_arm_joint_positions_srv = rospy.Service(
+                "%s/panda_arm/set_joint_positions" % rospy.get_name()[1:],
+                SetJointPositions,
+                self._arm_set_joint_positions_cb,
+            )
+            rospy.logdebug(
+                "Creating '%s/panda_arm/set_joint_efforts' service." % rospy.get_name()
+            )
+            self._set_arm_joint_efforts_srv = rospy.Service(
+                "%s/panda_arm/set_joint_efforts" % rospy.get_name()[1:],
+                SetJointEfforts,
+                self._arm_set_joint_efforts_cb,
+            )
+            rospy.logdebug(
+                "Creating '%s/panda_hand/set_joint_positions' service."
+                % rospy.get_name()
+            )
+            self._set_hand_joint_positions_srv = rospy.Service(
+                "%s/panda_hand/set_joint_positions" % rospy.get_name()[1:],
+                SetJointPositions,
+                self._hand_set_joint_positions_cb,
+            )
+            rospy.logdebug(
+                "Creating '%s/panda_hand/set_joint_efforts' service." % rospy.get_name()
+            )
+            self._set_hand_joint_efforts_srv = rospy.Service(
+                "%s/panda_hand/set_joint_efforts" % rospy.get_name()[1:],
+                SetJointEfforts,
+                self._hand_set_joint_efforts_cb,
+            )
         rospy.loginfo("'%s' services created successfully." % rospy.get_name())
 
         #############################################

panda_openai_sim/src/panda_openai_sim/core/moveit_server.py

@@ -196,6 +196,34 @@ def __init__(
             SetEe,
             self._arm_set_ee_callback,
         )
+        rospy.logdebug(
+            "Creating '%s/get_random_joint_positions' service." % rospy.get_name()
+        )
+        self._get_random_joints_positions_srv = rospy.Service(
+            "%s/get_random_joint_positions" % rospy.get_name()[1:],
+            GetRandomJointPositions,
+            self._get_random_joint_positions_callback,
+        )
+        rospy.logdebug("Creating '%s/get_random_ee_pose' service." % rospy.get_name())
+        self._get_random_ee_pose_srv = rospy.Service(
+            "%s/get_random_ee_pose" % rospy.get_name()[1:],
+            GetRandomEePose,
+            self._get_random_ee_pose_callback,
+        )
+        rospy.logdebug(
+            "Creating '%s/panda_arm/get_ee_pose' service." % rospy.get_name()
+        )
+        self._arm_get_ee_pose_srv = rospy.Service(
+            "%s/panda_arm/get_ee_pose" % rospy.get_name()[1:],
+            GetEePose,
+            self._arm_get_ee_pose_callback,
+        )
+        rospy.logdebug("Creating '%s/panda_arm/get_ee_rpy' service." % rospy.get_name())
+        self._arm_get_ee_rpy_srv = rospy.Service(
+            "%s/panda_arm/get_ee_rpy" % rospy.get_name()[1:],
+            GetEeRpy,
+            self._arm_get_ee_rpy_callback,
+        )
 
         # Create other services
         if create_all_services:
@@ -217,38 +245,6 @@ def __init__(
                 SetJointPositions,
                 self._hand_set_joint_positions_callback,
             )
-            rospy.logdebug(
-                "Creating '%s/panda_arm/get_ee_pose' service." % rospy.get_name()
-            )
-            self._arm_get_ee_pose_srv = rospy.Service(
-                "%s/panda_arm/get_ee_pose" % rospy.get_name()[1:],
-                GetEePose,
-                self._arm_get_ee_pose_callback,
-            )
-            rospy.logdebug(
-                "Creating '%s/panda_arm/get_ee_rpy' service." % rospy.get_name()
-            )
-            self._arm_get_ee_rpy_srv = rospy.Service(
-                "%s/panda_arm/get_ee_rpy" % rospy.get_name()[1:],
-                GetEeRpy,
-                self._arm_get_ee_rpy_callback,
-            )
-            rospy.logdebug(
-                "Creating '%s/get_random_joint_positions' service." % rospy.get_name()
-            )
-            self._get_random_joints_positions_srv = rospy.Service(
-                "%s/get_random_joint_positions" % rospy.get_name()[1:],
-                GetRandomJointPositions,
-                self._get_random_joint_positions_callback,
-            )
-            rospy.logdebug(
-                "Creating '%s/get_random_ee_pose' service." % rospy.get_name()
-            )
-            self._get_random_ee_pose_srv = rospy.Service(
-                "%s/get_random_ee_pose" % rospy.get_name()[1:],
-                GetRandomEePose,
-                self._get_random_ee_pose_callback,
-            )
         rospy.loginfo("'%s' services created successfully." % rospy.get_name())
 
         # Initiate service msgs
@@ -325,6 +321,7 @@ def _execute(self, control_group="both"):
         """
 
         # Plan and execute
+        # TODO: Add multistep _plan function
         if control_group.lower() == "arm":
             self.arm_plan = self.move_group_arm.plan()
             arm_retval = self.move_group_arm.go(wait=True)
@@ -1139,6 +1136,7 @@ def _get_random_joint_positions_callback(self, get_random_position_req):
             get_random_hand_joint_positions_srvs_exception = True
 
         # Get random joint positions (while taking into possible joint limits)
+        # FIXME: Why do we already set these here?
         random_arm_joint_values = random_arm_joint_values_unbounded
         random_hand_joint_values = random_hand_joint_values_unbounded
         if (
@@ -1319,10 +1317,8 @@ def _get_random_ee_pose_callback(self, get_random_ee_pose_req):
             == 0.0
         ):  # No bounding region was set
 
-            # Use unbounded ee_pose nand set success bool
+            # Create response message
             if not get_random_pose_srvs_exception:
-
-                # Create response message and break out of loop
                 resp.success = True
                 resp.ee_pose = random_ee_pose_unbounded.pose
 

panda_openai_sim/src/panda_openai_sim/envs/robot_envs/panda_robot_env.py

@@ -377,7 +377,7 @@ def __init__(
             rospy.logwarn("Failed to connect to '%s' service!" % MOVEIT_GET_EE_TOPIC)
             self._services_connection_status[MOVEIT_GET_EE_TOPIC] = False
 
-        # Connect to Moveit 'set_joints_positions' service
+        # Connect to Moveit 'set_joint_positions' service
         try:
             rospy.logdebug(
                 "Connecting to '%s' service." % MOVEIT_SET_JOINT_POSITIONS_TOPIC
@@ -871,10 +871,10 @@ def set_ee_pose(self, ee_pose):
 
                 # Return success bool
                 if not retval.success:
-                    logwarn_msg = "End effector pose not set as " + lower_first_char(
+                    logdebug_msg = "End effector pose not set as " + lower_first_char(
                         retval.message
                     )
-                    rospy.logwarn(logwarn_msg)
+                    rospy.logdebug(logdebug_msg)
                     return False
                 else:
                     return True

panda_openai_sim/src/panda_openai_sim/envs/task_envs/panda_task_env.py

@@ -62,9 +62,11 @@
     GetControlledJointsRequest,
 )
 
+# TODO: Add current target as ros param
+# TODO: Change reward topic
 # TODO: Check panda_task env with gym and openai_ros
 # TODO: Wait for joint trajectory control
-# TODO: Add fix gripper rotation see self._set_Action of gym
+# TODO: Add fix gripper rotation see self._set_Action of gym makes sure that init pose is set
 # TODO: Check log message
 # TODO: Check control switcher
 # TODO: Check why finger joints can not be controlled
@@ -1133,7 +1135,7 @@ def _set_init_pose(self):
         # Put finger joints in the right position if use_gripper_width is True
         if self._use_gripper_width:
             if "gripper_width" not in init_grip_joints_pose.keys():
-                rospy.logwarn(
+                rospy.loginfo(
                     "The joint position of 'panda_finger_joint2' was set to be "
                     "be equal to the value of 'panda_finger_joint1' as the "
                     "'use_gripper_width' variable is set to True."
@@ -1155,10 +1157,12 @@ def _set_init_pose(self):
 
         # Make sure the end effector z position is higher than the extra gripper height
         # 'gripper_extra_height'
+        # TODO: Validate wheter this pose is valid with the extra height
         if init_ee_pose["z"] <= self._gripper_extra_height:
             init_ee_pose["z"] = self._gripper_extra_height
 
         # Set initial pose
+        # TODO: Why only wait true for joint_positions
         self.gazebo.unpauseSim()
         ee_pose_retval = self.set_ee_pose(init_ee_pose)
         gripper_pose_retval = self.set_joint_positions(init_grip_joints_pose, wait=True)