Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Closed
fjp opened this issue Mar 9, 2021 · 3 comments · Fixed by #32
Closed

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

fjp opened this issue Mar 9, 2021 · 3 comments · Fixed by #32
Labels
bug Something isn't working help wanted Extra attention is needed

Comments

@fjp
Copy link
Member

fjp commented Mar 9, 2021

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 #24 (comment)
@fjp fjp added bug Something isn't working help wanted Extra attention is needed labels Mar 9, 2021
@fjp
Copy link
Member Author

fjp commented Mar 20, 2021 •
edited
Loading

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: <?xml version="1....
 * /rosdistro: noetic
 * /rosversion: 1.15.8

NODES
  /diffbot/
    controller_spawner (controller_manager/spawner)
    diffbot_base (diffbot_base/diffbot_base)
    motor_driver (grove_motor_driver/motor_driver.py)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rosserial_teensy (rosserial_python/serial_node.py)

ROS_MASTER_URI=http://192.168.0.9:11311/

process[diffbot/diffbot_base-1]: started with pid [17477]
process[diffbot/controller_spawner-2]: started with pid [17478]
process[diffbot/motor_driver-3]: started with pid [17479]
process[diffbot/rosserial_teensy-4]: started with pid [17481]
[ INFO] [1616228464.266134126]: Waiting for model URDF on the ROS param server at location: /diffbot/diffbot/robot_description
process[diffbot/robot_state_publisher-5]: started with pid [17486]
[ 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         

[ INFO] [1616228465.246231233]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.247149151]: 
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.335010634]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.335895109]: 
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.363893]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1616228465.380923]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1616228465.395484]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1616228465.422343]: Loading controller: joint_state_controller
[ INFO] [1616228465.434971120]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.435653434]: 
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.469947]: ROS Serial Python Node
[INFO] [1616228465.500385]: Connecting to /dev/ttyACM0 at 115200 baud
[ INFO] [1616228465.534924051]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.535581920]: 
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.542683]: Loading controller: mobile_base_controller
[INFO] [1616228465.557130]: Ready to receive motor commands
[ INFO] [1616228465.577144573]: Controller state will be published at 50Hz.
[ INFO] [1616228465.586393732]: Wheel separation will be multiplied by 1.
[ INFO] [1616228465.602278866]: Left wheel radius will be multiplied by 1.
[ INFO] [1616228465.602826128]: Right wheel radius will be multiplied by 1.
[ INFO] [1616228465.607057432]: Velocity rolling window size of 10.
[ INFO] [1616228465.610708512]: Velocity commands will be considered old if they are older than 0.5s.
[ INFO] [1616228465.614280946]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1616228465.620952216]: Base frame_id set to base_footprint
[ INFO] [1616228465.625282295]: Odometry frame_id set to odom
[ INFO] [1616228465.632935259]: Publishing to tf is enabled
[ INFO] [1616228465.638121295]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.638587948]: 
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.734892539]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.735207549]: 
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.736237482]: Odometry params : wheel separation 0.15, left wheel radius 0.0325, right wheel radius 0.0325
[ INFO] [1616228465.756304884]: Adding left wheel with joint name: front_left_wheel_joint and right wheel with joint name: front_right_wheel_joint
[ INFO] [1616228465.835021187]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.839560315]: 
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.844812941]: Dynamic Reconfigure:
DynamicParams:
	Odometry parameters:
		left wheel radius multiplier: 1
		right wheel radius multiplier: 1
		wheel separation multiplier: 1
	Publication parameters:
		Publish executed velocity command: disabled
		Publication rate: 50
		Publish frame odom on tf: enabled
[ INFO] [1616228465.934890138]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228465.935220610]: 
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.936268]: Controller Spawner: Loaded controllers: joint_state_controller, mobile_base_controller
[ INFO] [1616228466.034998008]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.035638768]: 
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] [1616228466.049382]: Started controllers: joint_state_controller, mobile_base_controller
[ INFO] [1616228466.134863385]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.135488349]: 
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] [1616228466.235012902]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.235438520]: 
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] [1616228466.334857391]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.335196344]: 
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] [1616228466.434844969]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.435223828]: 
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] [1616228466.534915192]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.535443528]: 
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] [1616228466.634966934]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.635697579]: 
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] [1616228466.734903309]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.735318963]: 
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] [1616228466.834878127]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.836404398]: 
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] [1616228466.934881593]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228466.935454373]: 
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] [1616228467.034978815]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.035494263]: 
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] [1616228467.134896764]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.135331437]: 
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] [1616228467.234983357]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.235677855]: 
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] [1616228467.335104245]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.335953943]: 
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] [1616228467.435015787]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.435721433]: 
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] [1616228467.534838739]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.535256338]: 
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] [1616228467.614292]: Requesting topics...
[ INFO] [1616228467.634893185]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.635381059]: 
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] [1616228467.642180]: Note: publish buffer size is 512 bytes
[INFO] [1616228467.652554]: Setup publisher on encoder_ticks [diffbot_msgs/Encoder]
[INFO] [1616228467.679633]: Note: subscribe buffer size is 512 bytes
[INFO] [1616228467.687515]: Setup subscriber on reset [std_msgs/Empty]
[ INFO] [1616228467.734871484]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.735316027]: 
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] [1616228467.834884060]: 
Read      ticks     angle     dangle    velocity  
j0:       -707135280-8.19753e+060         0         
j1:       65535     759.721   0         0         
[ INFO] [1616228467.835241661]: 
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] [1616228467.934888137]: 
Read      ticks     angle     dangle    velocity  
j0:       -5010     -58.0789  8.19747e+068.19972e+07
j1:       27608     320.048   -439.672  -4397.93  
[ INFO] [1616228467.935462694]: 
Write     velocity  p_error   i_error   d_error   pid out   percent   
j0:       0         -8.19972e+07-8.19747e+06-8.20197e+08-3.6901e+07-5.99641e+08
j1:       0         4397.93   439.672   43991.3   1982.69   32218.7   

[ INFO] [1616228468.034861437]: 
Read      ticks     angle     dangle    velocity  
j0:       -5038     -58.4035  -0.324593 -3.24519  
j1:       27636     320.373   0.324593  3.24519   
[ INFO] [1616228468.035243203]: 
Write     velocity  p_error   i_error   d_error   pid out   percent   
j0:       0         3.24519   -8.19747e+068.19787e+088.19787e+061.33215e+08
j1:       0         -3.24519  439.348   -44001.8  -437.654  -7111.87  

[ INFO] [1616228468.134896549]: 
Read      ticks     angle     dangle    velocity  
j0:       -5024     -58.2412  0.162296  1.62269   
j1:       27621     320.199   -0.173889 -1.73859  
[ INFO] [1616228468.135283612]: 
Write     velocity  p_error   i_error   d_error   pid out   percent   
j0:       0         -1.62269  -8.19747e+06-48.6705  -4.55465  -74.013   
j1:       0         1.73859   439.522   49.8294   4.6068    74.8605

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 fjp mentioned this issue Mar 20, 2021
Open
fjp added a commit that referenced this issue Mar 21, 2021
@fjp
WIP: test base hardware interface (#30)
b7e7bc8
fjp added a commit that referenced this issue Mar 26, 2021
@fjp
fix zero time period at control loop start (#30)
b05f9c0
fjp added a commit that referenced this issue Mar 26, 2021
@fjp
fix: Initialize encoder_ticks_ in hw interface
1232352 
Initialize values in encoder_ticks_ member array of hardware interface
to zero because receiving meaningfultick values from the microcontroller
might take some time. Alleviates problems in #30
@fjp
Copy link
Member Author

fjp commented Mar 28, 2021

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 encoder_ticks_ 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
Copy link
Contributor

Russ76 commented Mar 29, 2021

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.)

fjp added a commit that referenced this issue Mar 31, 2021
@fjp
change node execution order in bringup launch file
cb5ce1c 
To fix issues in #30 launch the encoder scrip on the
microcontroller before launching the hardware interface
control loop. This will make sure to send actual tick values
from the encoders to the hardware interface before the control
loop is running.
@fjp fjp mentioned this issue Apr 12, 2021
Merged
@fjp fjp closed this as completed in #32 Apr 30, 2021
fjp added a commit that referenced this issue Apr 30, 2021
@fjp
fix zero time period at control loop start (#30)
74e0005
fjp added a commit that referenced this issue Apr 30, 2021
@fjp
fix: Initialize encoder_ticks_ in hw interface
59978e6 
Initialize values in encoder_ticks_ member array of hardware interface
to zero because receiving meaningfultick values from the microcontroller
might take some time. Alleviates problems in #30
fjp added a commit that referenced this issue Apr 30, 2021
@fjp
change node execution order in bringup launch file
acdcf28 
To fix issues in #30 launch the encoder scrip on the
microcontroller before launching the hardware interface
control loop. This will make sure to send actual tick values
from the encoders to the hardware interface before the control
loop is running.
fjp added a commit that referenced this issue Apr 30, 2021
@fjp
fix #30: add method to check if encoder ticks are published
f4646a8
fjp added a commit that referenced this issue May 19, 2021
@fjp
fix zero time period at control loop start (#30)
74776fe
fjp added a commit that referenced this issue May 19, 2021
@fjp
fix: Initialize encoder_ticks_ in hw interface
8c73f22 
Initialize values in encoder_ticks_ member array of hardware interface
to zero because receiving meaningfultick values from the microcontroller
might take some time. Alleviates problems in #30
fjp added a commit that referenced this issue May 19, 2021
@fjp
change node execution order in bringup launch file
f13440f 
To fix issues in #30 launch the encoder scrip on the
microcontroller before launching the hardware interface
control loop. This will make sure to send actual tick values
from the encoders to the hardware interface before the control
loop is running.
fjp added a commit that referenced this issue May 19, 2021
@fjp
fix #30: add method to check if encoder ticks are published
a098e9c
fjp added a commit that referenced this issue May 20, 2021
@fjp
WIP: test base hardware interface (#30)
e25393f
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
bug Something isn't working help wanted Extra attention is needed
Projects
None yet
Milestone
No milestone
Development

Successfully merging a pull request may close this issue.

Wait for encoder ticks being published in ROS control hardware interface ros-mobile-robots/diffbot
2 participants
@fjp @Russ76