Interest in this package

[MaximilienNaveau]

Dear developers,

I am interested in this package.
I saw this fork:

• https://github.com/ArnaudDmt/state-observation/

And I saw that your fork is also from @mehdi-benallegue.
I was wondering which of the 3 packages were up to date with respect to the kinetics observer.

Thanks for the informations.

Best
M

[mehdi-benallegue]

Hello Maximilien, 

Nice to hear from you! I hope you are doing well.

@ArnaudDmt, please gide him through the versions.

Mehdi-

[MaximilienNaveau]

Hello @mehdi-benallegue ,

I hope you are doing well too!
I read your paper on the kinetics observer and it was very interesting.

@ArnaudDmt, could you please also indicate me a place where you actually use the kinetics observer.

It's a little difficult to understand the articulation and order of methods to call.
Actually I had one question about it.
In the code there are no dependencies toward a physics engine like Pinocchio or RBDL in order to compute the forward kinematics or jacobians etc.
I was wondering if it's a design thing. That the observer expect the user to compute the forward kinematics before the usage of the observer.

[ArnaudDmt]

Dear Maximilien, thank you for your interest in our work !
The most up-to-date code of the Kinetics Observer is that in my fork, in the addWaiko branch (sorry, I still haven't had time to clean up the repository).
If you want an implementation example, please have a look at its implementation for mc_rtc.

Here are the main functions to be called:

• setInitWorldCentroidStateVector: sets the initial state of the Kinetics Observer.
• addContact: creates a new contact that will be appended to the state vector.
• removeContact: removes the contact from the Kinetics Observer.
• updateContactWithNoSensor: updates the contact kinematics and measurements. / updateContactWithWrenchSensor: updates the contact kinematics and wrench measurements.
• setImu: add / update the imu with measurements and kinematics
• setCenterOfMass: updates the center of mass kinematics in the user frame (in the mc_rtc implementation: the floating base frame)
• setCoMAngularMomentum: updates the robot's angular momentum at the centroid frame.
• setCoMInertiaMatrix: updates the robot's inertia matrix, expressed at the centroid frame.
• inputAdditionalWrench: adds all the wrench measurements that are not associated with contacts as inputs
• update: runs the new iteration of the Kinetics Observer.

If needed, you can use the ContactsManager tool, also available in state-observation, which given the list of current contacts, runs the appropriate lambda functions on new, removed, and maintained contacts (also on contacts that are newly added to the manager). You can have a look at its use in the mc_state_observation implementation of the Kinetics Observer.

Regarding the absence of dependencies on Pinocchio, RBDyn, etc., this is indeed a design choice. The state-observation library defines the core structure of our observers as described in their associated papers, and therefore expects the required inputs to be provided directly.
We made this choice to keep the library as independent as possible and thus compatible with most frameworks. All data-handling overhead, including forward kinematics, is assumed to be handled by the library instantiating the observers.
In the case of CNRS-AIST JRL, where we mainly use mc_rtc, this library is called mc_state_observation.

Please don't hesitate to ask for further help and information !

[MaximilienNaveau]

Thank you very much for these explanations! I will try my implementation from here. If I am completely lost I will surely make contact again.

Again thank you for taking the time!

[MaximilienNaveau]

Hello,

I do have a good solution for the computation of the kinodynamics of the robot in base frame.

My last issue is about the contact detection. I read the code in the mc_rtc software you suggested.

I saw some Schmitt trigger filter on the force sensor data in order to identify contacts.

I was wondering if you had better solution for the contact estimation?

I reference actually this issue to your ongoing PR: #21 @ArnaudDmt

[mehdi-benallegue]

Hello,

We definitely have investigated better solutions and we are in the process of submitting a publication with that regard. But it is not yet implemented in our software stack. If you have a moment we can discuss your specific cases orally and propose solutions.

Best regards,

[MaximilienNaveau]

@ArnaudDmt I have an implementation detail question.

In https://github.com/ArnaudDmt/mc_state_observation/blob/addWaiko/src/MCKineticsObserver.cpp#L984 your comment specify that the inputRobot is a mirror of the robot model but placed at the origin. I understand why you do that and I understand how to replicate this in pinocchio for the robot configuration.

Though I am a bit at loss about the free-flyer state in this model as the middleware code is hidding it. Do you consider a none null velocity/acceleration for the freeflyer in the inputRobot?

So far I have a pretty naive implementation from your code. and I got:

• gt_pos_x is the ground truth signal of the base pose in X.
• est_pos_x is the estimated signal of the base pose in X.

You can see a large offset created in X. due to some biases? and the Y axis does not follow the pattern properly.
I may be doing something wrong in the frames to get that hence my question above.

And if you have tips and tricks to tune that I would be glad.
Thank you very much.

[ArnaudDmt]

Dear Maximilien,

The floating base of the input robot is placed at the origin, and both its 6D velocity and acceleration are considered zero. We use this inputRobot because we need the kinematics (pose, velocity, and / or acceleration) of the COM, IMU, and contacts in the floating base frame. However, within rbdyn, most convenience functions cannot return the kinematics of a frame within another but only in the world frame. The inputRobot is thus a trick that gives us the kinematics of the desired frame in the floating base using rbdyn's convenience functions. Maybe you can get them more easily in pinocchio.

It is funny that the position in x gets an offset from the beginning, maybe it would be due to a wrong pitch estimation?
You can maybe check the base orientations too. From what I understand the robot is not walking, right, or maybe stepping without moving forward? It is hard to tell from the plot if the error may come from wrong frame transformations, but most of the input kinematics (I believe that the only exception is the contact pose in addContacts, which is in the world) must be given in the "user" frame, which is the floating base frame in our implementation.

If you need, we can have a call so you we have a look at your tuning and implementation.

Best,
Arnaud.

[MaximilienNaveau]

Thanks a lot for your answer it's clearer now. I sent you an email so we can continue this conversation privately.