base controller never achieves velocities in gazebo

[dHonerkamp]

Hi,

I am starting the pr2 with all default configurations in gazebo (ROS noetic). I then send twist commands to the base controller as outlined in the tutorial (http://wiki.ros.org/pr2_controllers/Tutorials/Using%20the%20robot%20base%20controllers%20to%20drive%20the%20robot0), vaying the velocities and publishing them at 50 hz.

Doing so I find that:

• the max linear velocity is at 0.26, not the specified 2.0
• for any linear velocity I send, the controller consistently achieves exactly 0.6 * this velocity
• the max angular velocity is around 0.38
• for any angular velocity I send, the controller achieves roughly 0.5 * this velocity
• independent of the final velocities, acceleration is also very slow. I'm not sure if this might be due to large inertias or if the controller gains are just very suboptimal
• on the real robot (though running ROS hydro) we do not have any of these problems, nor any large inertias

Somewhat related to #392 though the differences in gazebo are just so much larger that I assume it is something else.

I was wondering if anyone experienced the same and or what configurations I might best look at to solve this? There is at least one other person encountering the same issue, but no solution: https://stackoverflow.com/questions/64525521/gazebo-simulated-pr2-robot-maximum-speed-is-not-respected

[v4hn]

Hi there,

out of curiosity, which lab are you from and did you recently test the navigation behavior as done in #392?
The last time somebody touched the relevant controller configurations was 10 years ago, so I'm not surprised they are far from ideal with a recent gazebo. Honestly I wouldn't be surprised if they never worked well to begin with.

So as a first step you should definitely try to tweak these configurations and see whether/how it changes behavior.

[dHonerkamp]

Thanks for the reply. I'm with the Robot Learning Lab in Freiburg, Germany.

That I can very much imagine. But I don't think a suboptimal configuration can explain why it never even comes close to achieve the target velocity.

I've been digging in the code a bit now, and I'm not sure if I misunderstand it, or whether the PID arguments for the wheels are completely wrong:

    double command = wheel_pid_controllers_[i].computeCommand(
          - wheel_caster_steer_component.linear.x/base_kinematics_.wheel_[i].wheel_radius_,
          base_kinematics_.wheel_[i].wheel_speed_cmd_ - filtered_wheel_velocity_[i],
          ros::Duration(dT));

where the signature is Pid::computeCommand(double error, double error_dot, ros::Duration dt). I'm not sure how the first argument would ever be the error and also logging it, the values are always almost 0. As such I am very surprised this works as all, unless I really misunderstand something.

I've replaced it with

    double command = wheel_pid_controllers_[i].computeCommand(
              error,
              base_kinematics_.wheel_[i].wheel_speed_cmd_ - filtered_wheel_velocity_[i],
              ros::Duration(dT));

and now it actually (almost) achieves the commanded velocity

[v4hn]

Sorry, I don't have the cycles to look at that in detail... Could you test your code on the physical robot as well and provide controller parameters (assuming they need to be adapted)? I will try this on our own robot then and if it works well we can merge it upstream. This snippet got attention in the past as well and it's definitely possible that this is plain wrong. It's astonishing how well it works on the physical system though, so it's hard to believe!

[dHonerkamp]

I completely understand.

If we manage to test this on the actual robot, I'll let you know, though I am a bit afraid to test it there, also given that it is working well there as is.

I was also a bit too fast. With this change I achieve velocities up to 0.25m/s in gazebo, but the robot is still not able to move any faster, no matter how large the output of the controller (I guess it gets capped at the effort limits anyway). Is it possible that some things in the gazebo physics changed so that the original efforts are just no longer sufficient to move the robot appropriately in simulation? And you wouldn't have any hints at what parameters to best look at for this?

[v4hn]

If we manage to test this on the actual robot, I'll let you know, though I am a bit afraid to test it there, also given that it is working well there as is.

Yes, that's why I will not be the first to test changes in this code. :) It might be a good idea to jack up the robot for such initial experiments. But it's not necessarily working perfectly on the real system as observed in the other issue.

Is it possible that some things in the gazebo physics changed so that the original efforts are just no longer sufficient to move the robot appropriately in simulation?

That's absolutely possible! As I said, sadly nobody modified the PR2 side of the simulation in a long time. :/ I don't know what parameters best to adapt here, but I would not aim for physical plausibility. The spring mechanism in the arm lift joints is modelled by disabling gravity for the arms, so there is not much plausible about the model anyway...