Sometimes the IMU does not provide any data (all zeros) when used through the master-board network.

[Lhumd]

I am trying to use IMU for Bolt. The problem is that sometimes base_attitude_ is equal to zero. The behavior is changing with different compiling. For example, I added some prints, then it works, and by adding more prints, it doesn't work.

To me, it doesn't seem the problem comes from hardware or connection because master_board_example works fine.

[MaximilienNaveau]

The tests to be done in order to check this:

• run the example.cpp controller from the master_board_sdk with:
  • 5 times in release
  • 5 times in debug
• run the demo from bolt where we simply print stuff (coding it if needed):
  • 5 times in release
  • 5 times in debug

report the results here: (imu working or not, any error that pops up, etc.)

[Lhumd]

In debug and release mode all of five times (I didn't recompile it) in example.cpp, attitude was a nonzero number, and attitude was zero in hardware_calibration.

[MaximilienNaveau]

ok then there is a bug in bolt/odri
Just to make sure @Lhumd : you modified the hardware_calibration executable to print the imu data?
We should use the bolt_demo_pd.cpp in order to do this it would be cleaner.
In any case can you write here the code lines you added to read/print the imu data?

[Lhumd]

I changed these lines.
https://github.com/open-dynamic-robot-initiative/bolt/blob/edaneshmand/ament/src/bolt.cpp#L142-#L146
to

if (!(base_attitude_(1) < 0.6 && base_attitude_(1) > -0.6 &&
       (base_attitude_(0) < -2.5 || base_attitude_(0) > 2.5)))
   {
       std::cout << base_attitude_ << std::endl;
       robot_->ReportError("Base attitude not in the defined parameter.");
   }

[jviereck]

Hi, can you try to print the imu_attitude before converting it back and forth? What I mean is adding the lines like this:

static int counter = 0;

if (counter++ % 2000 == 0) {
  std::cout << "IMU attitude: " << robot_if_->imu_data_attitude(0) << "," << robot_if_->imu_data_attitude(1) << "," robot_if_->imu_data_attitude(2) << std::endl;
}

in IMU::ParseSensorData

https://github.com/open-dynamic-robot-initiative/odri_control_interface/blob/main/src/imu.cpp#L80

This will print the raw imu attitude as provided from the master board. If this is non-zero but bolt's base_attitude_ is zero, then we know something is wrong with the conversion code.

[MaximilienNaveau]

I created a debug demo for bolt:
https://github.com/open-dynamic-robot-initiative/bolt/blob/edaneshmand/ament/demos/bolt_demo_sensor_reading.cpp

This is the demo you should run @Lhumd

[MaximilienNaveau]

@Lhumd Can you do what @jviereck said and run the new demo?

[Lhumd]

Hi, can you try to print the imu_attitude before converting it back and forth? What I mean is adding the lines like this:

static int counter = 0;

if (counter++ % 2000 == 0) {
  std::cout << "IMU attitude: " << robot_if_->imu_data_attitude(0) << "," << robot_if_->imu_data_attitude(1) << "," robot_if_->imu_data_attitude(2) << std::endl;
}

in IMU::ParseSensorData

https://github.com/open-dynamic-robot-initiative/odri_control_interface/blob/main/src/imu.cpp#L80

This will print the raw imu attitude as provided from the master board. If this is non-zero but bolt's base_attitude_ is zero, then we know something is wrong with the conversion code.

It prints zero.

[jviereck]

Maybe stupid question, but the other measurements like joints etc are all okay? I don't understand why the IMU is not working when using this interface reading the raw values from the robot_if but things work fine when running the low-level example.

[Lhumd]

The only thing it seems working is the slider!
It is the result of running max's code.\

des joint_tau  : : [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, ]
act joint_pos                  : : [-0.000, -0.000, -0.000, -0.000, 0.000, 0.000, ]
act joint_vel                  : : [-0.000, -0.000, -0.000, -0.000, 0.000, 0.000, ]
act joint torq                 : : [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, ]
act joint target torq          : : [-0.000, -0.000, -0.000, -0.000, 0.000, 0.000, ]
act status motor ready         : : [False, False, False, False, False, False, ]
act status motor enabled       : : [False, False, False, False, False, False, ]
act status motor board enabled : : [False, False, False, ]
act status motor board errors  : : [0, 0, 0, ]
act slider pos                 : : [0.789, 0.000, 0.000, 0.000, ]
act imu quat                   : : [0.000, 0.000, 0.000, 1.000, ]
act imu rpy                    : : [0.000, 0.000, 0.000, ]
act imu acc                    : : [0.000, 0.000, 0.000, ]
act imu gyroscope              : : [0.000, 0.000, 0.000, ]
act imu lin acc                : : [0.000, 0.000, 0.000, ]
act e-stop                     : false

[Lhumd]

My print doesn't include a counter to make sure that we are not loosing realtime. Could it be the problem?
If I add a counter, all parameters will be nonzero after a while.
Something like this:

Sensor reading loop started 
Lhum 0
0
0
ERROR: Base attitude not in the defined parameter.
des joint_tau  : : [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, ]
act joint_pos                  : : [-0.000, -0.000, -0.000, -0.000, 0.000, 0.000, ]
act joint_vel                  : : [-0.000, -0.000, -0.000, -0.000, 0.000, 0.000, ]
act joint torq                 : : [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, ]
act joint target torq          : : [-0.000, -0.000, -0.000, -0.000, 0.000, 0.000, ]
act status motor ready         : : [False, False, False, False, False, False, ]
act status motor enabled       : : [False, False, False, False, False, False, ]
act status motor board enabled : : [False, False, False, ]
act status motor board errors  : : [0, 0, 0, ]
act slider pos                 : : [0.789, 0.000, 0.000, 0.000, ]
act imu quat                   : : [0.000, 0.000, 0.000, 1.000, ]
act imu rpy                    : : [0.000, 0.000, 0.000, ]
act imu acc                    : : [0.000, 0.000, 0.000, ]
act imu gyroscope              : : [0.000, 0.000, 0.000, ]
act imu lin acc                : : [0.000, 0.000, 0.000, ]
act e-stop                     : false
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
.
.
.
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
ERROR: Base attitude not in the defined parameter.
des joint_tau  : : [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, ]
act joint_pos                  : : [0.001, 0.001, 0.001, 0.001, -0.001, -0.001, ]
act joint_vel                  : : [0.006, -0.000, 0.006, 0.006, -0.006, -0.006, ]
act joint torq                 : : [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, ]
act joint target torq          : : [-0.004, -0.004, 0.000, -0.002, -0.002, 0.003, ]
act status motor ready         : : [False, False, False, False, False, False, ]
act status motor enabled       : : [True, True, True, True, True, True, ]
act status motor board enabled : : [True, True, True, ]
act status motor board errors  : : [0, 0, 0, ]
act slider pos                 : : [0.789, 0.000, 0.000, 0.000, ]
act imu quat                   : : [-1.000, 0.001, -0.007, 0.003, ]
act imu rpy                    : : [-3.136, -0.014, -0.001, ]
act imu acc                    : : [-0.115, 0.014, 9.835, ]
act imu gyroscope              : : [0.002, -0.002, 0.000, ]
act imu lin acc                : : [-0.021, -0.083, 0.032, ]
act e-stop                     : false

[jviereck]

It takes 5 seconds to initialize the motor boards (calibrate the motors). Is that the period where you see zero values or does it take longer until you see non-zero values?

[Lhumd]

Approximately, yes.
When I ran max's code without a counter for my print, all parameters will never be nonzero.

[MaximilienNaveau]

That's what I suspect there a wait until ready that I missed somewhere, wait!

[MaximilienNaveau]

I pushed a fix that is waiting until the robot is ready to perform any action in the demo.
Hence this should fix the issue.

For the DGM the wait is done in the main loop inside the (send_command)
This init logic might still be flawed for the dgm I will double check.

[MaximilienNaveau]

@Lhumd can you try again updating the code this should work now.

[MaximilienNaveau]

@ALL I am not sure what is the status of this issue, does people still see the imu not going up sometimes?

[jviereck]

On solo12, I had the IMU not engage 1 out of roughly 10 times. What I do in the code is check during startup if the IMU reads zeros. If that's the case, I print an error message and reboot the robot.

# Check if the IMU is working.
imu_gyro = head.get_sensor('imu_gyroscope')
if np.all(imu_gyro == np.zeros(3)):
    print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
    print('!!!                                                       !!!')
    print('!!!                 IMU IS NOT WORKING                    !!!')
    print('!!!                                                       !!!')
    print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')

    print('imu_gyro:', imu_gyro)

[MaximilienNaveau]

👍
Then this issue will still stay up until further notice.