Initialization problem on real robot: wheels immediately start rotating after bringup

fjp commented 3 years ago

I am using "diffbot_bringup_with_laser" and I wonder why the tracks are turning? Shouldn't it wait for a navigation package to be launched? Move_base isn't running yet, is it?

This is a bug, I encounter the same problem on my robot. I suspect missing zero initializations in the diffbot_hw_interface.cpp but couldn't find the exact problem so far and didn't investiage further yet. I do set the joint states and commands to zero here in the code at the beginning but it doesn't seem to help.

Another source of this problem might be the motor driver code.

What I do to "solve" this, is to send an empty message on the /diffbot/reset topic to the encoders that might not be zero while the wheels spin. And then in my case I can stop the wheels by just blocking the wheels of the robot during this resetting, or I also make use of the motor driver reset button.

Originally posted by @fjp in https://github.com/fjp/diffbot/issues/24#issuecomment-794489216

fjp commented 3 years ago

Hardware Interface log

``` fjp@diffbot:~/catkin_ws$ roslaunch diffbot_bringup bringup.launch ... logging to /home/fjp/.ros/log/82733adc-8954-11eb-a952-438647b07a39/roslaunch-diffbot-17421.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB. started roslaunch server http://192.168.0.26:37407/ SUMMARY ======== PARAMETERS * /diffbot/hardware_interface/joints: ['front_left_whee... * /diffbot/joint_state_controller/extra_joints: [{'name': 'rear_c... * /diffbot/joint_state_controller/publish_rate: 50 * /diffbot/joint_state_controller/type: joint_state_contr... * /diffbot/mobile_base_controller/angular/z/has_acceleration_limits: True * /diffbot/mobile_base_controller/angular/z/has_velocity_limits: True * /diffbot/mobile_base_controller/angular/z/max_acceleration: 6.0 * /diffbot/mobile_base_controller/angular/z/max_velocity: 2.0 * /diffbot/mobile_base_controller/base_frame_id: base_footprint * /diffbot/mobile_base_controller/left_wheel: front_left_wheel_... * /diffbot/mobile_base_controller/linear/x/has_acceleration_limits: True * /diffbot/mobile_base_controller/linear/x/has_velocity_limits: True * /diffbot/mobile_base_controller/linear/x/max_acceleration: 0.6 * /diffbot/mobile_base_controller/linear/x/max_velocity: 0.2 * /diffbot/mobile_base_controller/pose_covariance_diagonal: [0.001, 0.001, 0.... * /diffbot/mobile_base_controller/publish_rate: 50 * /diffbot/mobile_base_controller/right_wheel: front_right_wheel... * /diffbot/mobile_base_controller/twist_covariance_diagonal: [0.001, 0.001, 0.... * /diffbot/mobile_base_controller/type: diff_drive_contro... * /diffbot/mobile_base_controller/wheel_radius: 0.0325 * /diffbot/mobile_base_controller/wheel_separation: 0.15 * /diffbot/robot_description:

The problem might be that the encoder values are not zero after launching the bringup package. And also the read velocity in the hardware interface is something quite high at the start:

[ INFO] [1616228464.459879850]: Initializing DiffBot Hardware Interface ...
[ INFO] [1616228464.460056253]: Number of joints: 2
[ INFO] [1616228464.460419242]: pid namespace: pid/left_motor
[ INFO] [1616228464.460576090]: Initialize PID
[ INFO] [1616228464.460676420]: Initializing dynamic reconfigure in namespace /diffbot/pid/left_motor
[ INFO] [1616228464.557533459]: Update PID Gains: F=0.8, P=0.35, I=0.5, D=0.01, out_min=-6.15385, out_max=6.15385
[ INFO] [1616228464.586639267]: Initialized dynamic reconfigure
[ INFO] [1616228464.586828372]: Initialized PID: F=0.8, P=0.35, I=0.5, D=0.01, out_min=-6.15385, out_max=6.15385
[ INFO] [1616228464.586925684]: Update PID output limits: lower=6.15385, upper=-6.15385
[ INFO] [1616228464.587086976]: pid namespace: pid/right_motor
[ INFO] [1616228464.587175251]: Initialize PID
[ INFO] [1616228464.587251823]: Initializing dynamic reconfigure in namespace /diffbot/pid/right_motor
[ INFO] [1616228464.680847326]: Update PID Gains: F=0.8, P=0.35, I=0.5, D=0.01, out_min=-6.15385, out_max=6.15385
[ INFO] [1616228464.706535566]: Initialized dynamic reconfigure
[ INFO] [1616228464.706727079]: Initialized PID: F=0.8, P=0.35, I=0.5, D=0.01, out_min=-6.15385, out_max=6.15385
[ INFO] [1616228464.706817392]: Update PID output limits: lower=6.15385, upper=-6.15385
[ INFO] [1616228464.707294470]: ... Done Initializing DiffBot Hardware Interface
[ INFO] [1616228464.934849034]:
Read ticks angle dangle velocity
j0: -707135280-8.19753e+06-8.19753e+06-1.32926e+12
j1: 65535 759.721 759.721 1.23191e+08
[ INFO] [1616228464.935248097]:
Write velocity p_error i_error d_error pid out percent
j0: 0 1.32926e+128.19753e+062.15544e+172.1559e+153.50334e+16
j1: 0 -1.23191e+08-759.721 -1.99759e+13-1.99802e+11-3.24678e+12

[ INFO] [1616228465.034950868]:
Read ticks angle dangle velocity
j0: -707135280-8.19753e+060 0
j1: 65535 759.721 0 0
[ INFO] [1616228465.035816139]:
Write velocity p_error i_error d_error pid out percent
j0: 0 0 0 0 0 0
j1: 0 0 0 0 0 0

[ INFO] [1616228465.134998480]:
Read ticks angle dangle velocity
j0: -707135280-8.19753e+060 0
j1: 65535 759.721 0 0
[ INFO] [1616228465.135785402]:
Write velocity p_error i_error d_error pid out percent
j0: 0 0 0 0 0 0
j1: 0 0 0 0 0 0

At the start the set_velocity from the controller should be zero. But as you can see in the log at the first read cycle:

[ INFO] [1616228464.934849034]:
Read ticks angle dangle velocity
j0: -707135280-8.19753e+06-8.19753e+06-1.32926e+12
j1: 65535 759.721 759.721 1.23191e+08
[ INFO] [1616228464.935248097]:
Write velocity p_error i_error d_error pid out percent
j0: 0 1.32926e+128.19753e+062.15544e+172.1559e+153.50334e+16
j1: 0 -1.23191e+08-759.721 -1.99759e+13-1.99802e+11-3.24678e+12

this velocity difference (set_vel = 0, measured_vel = "something high") leads to a pid out that is much too high.

fjp commented 3 years ago

I think I found the problem and work on it in the hw_init branch. After it is fixed completely I will do a PR to the main noetic-devel branch.

There are/were mainly three errors causing the problem:

division by zero time period in the first cycle of the control loop start (this is fixed in commit b05f9c0)
the encoderticks variable wasn't initialized correctly (fixed by 1232352)
The node running on the Teensy microcontroller starts to publish encoder values after the hardware interface control loop (read, update write) is already running. This can cause a jump in encoder tick values, e.g. from zero to whatever value is first published from the Teensy.

For the last unsolved error I think the best way to solve it, is to let the hw intf control loop wait for the encoder messages being published. Another way in solving this, could be to send the /reset message from the hardware interface just before the first loop cycle starts. This will lead to zero ticks and therefore no jump.

Russ76 commented 3 years ago

Sounds like you're fixing it, Franz! I'll have to check through my Teensy code when you finish, as mine is controlling the motors, too. (No Grove device.)

ros-mobile-robots / diffbot

Initialization problem on real robot: wheels immediately start rotating after bringup #30