Implement instantaneous speed/acceleration getters

[PeterBowman]

yarp::dev::IEncoders allows querying instantaneous speeds and accelerations. We have not implemented this in the TechnosoftIpos device, yet. The hardware is probably not capable of providing such info, thus we have to estimate speeds and accelerations basing on available encoder reads. @jgvictores found a convenient tool at icub-main for such purpose:

• http://www.icub.org/software_documentation/adaptWinPolyEstimator_8h.html
• http://www.icub.org/software_documentation/icub-main_2src_2modules_2velocityObserver_2main_8cpp.html
• https://github.com/robotology/icub-main/blob/4f5881acafab693f763dd277f87a8996fdea01a3/src/libraries/ctrlLib/include/iCub/ctrl/adaptWinPolyEstimator.h
• https://github.com/robotology/icub-main/blob/4f5881acafab693f763dd277f87a8996fdea01a3/src/libraries/ctrlLib/src/adaptWinPolyEstimator.cpp

[jgvictores]

Note for encoder velocity: if we want to avoid the use of a buffer of a certain size (or just lazy?), we could think about exponentially averaged weights (may include bias correction if desired)

[PeterBowman]

The hardware is probably not capable of providing such info

P091.063.CANopen.iPOS.UM, section 11.2.3, Object 606Ch: Velocity actual value:

The velocity actual value is given in user-defined velocity units and is read from the velocity sensor.

Do we have a velocity sensor?

[jgvictores]

Note for encoder velocity: if we want to avoid the use of a buffer of a certain size (or just lazy?), we could think about exponentially averaged weights (may include bias correction if desired)

Correction to myself: This could be a good pre-processing step for a noisy sensor, to avoid peaks, but would not avoid the actual computation of the derivative. Acts like a filter, similar to what is done at driver-level in the JR3. However, I do not think our relative encoders are that noisy at all. If desired, averaging on the last 2-3 values would be more than enough.

Do we have a velocity sensor?

Not that I'm aware about, not even sure if there are physical pins for some kind of tachometer. It would be great if it computed an estimate internally in absence of a direct measure. Maybe we should ask Technosoft (@smcdiaz any idea on this?)?

[PeterBowman]

Read actual instantaneous speeds every 0.1 seconds:

while [ 1 ]; do echo 'get esp 3' | yarp rpc /teo/rightArm/rpc:i; sleep 0.1; done

Use eac for instantaneous accelerations, esps and eacs in multi-joint queries.

[PeterBowman]

Found this at icub-main: https://github.com/robotology/icub-main/blob/c3ad642ed0aacdb3216f692cc73916936079714b/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp#L1731-L1785.

Edit: these lines parse a bottle coming from somewhere, they don't contain actual calculations.

[PeterBowman]

Done at https://github.com/roboticslab-uc3m/yarp-devices/commit/f8ee7fa2506b9b85d083e3854b0f7daa16372e7f.

[PeterBowman]

I learned that we can get a decent measurement of instantaneous velocities via Object 6069h, "Velocity sensor actual value" or Object 606Ch, "Velocity actual value" (both report the same value). Despite the iPOS user manual, we do not have a "velocity encoder" but still can retrieve these values from the drive. I guess the iPOS firmware performs low-level differentiation like we do in the EncoderRead class:

https://github.com/roboticslab-uc3m/yarp-devices/blob/daf0bc55704f16e57ad1538b87d62bb9d13ca525/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp#L114-L137

[PeterBowman]

Anyway, the outcome of our differentiation process and whatever is generated by the iPOS drive internal processes are quite similar: query-speeds-20191225.txt.

Edit: not quite - one of those two data streams represents smooth transitions. I forgot to label them, but it is my guess that the iPOS-generated values are nicer (presumably thanks to some internal filtering, although I don't see any delay).

[PeterBowman]

Technosoft support confirms the velocity values are obtained via differentiation. Also, Object 6069h: Velocity sensor actual value reads the velocity always in encoder increments and Object 606Ch: Velocity actual value reads velocity in user-defined units.