Update ICub wearable device with virtual link sensors and accelerometers

yeshasvitirupachuri commented 5 years ago

To realize the idea of doing a validation of HDE with icub robot, we need to provide the same kind of data from the ICub wearable device, as that of Xsens Suit wearable device. As discussed with @diegoferigo and @lrapetti , we need to update the ICub device with new sensors. Currently, the ICub device has virtual joint sensors implemented https://github.com/robotology-playground/wearables/issues/32 and ft sensors implemented. The keys additions to ICub device can be summarized as:

[x] Implementing virtual link sensors to provide link orientation and velocity. This can be achieved using idyntree model and setting the robot state with the joint quantities already available in the ICub device
[ ] Implementing accelerometer sensors to link acceleration inputs similar to xsens suit device. This is not clearly understood at the moment and it has to be investigated a bit more.
[x] Investigating how to use ft values from the sensors i.e. not wbd wrench estimates. Currently, the ft values are obtained by connecting to a port passed through the configuration file for the ftsensors. We use the WBD ports at the moment to get the wrench estimates at the of the robot. Instead, to be similar to the human case, we need to use the ft values from the sensors at the feet of the robot and for this the correct ports are opened on the robot through wrappers.

Once the above updates are added to the ICub wearable device, it will stream wearable data that is similar to the Xsens suit wearable device. So, in principle, we will be able to test HDE with ICub wearable device directly to do a validation of the joint torque estimates.

CC @lrapetti @diegoferigo @claudia-lat @DanielePucci @traversaro

yeshasvitirupachuri commented 5 years ago

@traversaro Ping

traversaro commented 5 years ago

What is the goal of this activity?

My two cents from what I understand:

If the goal is to try the BERDY-like estimation algorithms on a robot, perhaps we can try to exploit the raw sensors (gyroscopes, accelerometers, etc, etc) that are available on the robot, instead of replicating the processed measurements that we get from the Xsens suit. See https://github.com/robotology/codyco-modules/pull/240 for a related PR.
If the goal to validate the whole software stack for human estimation, perhaps it could make sense to use synthetic data for the human model, for example obtained by assuming an arbitrary set of joint accelerations, velocity, position and external forces, and computing all the other relevant quantities using inverse dynamics and forward kinematics, to make sure that the stack is correctly estimating the provided inputs.

For this reason, I am not sure I understand why you want to replicate the human set of sensors of the robot. For example, you want to run the IK to obtain the joints position instead of using the joint positions directly? Anyhow, to address your points (but given that I do not understand the goal it is possible I am misunderstanding something) :

Implementing virtual link sensors to provide link orientation and velocity. This can be achieved using idyntree model and setting the robot state with the joint quantities already available in the ICub device

If I am not wrong, Xsens suit provides the rotation w.r.t. to an inertial/world frame, so you would also need some kind of inertial_R_base measurement/estimate (this is related to @prashanthr05 work).

Implementing accelerometer sensors to link acceleration inputs similar to xsens suit device. This is not clearly understood at the moment and it has to be investigated a bit more.

@nunoguedelha worked on this (i.e. estimating link acceleration from raw sensor acceleration) but just at the investigation level, as it proved to be non trivial to provide good results w.r.t. to noise rejection.

Investigating how to use ft values from the sensors i.e. not wbd wrench estimates. Currently, the ft values are obtained by connecting to a port passed through the configuration file for the ftsensors. We use the WBD ports at the moment to get the wrench estimates at the of the robot. Instead, to be similar to the human case, we need to use the ft values from the sensors at the feet of the robot and for this the correct ports are opened on the robot through wrappers.

If you want to be similar to the human, I guess the WBD estimated forces is indeed the correct port (otherwise you would ignore the weight of the foot of the robot, that I guess it is more then the weight of the support flange of the sandals in the human). If instead you want to use directly the raw internal FT measurements, I suggest you to use the internal force berdy sensor, instead of the external force berdy sensor that we are currently using for the human.

yeshasvitirupachuri commented 5 years ago

@traversaro The main goal is to validate our online human dynamics estimation stack using the data coming from robot instead of the xsens suit. I do agree that using synthetic data for the human model will help us to validate HDE but doing it through robot will enable us to perform more complicated dynamic tasks to validate the software stack than possible with the synthetic data. Ideally, the validation approach that can hopefully lead to a publication (an experimental work) is to use the same sensory inputs to HDE, currently coming from the xsens suit wearable device, be replaced with the sensory inputs from ICub wearable device.

If I am not wrong, Xsens suit provides the rotation w.r.t. to an inertial/world frame, so you would also need some kind of inertial_R_base measurement/estimate (this is related to @Prashanth05 work).

@diegoferigo and I discussed about this a bit and decided to consider the inertial_R_base to be identity for the time being. We can incorporate developments from @prashanthr05 once they are tested properly

@nunoguedelha worked on this (i.e. estimating link acceleration from raw sensor acceleration) but just at the investigation level, as it proved to be non trivial to provide good results w.r.t. to noise rejection.

I will discuss with @nunoguedelha to know more on how to handle sensor acceleration and link acceleration

If you want to be similar to the human, I guess the WBD estimated forces is indeed the correct port (otherwise you would ignore the weight of the foot of the robot, that I guess it is more then the weight of the support flange of the sandals in the human). If instead you want to use directly the raw internal FT measurements, I suggest you to use the internal force berdy sensor, instead of the external force berdy sensor that we are currently using for the human.

Now it makes sense to use the WBD ports as they are close to the FTShoes in terms of the data produced from the human side of the experiment. Also, we can expand to the internal FT measurements using internal force berdy sensor.

yeshasvitirupachuri commented 5 years ago

Previously ICub wearable device was limited in terms of adding the wrench sources. This limitation of Wrench handling is updated in https://github.com/robotology-playground/wearables/commit/d9acd6cd5c7f9251cde80dcae49ecebcbe475727 and now number of wrench ports added through the configuration file

VirtualLinkKinSensor with getLinkPose() method is added in https://github.com/robotology-playground/wearables/commit/460cc9a67fda6b4dea3f82d4f84b2d1edc108f25

yeshasvitirupachuri commented 5 years ago

Currently, the robot joints are handled through individual part control boards. As @diegoferigo suggested it is better to consider the remotecontrolboardremapper to get access all the joints directly.

nunoguedelha commented 5 years ago

@Yeshasvitvs , regarding the accelerometers handling on the robot:

for accessing the measurements you should use the Multiple Analog Sensors Interfaces which can group and remap the measurements from all the accelerometers part wise (the accelerometers and gyroscopes measurements of a given robot part, for instance the left_leg, will be published on a single Yarp port). One of the main interface headers (extensively commented by @traversaro) is yarp/src/libYARP_dev/include/yarp/dev/MultipleAnalogSensorsInterfaces.h .
here is an epic that was tracking the activation of the interfaces on the iCub04 and iCub02 robots configuration, as well as the handling of the MATLAB bindings (just for info): https://github.com/robotology-playground/sensors-calib-inertial/issues/18
For the Link Acceleration estimation from accelerometers and gyroscopes, I wrote a paper (yet to complete and publish) on the topic, and it's the last chapter of my thesis (https://gitlab.com/nunoguedelha/my-phd-thesis/blob/master/nuno-guedelha-cycle30-phd-thesis.pdf). As @traversaro mentioned, although results are quite good on simulation, it is very difficult to get proper results on the real robot due to too much noise in the accelerometers measurements and vibrations in the robot links (probably induced by the motors). This problem affects mainly the angular acceleration estimation. The direct numerical derivative of the gyroscope readings still gives better results, although it is also impacted by the accelerometers noise.

We can discuss on this F2F if you wish.

yeshasvitirupachuri commented 5 years ago

http://wiki.icub.org/wiki/Distributed_Inertial_sensing

robotology / human-dynamics-estimation

Update ICub wearable device with virtual link sensors and accelerometers #415