fix: apply fix for 'set_model_configuration' joint limit bug

This commit adds a fix that prevents the joint positions being
pushed outside of the joint limits when Gazebo's
'set_model_configuration' service is used. See
frankaemika/franka_ros#225 for more
information.

src/ros_gazebo_gym/task_envs/panda/config/panda_pick_and_place.yaml

@@ -45,7 +45,7 @@ panda_pick_and_place:
  randomize_first_episode: True # Also randomize the pose in the first episode.
  attempts: 10 # Maximum number of attempts for sampling a random initial pose within the set bounds.
  pose_sampling_type: "end_effector_pose" # From which set we should sample the initial pose. Options: 'end_effector_pose' and 'joint_positions'.
- moveit_control: True # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
+ moveit_control: False # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
 
  # The initial robot pose (used when random is disabled).
  init_pose:

src/ros_gazebo_gym/task_envs/panda/config/panda_push.yaml

@@ -45,7 +45,7 @@ panda_push:
  randomize_first_episode: True # Also randomize the pose in the first episode.
  attempts: 10 # Maximum number of attempts for sampling a random initial pose within the set bounds.
  pose_sampling_type: "end_effector_pose" # From which set we should sample the initial pose. Options: 'end_effector_pose' and 'joint_positions'.
- moveit_control: True # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
+ moveit_control: False # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
 
  # The initial robot pose (used when random is disabled).
  init_pose:

src/ros_gazebo_gym/task_envs/panda/config/panda_reach.yaml

@@ -45,7 +45,7 @@ panda_reach:
  randomize_first_episode: True # Also randomize the pose in the first episode.
  attempts: 10 # Maximum number of attempts for sampling a random initial pose within the set bounds.
  pose_sampling_type: "end_effector_pose" # From which set we should sample the initial pose. Options: 'end_effector_pose' and 'joint_positions'.
- moveit_control: True # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
+ moveit_control: False # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
 
  # The initial robot pose (used when random is disabled).
  init_pose:

src/ros_gazebo_gym/task_envs/panda/config/panda_slide.yaml

@@ -45,7 +45,7 @@ panda_slide:
  randomize_first_episode: True # Also randomize the pose in the first episode.
  attempts: 10 # Maximum number of attempts for sampling a random initial pose within the set bounds.
  pose_sampling_type: "end_effector_pose" # From which set we should sample the initial pose. Options: 'end_effector_pose' and 'joint_positions'.
- moveit_control: True # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
+ moveit_control: False # Use MoveIt to set the initial pose. If `false` the Gazebo `set_model_configuration` service will be used.
 
  # The initial robot pose (used when random is disabled).
  init_pose:

src/ros_gazebo_gym/task_envs/panda/panda_reach.py

@@ -24,6 +24,7 @@
 import numpy as np
 import rospy
 import tf2_geometry_msgs
+from franka_msgs.srv import ResetJointStates, ResetJointStatesRequest
 from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Vector3
 from gym import spaces, utils
 from ros_gazebo_gym.common import Singleton
@@ -41,9 +42,7 @@
 from ros_gazebo_gym.common.markers import SampleRegionMarker, TargetMarker
 from ros_gazebo_gym.core import ROSLauncher
 from ros_gazebo_gym.core.helpers import get_log_path, load_ros_params_from_yaml
-from ros_gazebo_gym.exceptions import (
- EePoseLookupError,
-)
+from ros_gazebo_gym.exceptions import EePoseLookupError
 from ros_gazebo_gym.robot_envs.panda_env import PandaEnv
 from rospy.exceptions import ROSException, ROSInterruptException
 from std_msgs.msg import ColorRGBA, Header
@@ -66,6 +65,7 @@
 MOVEIT_SET_JOINT_POSITIONS_TOPIC = "panda_moveit_planner_server/set_joint_positions"
 MOVEIT_GET_RANDOM_EE_POSE_TOPIC = "panda_moveit_planner_server/get_random_ee_pose"
 MOVEIT_ADD_PLANE_TOPIC = "panda_moveit_planner_server/planning_scene/add_plane"
+FRANKA_GAZEBO_RESET_JOINT_STATES_TOPIC = "reset_joint_states"
 VALID_EE_CONTROL_JOINTS = ["x", "y", "z", "rx", "ry", "rz", "rw"]
 CONFIG_FILE_PATH = "config/panda_reach.yaml"
 PANDA_REST_CONFIGURATION = [
@@ -301,8 +301,8 @@ def __init__( # noqa: C901
  "Failed to connect to '%s' service!" % moveit_add_plane_srv_topic
  )
 
- # Connect to MoveIt 'planning_scene/set_joint_positions' service
  if self._moveit_init_pose_control:
+ # Connect to MoveIt 'planning_scene/set_joint_positions' service
  try:
  moveit_set_joint_positions_srv_topic = (
  f"{self.robot_name_space}/{MOVEIT_SET_JOINT_POSITIONS_TOPIC}"
@@ -325,6 +325,32 @@ def __init__( # noqa: C901
  "Failed to connect to '%s' service!"
  % moveit_set_joint_positions_srv_topic
  )
+ else:
+ # Connect to franka_gazebo's 'reset_joint_positions' service
+ try:
+ franka_gazebo_reset_joint_states_srv_topic = (
+ f"{self.robot_name_space}/{FRANKA_GAZEBO_RESET_JOINT_STATES_TOPIC}"
+ )
+ rospy.logdebug(
+ "Connecting to '%s' service."
+ % franka_gazebo_reset_joint_states_srv_topic
+ )
+ rospy.wait_for_service(
+ franka_gazebo_reset_joint_states_srv_topic,
+ timeout=CONNECTION_TIMEOUT,
+ )
+ self._franka_gazebo_reset_joint_states_srv = rospy.ServiceProxy(
+ franka_gazebo_reset_joint_states_srv_topic, ResetJointStates
+ )
+ rospy.logdebug(
+ "Connected to '%s' service!"
+ % franka_gazebo_reset_joint_states_srv_topic
+ )
+ except (rospy.ServiceException, ROSException, ROSInterruptException):
+ rospy.logwarn(
+ "Failed to connect to '%s' service!"
+ % franka_gazebo_reset_joint_states_srv_topic
+ )
 
  # Create current target publisher
  rospy.logdebug("Creating target pose publisher.")
@@ -1067,6 +1093,41 @@ def _create_action_space(self):
  dtype="float32",
  )
 
+ def _set_panda_configuration(self, joint_names, joint_positions):
+ """Set the panda robot to a given configuration.
+
+ Args:
+ joint_positions (list): The desired joint positions.
+ joint_names (list): The joint names for which you want to set the joint
+ position.
+
+ Returns:
+ bool: Whether the panda configuration was successfully set.
+
+ .. info::
+ This method was implemented to ensure that the joint positions stay inside
+ the joint_limits when Gazebo's ``set_model_configuration`` service is used.
+ For more information, see
+ `https:/frankaemika/franka_ros/issues/225 <https:/frankaemika/franka_ros/issues/225>`_.
+ """ # noqa: E501
+ retval = self.gazebo.set_model_configuration(
+ model_name="panda",
+ joint_names=joint_names,
+ joint_positions=joint_positions,
+ )
+
+ # Reset the franka joint positions
+ # NOTE: Needed because https:/frankaemika/franka_ros/issues/225
+ if hasattr(self, "_franka_gazebo_reset_joint_states_srv"):
+ resp = self._franka_gazebo_reset_joint_states_srv.call(
+ ResetJointStatesRequest()
+ )
+ retval = resp.success
+ else:
+ retval = False
+
+ return retval
+
  @property
  def ee_pose(self):
  """Returns the ee pose while taking the `reward_frame_offset` into account when
@@ -1440,8 +1501,7 @@ def _set_init_pose(self): # noqa: C901
  self._randomize_first_episode or self.episode_num != 0
  ): # Retrieve random EE pose
  rospy.logdebug("Retrieve random EE pose.")
- self.gazebo.set_model_configuration(
- model_name="panda",
+ self._set_panda_configuration(
  joint_names=self.joints["both"],
  joint_positions=PANDA_REST_CONFIGURATION[
  : len(self.joints["both"])
@@ -1560,13 +1620,11 @@ def _set_init_pose(self): # noqa: C901
  )
 
  # Set initial model configuration and return result
- # NOTE: Here two modes can be used: MoveIT or the set_model_configuration
- # service.
- # IMPROVE: The MoveIt method can be removed if #TODO is fixed.
+ # NOTE: Here two modes can be used: MoveIT or Gazebo's
+ # 'set_model_configuration' service.
  if not self._moveit_init_pose_control: # Use Gazebo service
  rospy.loginfo("Setting initial robot pose.")
- init_pose_retval = self.gazebo.set_model_configuration(
- model_name="panda",
+ init_pose_retval = self._set_panda_configuration(
  joint_names=self._init_model_configuration.keys(),
  joint_positions=self._init_model_configuration.values(),
  )