JointPositionController oscillation

[skrede]

I've been trying to get a simulation of the Kuka KR6 Agilus running in Gazebo, and while almost there, there is one problem that remains.

I'm running Gazebo 9 with ROS Melodic.

Each joint is controlled by a JointPositionController, and all but one joint behaves sufficiently. I won't paste configurations here, but the repository is available on Github: https://github.com/skrede/kuka_gazebo

The joint in question is joint_a4, and the problem is that the joint has a given setpoint but somehow the controller seems confused as to how to get there. If I echo the following topic (of message type control_msgs/JointControllerState)

rostopic echo /joint_a4_position_controller/state

to terminal, I can see that the setpoint is specified, but the command and error fields seem to perturb violently, as shown in the following printout of the topic.

---
header: 
  seq: 74830
  stamp: 
    secs: 748
    nsecs: 825000000
  frame_id: ''
set_point: -1.69285065965e-08
process_value: -3.05827630699
process_value_dot: 1.96346264844
error: 3.05827629006
time_step: 0.001
command: 1175.71498185
p: 500.0
i: 5.0
d: 180.0
i_clamp: 0.0
antiwindup: False
---
header: 
  seq: 74831
  stamp: 
    secs: 748
    nsecs: 835000000
  frame_id: ''
set_point: -1.69285065965e-08
process_value: -3.03920837904
process_value_dot: 1.76346476062
error: -3.24397694507
time_step: 0.001
command: -1939.41204719
p: 500.0
i: 5.0
d: 180.0
i_clamp: 0.0
antiwindup: False

This might not be the correct place to raise the issue, but I'm not sure where the problem lies. I suppose it could be the URDF or some other configuration that confuse the controller, but it seemed to work just fine using position_controllers/JointTrajectoryController. This problem occured after using JointPositionController. I started looking at the urdf configurations, but I'm not even sure what to look for. I also tried to adjust PID gains, but that didn't help either.

A video of the problem is available here: https://youtu.be/PqQh9spXjgM

The package is somewhat easy to get running, given dependencies are already installed. I haven't listed them in the repository, but the package.xml should state dependencies.

When running Gazebo, the joint starts to perturb right away. This can be seen visually or by echoing the abovementioned topic to terminal. The setpoint does not change, i.e., nothing is sent via the topic

/joint_a4_position_controller/command

With that I mean the command-field listed in the terminal dump above is not sent to the controller.

I've made a thread on both answers.ros.org and answers.gazebosim.org, but there's no response. I hope it's appropriate to make an issue here as well.

[skrede]

The problem is not present if I reduce the lower limit of the joint. This isn't a solution but a workaround, though.

      <limit effort="100" lower="${-DEG2RAD * 174}" upper="${DEG2RAD * 185}" velocity="${DEG2RAD * 381}"/>
      <!--<limit effort="100" lower="${-DEG2RAD * 185}" upper="${DEG2RAD * 185}" velocity="${DEG2RAD * 381}"/>-->

If I specify a lower limit α < -174 the joint behaves as shown in the video. F.ex, the following will behave as in the video lower="${-DEG2RAD * 175}"

The configuration can be found here

[skrede]

This seems to be related to the size of the joint position interval. At first I thought other joints don't have this problem, but they do. If the difference between lower and upper limits gets too big, they start to oscillate as well.

[francofusco]

I had a similar problem yesterday, but with limits set to -20*pi and 20*pi. I know they are absurd, but in my opinion the problem had to be addressed (at least to allow a more reasonable range such as [-2*pi,2*pi]). The problem is with the shortest_angular_distance_with_limits from the angles pagkage. As you said, avoiding overlapping limits solves the problem , but this would not be possible if the total range of rotation was really supposed to be larger than 2*pi. I opened a PR ros/angles#16 to add support for "large limits" in shortest-angle calculations which should solve the issue.

[francofusco]

Just a short update about the PR ros/angles#16 I opened at angles: after the first review, I followed the suggestions and the "fix" for large rotation limits will be provided in the form of the function called shortest_angular_distance_with_large_limits. I believe it will be necessary to update some of the classes that use shortest_angular_distance_with_limits when dealing with "regular", i.e., non-continuous, revolute joints.

[aniruddhkb]

I can confirm that the joint oscillation is caused by overlapping joint limits. In my case, the problem manifested in even freakier behaviour -- the joints failed to move properly at all.

[francofusco]

The problem should have been fixed by #466, but I do not know if it is already available in the binaries as well... If not, you can always clone the controllers in your workspace and build them locally, the oscillations should then disappear :smiley:

By the way, @skrede can you check if the mentioned PR works for you as well (and close the issue in this case)?