peci1
commented
12 months ago I guess that, that particular piece of code is not arbitrary and the increment based update is needed for other purposes so if this particular workaround can't be applied, another one based on "out of bounds" increments could be also applied, to at least return to the correct values after the collision occurs.
The code has been there from the very first commit. And the alternative plugin doesn't use it: https://github.com/ros-simulation/gazebo_ros_pkgs/blob/3164e4c6299407209a15fd87300f4364dc2e8063/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp#L138 . I'd therefore think removing the shortest angular distance should be ok. However, with 0 test coverage of the gazebo_ros_control plugin, it would need more deliberation before merging such change.
@davetcoleman You're the commiter of the initial commit that brought the piece-by-piece angular position updates using angles::shortest_angular_distance(). Any possibility you remember why was this approach chosen instead of direct reading of the simulation positions?
Description
When the robot suffers a heavy collision state in Gazebo, gazebo_ros_control updates the joint_state_controller with values outside of the robot limits. This occurs when, while the robot is in a heavy collision state, the joints perform high value jumps on every physics iteration. These value jumps are then causing big increments in the controller's stored joint values in the following lines: https://github.com/ros-simulation/gazebo_ros_pkgs/blob/231a7219b36b8a6cdd100b59f66a3df2955df787/gazebo_ros_control/src/default_robot_hw_sim.cpp#L277-L278 Once those increments have been applied, the values of the joints never return to the correct ones, they drop out of the (-2pi, 2pi) range.
Replicability
This issue as already been reported in https://github.com/ros-controls/ros_controllers/issues/317 , although the real issue comes from this repository (or from the gazebo behavior to collisions). This bug can easily be replicated following the guidelines exposed on this comment https://github.com/ros-controls/ros_controllers/issues/317#issuecomment-1160495536 When reproduced, the robot performs this trajectory:
and with this output the gazebo joint value (gazebo_pos), the controller joint value (joint_val) and the performed increments (increment) are shown for the wrist 2 joint:
The controller joint value exceeds the robot limits (-2pi,2pi) due to the increments caused in the collision, thus causing incorrect joint values to be published by joint_state_controller.
Possible workaround
The issue relies in the following lines: https://github.com/ros-simulation/gazebo_ros_pkgs/blob/231a7219b36b8a6cdd100b59f66a3df2955df787/gazebo_ros_control/src/default_robot_hw_sim.cpp#L261-L283 For the prismatic joints, the update is done using the actual simulation value, but for non prismatic joints, the update is performed applying an increment using the previous controller joint value and the actual simulation value. This increment causes the value to exceed the limits. Updating the values directly with the simulation values solves de issue, i.e. updating all the joints like the prismatic ones. I guess that, that particular piece of code is not arbitrary and the increment based update is needed for other purposes so if this particular workaround can't be applied, another one based on "out of bounds" increments could be also applied, to at least return to the correct values after the collision occurs.