ZMP and CoM error increasing in StabilizerTask when using Attitude observer

[mmurooka]

In the today's HRP2KAI experiments I performed with @Dwaaap, when the Attitude observer is added to observer pipeline, the robot gradually leans backward and falls down even though the robot was just standing with loco-manipulation controller and StabilizerTask. This was the first trial for us to use Attitude observer with loco-manipulation controller.

(I don't have graph now, but the output angles from the Attitude observer was also gradually increasing)

With removing attitude observer and use the Kalman filter RTC in hrpsys, this behavior did not happen. (We still see the small stationary offset but I think it's expected results)
https://gite.lirmm.fr/choreonoid-hrp-installation/hrp-choreonoid/-/blob/31567a4b531547fcb0bd78fb13208e84af8d67fa/projects/HRP2KAI/exp_mc.py#L222

[arntanguy]

@mmurooka,
cc @mehdi-benallegue

Has this been investigated further?
We've been using the Attitude observer with LIPMWalking and other robots and haven't noticed such a behaviour.

[mmurooka]

Has this been investigated further?

No.

We've been using the Attitude observer with LIPMWalking and other robots and haven't noticed such a behaviour.

Maybe I should check that this issue is reproduced with real robot again.
If necessary, you can close this issue until it is confirmed that the issue is reproduced.

[mehdi-benallegue]

We had a related issue one on HRP2-Kai but not in simulation. I tried to investigate it but I could not reproduce it with other robots and that robot was busy with experiments so I could not test it again. I remember some other issues were solved with the stabilization in the mean time (@mitsuharu-morisawa, @Dwaaap (?) ) so I am not sure about now.

[mitsuharu-morisawa]

What configuration options should we use for HRP5P, HRP2KAI and others ?
Can we use it as well as JVRC ?
It doesn't looks that the robot is occupied by the experiment for me.

[mmurooka]

What configuration options should we use for HRP5P, HRP2KAI and others ?

Just add following line to observers

  - type: Attitude

https://gite.lirmm.fr/jrp-2019/mc_hwm_planner/-/blob/master/etc/HwmController.in.yaml#L65

This worked both of HRP2KAI and HRP5P in choreonoid, and real HRP5P. For real HRP2KAI, the problem reported in this issue caused.

[arntanguy]

On HRP4LIRMM, we noticed that the orientation estimation of the Attitude observer drifts with time. And when used along with the StabilizerTask, this in turn leads to drift in DCM/ZMP. This could be related to this issue, and having both robots here I can try to investigate further.

[arntanguy]

I can confirm this bug on HRP2012C. I observed similar drift to what @mmurooka reported and to our previous observations with @antodld on HRP4LIRMM.

Here are the plots for two situations:

• HRP2KAI standing with a Posture controller, and the Attitude observer active. The plot is the RPY orientation estimated by the observer, and as you can see it drift quite fast and consistently. Corresponding log file: https://seafile.lirmm.fr/f/2168be2db8bf4f179bbb/?dl=1

• HRP2KAI standing with a StabilizerTask. The real robot slowly leans backwards and to its right until it eventually falls after a minute or so. I added the real robot's ZMP measurement and feet limits in dotted lines here such that you can see the robot leaning backwards. Corresponding log: https://seafile.lirmm.fr/f/db777c36e9da4fdd9bc6/?dl=1

@mehdi-benallegue Any idea what might be going wrong? I don't recall having noticed similar issues in the past, so it could be a regression.

[arntanguy]

In simulation, there seems to be a small drift initially, but it's much more subtle and it eventually stabilizes around a fixed value. Here is the plot for the robot standing still in half-sitting (no stabilizer) in choreonoid. Corresponding log: https://seafile.lirmm.fr/f/cfb53d9725ff4822982c/?dl=1

[mehdi-benallegue]

Is this last plot in simulation in degrees or in radians ? In any case the angle is very small, I don't think that it is a bug. The simulated robot cannot be perfectly vertical and I think it is normal to see the estimator converge to another value.

For me the only reasonable cause for this to happen is because there is a bias in the gyrometer. The default tuning of the estimator assumes that the gyro is of good quality (that is what we have on HRP-5P and I think also on HRP-4CR) and we don't have on HRP-2Kai (I don't know about HRP-4LIRMM). I implemented a feature to take the bias drift into account, and it seems it does not work yet).

I am not sure it is a regression, if my explanation is correct, the quality of the estimation depends a lot on the bias of the gyro, so it may appear differently from a trial to another. And it did happen to me on HRP-2Kai before according to the beginning of this thread.

[mehdi-benallegue]

I will check the logs, but I currently don't have my main PC for an undetermined amount of time...

[mmurooka]

Today, I tried Attitude observer again (without any changing) for HRP2001 real robot with @mitsuharu-morisawa and it worked. Walking stability was improved with Attitude observer. I don't know why it is working now. Details are https://gite.lirmm.fr/jrp-2019/mc_hwm_planner/-/issues/44#note_26514

[mmurooka]

We have a clue as to the cause of this. Summarizing, the estimated orientation is drifting due to a slight offset in the angular velocity measured by the gyro in real HRP2KAI.

The following log of Accelerometer_angularVelocity shows that it is oscillating but the average value is slightly offset from zero.
#6 (comment)

HRP2KAI standing with a Posture controller, and the Attitude observer active. The plot is the RPY orientation estimated by the observer, and as you can see it drift quite fast and consistently. Corresponding log file: https://seafile.lirmm.fr/f/2168be2db8bf4f179bbb/?dl=1

The offset of angular velocity is approximately -0.001 for the x component and 0.001 for the y component. If I manually add the same offset to the following line (i.e., + Eigen::Vector3d(-0.001, 0.001, 0.0)), the same drift issue of the estimated orientation was reproduced in the Choreonoid simulation as shown in the following graph.

mc_state_observation/src/AttitudeObserver.cpp

Line 124 in 1817589

measurement.tail<3>() = rateIn;

#17 will probably solve this issue, but it seems to be still WIP. Would the simple solution of increasing the covariance of the gyro also work?

[mmurooka]

Would the simple solution of increasing the covariance of the gyro also work?

Drift of the estimated orientation was reduced and saturated with the following configurations even with gyro offset in the Choreonoid.

    - type: Attitude
      config:
        KalmanFilter:
          gyr_cov: 1e-6 # default is 1e-10

@arntanguy If you have time, could you try it with the real HRP2KAI?

[arntanguy]

That's great, thanks for the investigation!
I will give it a try tomorrow as I am not in the lab today.

[arntanguy]

With the suggested configuration gyr_cov: 1e-6 the Attitude observer no longer diverges with HRP2012c. We were able to walk using LIPMWalking controller.

Here is the attitude observer during the whole demo (some pushes at the start, and walking in place then forward):

And here is a forward walk. As you can see the DCM and ZMP are no longer diverging.

[mmurooka]

@arntanguy Thanks for the real robot test. How is the stability of the walking compared to before (using RTC's KalmanFilter)?

[mehdi-benallegue]

Antonin and I made also long tests on HRP2 here, and with a value of gyr_cov: 1e-6 we were able to perform dynamic stepping in closed-loop.

For the comparison between AttitudeEstimator and RTC's Kalman Filter, it may be kind of similar in usual walking motions (since the original Kalman filter was good enough for the DRC. However, the difference would be very clear for quick changes in orientation, e.g. if you incline the torso, the previous KF would be too slow, and usually, the robot falls. That is why it was decided to use the Attitude estimator as default since 2017. If you could include this motion if you perform tests, it would be useful.

[mmurooka]

@mehdi-benallegue Thank you for the explanation. I agree about the improvement in tilting posture.

My question is a bit more robot specific (and maybe I shouldn't be doing this in this Issue), but we are having some stability issues in (not so dynamic) walking with the RTC's KalmanFilter in JRP loco-manipulation at HRP2012. So we tried AttitudeObserver, but before changing the parameters, it diverged. The question was whether AttitudeObserver now solved that problem for HRP2012. The details of this issue are tracked at here (in the closed repo).

[mehdi-benallegue]

Oh I did not know about this discussion. Thank you!

By the way, @antodld told me that I made a mistake and that the final value that was used was also gyr_cov: 1e-6, so I fixed my previous comment.