/joint_states return inaccurate joints information

[YY-GX]

Describe the bug

Hi all,

Thank you again for your contributions to the repo!! But I have one small issue while using it, could you help me with that?

In the Reach task, I try to use delta_joints as action space. However, after changing the action space, I found that the robot cannot move as I expect. Actually, I found that the joints information published via /joint_states is not accurate.

Reproduction steps

1. In src/ros-gazebo-gym/src/ros_gazebo_gym/task_envs/panda/panda_reach.py, aside from those action/observation space modifications I made, the biggest change I made is in _set_action() function. I add several lines of code in the initial of the function:

   def _set_action(self, action):
   
       if self.robot_control_type == "position":
           crr_joints_pos = copy.deepcopy(np.array(self.joint_states.position)) 
           action = np.array(crr_joints_pos) + action.copy()
   
       # The following parts are the same
       # Throw error and shutdown if action space is not the right size.
       if not action.shape == self.action_space.shape:
           err_msg = (
               f"Shutting down '{rospy.get_name()}' since the shape of the supplied "
               f"action {action.shape} while the gymnasium action space has shape "
               f"{self.action_space.shape}."
           )
           ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1)
       ......
       ......
       ......

2. I collect some demonstrations and then do the demonstration replay. In my application, the state is the 7 joints of the robot and the actions are generated via s{t+1} - s{t}. However, I found that the robot cannot move as it did in the demonstration. But when I directly let the robot move to s_{t+1}, it works perfectly. The action (i.e., at = s{t+1} - s_{t}) is accurate as well. Therefore, the issue must come from the s_t, which is the self.joint_states.position I use in the above code. I am not sure where is wrong, either the /joint_states is inaccurate, or there's some lag between using the self.joint_states.position and the real current joints.

The demonstration file is attached: demos.zip

Could you help me with this issue? Thank you so much!!

Expected behaviour

No response

Screenshots / Live demo link

No response

System information

• Ubuntu 20

Additional context

No response

[rickstaa]

Hey @YY-GX, thanks for reaching out with your question! The /joint_states topic originates from the franka_ros package. It might be helpful to verify if you're encountering similar issues when directly interacting with the robot through that package. For modifications related to position control in the set_joint_positions method, you can refer to this section of the code:

set_joint_positions method in panda_env.py

Unfortunately, I'm currently strapped for time and won't be able to dive deeper into debugging your issues. I hope the pointer above offers a good starting place for troubleshooting.