Derivative error issue in the effort-based joint position controller

[jim-rothrock]

I have found that joints that use the effort-based joint position controller don't settle as quickly as I would expect. Here is a test case:

1. Go to a directory in your ROS path.
2. git clone https://github.com/jack-oquin/ackermann_teleop.git
3. git clone https://github.com/wunderkammer-laboratory/ackermann_vehicle.git
4. roslaunch ackermann_vehicle_gazebo ackermann_vehicle.launch namespace:=/ cmd_timeout:=0
5. rosrun ackermann_qt qteleop.py

Use the GUI to drive the vehicle back and forth. Most important, click the "brake" button to bring the vehicle to a sudden stop. You'll see the chassis oscillate quite a bit, as if the shock absorbers aren't doing enough damping.

If you go into joint_position_controller.cpp and remove vel_error from the call to computeCommand(), then rebuild and drive the vehicle again, you'll notice that the shock absorbers have much better damping characteristics. In fact, they are doing exactly what I'd expect, given their parameters. Perhaps JointPositionController::update() could be modified so that it would call the two-parameter version of computeCommand() if the command originated from a call to setCommandCB().

[adolfo-rt]

I think I know what the underlying problem is here. The controller has two setCommand overloads, one that accepts position-only commands, and one that accepts position-velocity commands. It turns out that the only one being called from the ROS API is the former, the position-only command (here, in the callback). The bad thing about the position-only overload is that it is setting zero desired velocity and sending that to Pid.computeCommand in the update.

This explains why you have a slower response, the Pid is always trying to reach the position command with zero velocity. I see two solutions to fix your problem:

• Estimate of the desired velocity by finite differences in the position-only setCommand overload (rather that using always zero).
• Do some bookkeeping so update(...) knows if the last command is position-only or position-velocity, and can call the appropriate Pid.computeCommand() overload in turn.

@davetcoleman was using this controller recently. Dave, did you stumble upon this issue?, or were you calling directly the setCommand() overload with position-velocity data (ie. not using the ROS API)?.

[davetcoleman]

I believe I caused this inadvertently. Will submit fix today/tomorrow.

[davetcoleman]

I do not have a setup right now to actually run and test this fix, but let me know if it solves your problem. I created a new flag that indicates which setCommand was called and uses the correct computeCommand based on its value.

[jim-rothrock]

Yes, the new code fixes the problem.

[davetcoleman]

Great!