joint_state_controller provides different joint_states from the gazebo plugin joint_state_publisher

[jacknlliu]

When using joint_state_controller with gazebo, /joint_states will provide wrong and large joint data when robot has heavy collision with some object in the gazebo world. But if we use gazebo plugin joint_state_publisher, the joint states will be right!

I find that gazebo plugin joint_state_publisher gets the joint states directly from gazebo, see here. But I can't get where the joint states update in joint_sate_controller.

How does the joint_state_[i] update in the following codes?

ros_controllers/joint_state_controller/src/joint_state_controller.cpp

Line 93 in e5a6c5a

realtime_pub_->msg_.position[i] = joint_state_[i].getPosition();

[bmagyar]

It does it exactly the same way. My only guess is that you restart gazebo time which break the ros_control plugin.

[jacknlliu]

It can't provide the exact joint data, even though we never reset gazebo time.

[jacknlliu]

I found that the joint data from the gazebo API directly seems reasonable, see here, but after this function angles::shortest_angular_distance(), the joint angles become very large after robot had a collision with some object in gazebo.

I think I should provide a basic ros package for proving this.

[bmagyar]

That would be great. If we can reproduce it cleanly, we can fix it.

[jacknlliu]

Hi, @bmagyar Sorry for the delay. Could you try the kinova_ros package with the following command?

roslaunch kinova_gazebo robot_launch.launch kinova_robotType:=j2n6s300

After the robot initial, we can find that joint-state-controller provides the wrong joint positions.

[ignacioDeusto]

The issue can be easily reproduced using the ur_robot_driver repository. It starts after certain collision circumstances. In order to reproduce it, in any ros workspace:

1. In the root workspace directory clone the ur_robot_driver repository:

git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git src/Universal_Robots_ROS_Driver

git clone -b calibration_devel https://github.com/fmauch/universal_robot.git src/fmauch_universal_robot

2. Update rosdep and install all the missing dependencies:

sudo apt update -qq

rosdep update

rosdep install --from-paths src --ignore-src -y

3. Build the workspace with catkin_make or using buildtools:

catkin_make

or

catkin build

4. Source the workspace:

source devel/setup.bash

5. Launch the Gazebo simulation:

roslaunch ur_gazebo ur3e_bringup.launch

6. Open two new terminals within the same workspace and source them.
7. On one terminal check the joint state values:

rostopic echo /joint_states

9. On the other terminal execute the following command:

rostopic pub /pos_joint_traj_controller/command trajectory_msgs/JointTrajectory "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names:
- "shoulder_pan_joint"
- "shoulder_lift_joint"
- "elbow_joint"
- "wrist_1_joint"
- "wrist_2_joint"
- "wrist_3_joint"
points:
- positions: [0,3.14,0,0,0,0]
  velocities: [0,0,0,0,0,0]
  accelerations: [0,0,0,0,0,0]
  effort: [0,0,0,0,0,0]
  time_from_start: {secs: 4, nsecs: 0}" -1

The robot should try to go to that position crossing the floor and while heavy colliding with the floor the bug appears, showing a joint position that exceed by far the actual limits.

After some further experimentation, you can check how they're actually the correct joint values but outside the bounds. For example, when sending that command for the first time, I got the following positions values:
[18.849573233427748, 3.1399866768081672, 18.849762255449107, 3.7085518315604915e-05, -43.98228751908111, -6.283167197213825]
As you can see those are equivalent to:
[0, 3.1399866768081672, 0, 0, 0, 0]
But outside the 2pi range.
Now if I command the first joint to move to 4 rad I get:
[22.849555914307878, 3.1399866768081672, 18.849762255449107, 3.7085518315604915e-05, -43.98228751908111, -6.283167197213825]
That is also equivalent to:
[4, 3.1399866768081672, 0, 0, 0, 0]

This issue reminds me of ros/angles#2 , that was actually closed long ago but with those weird collision states may be related.

[ignacioDeusto]

The issue has been detected and reported in the gazebo_ros_pkgs repository as the main bug concerns the gazebo_ros_control package sources. ros-simulation/gazebo_ros_pkgs#1405 (comment)