In end_effector_control, set_ee_pose() succeed for reach/push/slide, but fails for pick_and_place

[YY-GX]

Describe the bug

Thank you so much for contributing this awesome repo!!

However, I get one strange bug (only happen for pick_and_place but not for reach, push and slide), so I feel the bug is with the gripper: I set the control_type as "end_effector". Then I create the env and use env.step(ee_pos) where the ee_pos is exactly the current pose, but the robot doesn't move at all. Then I found that the set_ee_pose() function used return False and the message say "ee pose could not be set". Since the pose I use is exactly the current pose, so I cannot understand why the moveit cannot plan a path to reach it. I guess some collision might happen within the robot?

It's appreciated if you could help with this issue :)

Reproduction steps

1. Create a new script "playground.py"
2. Create the env and call the env.step like:

   
   #!/usr/bin/env python3
   """A small example script that shows how to use the `ros_gazebo_gym`_ package to train a
   (SAC) agent to solve a `Panda`_ environment using `Stable Baselines3`_.

.. _ros_gazebo_gym: https://github.com/rickstaa/ros-gazebo-gym """ import os import re import time

import gymnasium as gym import numpy as np import rospkg import rospy import torch from ros_gazebo_gym.common.helpers import ( list_2_human_text, to_pascal_case, to_snake_case, ) from ros_gazebo_gym.core.helpers import ros_exit_gracefully from ros_gazebo_gym.task_envs.task_envs_list import ENVS from stable_baselines3 import SAC

import pickle

if name == "main": # noqa: C901 rospy.init_node( "playground", anonymous=True, )

# Retrieve input arguments.
try:
    control_type = rospy.get_param("~control_type")
except KeyError:
    control_type = "effort"
try:
    env_type = rospy.get_param("~environment_type")
    print("Env Type: ", env_type)
except KeyError:
    env_type = "slide"
try:
    print(ENVS.keys())
    print(f"Panda{to_pascal_case(env_type)}")
    env_id = (
        "ros_gazebo_gym:"
        + [
            env
            for env in ENVS.keys()
            if env.startswith(f"Panda{to_pascal_case(env_type)}")
        ][0]
    )
    print("env_id: ", env_id)
except IndexError:
    valid_env_ids = list(ENVS.keys())
    valid_env_cmds = [
        re.sub(r"panda_|(-v\d+)", "", to_snake_case(env)) for env in valid_env_ids
    ]
    valid_env_str = [
        f"'{cmd}' (ros_gazebo_gym:{id})"
        for cmd, id in zip(valid_env_cmds, valid_env_ids)
    ]
    rospy.logerr(
        f"Could not find 'environment_type' 'Panda{to_pascal_case(env_type)}'. "
        f"Valid options are: {list_2_human_text(valid_env_str)}."
    )
    ros_exit_gracefully(
        shutdown_message=f"Shutting down {rospy.get_name()}", exit_code=1
    )

# Initialize the ros_gazebo_gym Panda environment.
rospy.loginfo(f"Creating ros_gazebo_gym '{env_id}' gymnasium environment...")

env = gym.make(
    env_id,
    control_type=control_type,
    action_space_dtype=np.float32,
    observation_space_dtype=np.float32,
)

# Initialize the logging system.
rospack = rospkg.RosPack()
pkg_path = rospack.get_path("ros_gazebo_gym_examples")
outdir = os.path.join(pkg_path, "training_results")
last_time_steps = np.ndarray(0)

# Load parameters from the ROS param server.
# NOTE: Parameters are stored in a yaml files inside the config directory and
# loaded at runtime by the launch file.
alpha = rospy.get_param("/ros_gazebo_gym_panda_example_params/alpha")
gamma = rospy.get_param("/ros_gazebo_gym_panda_example_params/gamma")
n_episodes = rospy.get_param("/ros_gazebo_gym_panda_example_params/n_episodes")
n_steps = rospy.get_param("/ros_gazebo_gym_panda_example_params/n_steps")
inference = rospy.get_param("/ros_gazebo_gym_panda_example_params/inference")
inference_n_episodes = rospy.get_param(
    "/ros_gazebo_gym_panda_example_params/inference_n_episodes"
)
total_timesteps = n_steps * n_episodes

obs, _ = env.reset()
ee = env.get_ee_pose().pose
obs, _, _, _, _ = env.step(np.array([ee.position.x, ee.position.y, ee.position.z,
                                     ee.orientation.x, ee.orientation.y, ee.orientation.z, ee.orientation.w,
                                     0.08, 5]))

3. write a launch file to start this node with set control_type as "end_effector" and env as "pick_and_place":

4. then you can see that the plan doesn't generate at all (you may need to print the output of `set_ee_pose()`).

### Expected behaviour

moveit doesn't plan

### Screenshots / Live demo link

_No response_

### System information

- OS: [e.g. Windows, Mac, Linux, iOS, android]
- Version [e.g. 22]

### Additional context

_No response_

[rickstaa]

@YY-GX, I appreciate your detailed bug report. I've successfully replicated the issues you highlighted and have implemented the necessary fixes. I still have to perform some tests, but when ready, these updates will be integrated into the main branch and included in the upcoming release, version 2.0.0. You can track the progress of this release at https://github.com/rickstaa/ros-gazebo-gym/pull/66.

[rickstaa]

Hey @YY-GX, hope you're doing well! I've just rolled out v2.0.0, which addresses numerous bugs. However, there's still an unresolved issue related to setting the ee_frame_offset or handling the Panda Hand when the control type is set to end-effector (see https://github.com/rickstaa/ros-gazebo-gym/issues/97). In my projects, I've moved away from using end-effector control, and currently, I don't have the capacity to dive into this problem further.