Prepare alternative (external) PID controller implementation

[PeterBowman]

I was wondering if moving the position/velocity control loops out of the embedded iPOS firmware would be both possible and beneficial, mostly in terms of things we cannot achieve today (i.e. impedance control and compliance). It is my guess that the "slow" iPOS control loops, i.e. 0.5-1 ms, are probably fast enough for our needs. For this new implementation, I am considering a 5 ms comms/control loop basing on available CAN bandwidth. Torque/current control shall remain handled by the embedded "fast" loop (0.05-0.1 ms).

Related issues:

• https://github.com/roboticslab-uc3m/teo-gazebo-models/issues/2: it is hard to replicate in simulation the three-stage cascading embedded PID controller, therefore let's just replicate the existing, much simpler PID control scheme from Gazebo-YARP in the real robot.
• https://github.com/roboticslab-uc3m/yarp-devices/issues/261: if implemented, should this be always running under the hood (e.g. as an offset for the new PID)?
• https://github.com/roboticslab-uc3m/yarp-devices/issues/258: compliance needs this.

New interface implementations are expected for:

• yarp::dev::IImpedanceControl
• yarp::dev::IInteractionMode
• yarp::dev::IPidControl

Additional tools should be developed for obtaining friction and PID parameters. If done right, perhaps the simulator could yield similar results in case testing on the real joints renders difficult.

[PeterBowman]

Regarding trajectory generation, I think I will go with a simple trapezoidal one. See gazebo-yarp implementation (https://github.com/robotology/gazebo-yarp-plugins/pull/527):

• https://github.com/robotology/gazebo-yarp-plugins/blob/46ddbda29b98c1985f79c5966ea0f964bec68c02/plugins/controlboard/include/yarp/dev/ControlBoardDriverTrajectory.h#L101-L123
• https://github.com/robotology/gazebo-yarp-plugins/blob/46ddbda29b98c1985f79c5966ea0f964bec68c02/plugins/controlboard/src/ControlBoardDriverTrajectory.cpp#L468-L593

Some info on minimum jerk trajectory generators:

• https://arxiv.org/pdf/2102.07459.pdf 2102.07459.pdf
• https://www.sciencedirect.com/science/article/pii/S0893608011002978

[PeterBowman]

I am introducing a buffer class for storing high-frequency commands. This will be an intermediate layer between the commands sent to the robot through the YARP network ("slow" pace) and the commands sent to the iPOS via CAN bus ("fast" pace), using linear interpolation to ensure certain degree of smoothness (cf. https://github.com/roboticslab-uc3m/yarp-devices/issues/254). In the embedded PID implementation, this need not to pose any difference as users may still adjust the sender's period to the internal preconfigured --syncPeriod, which will bypass any interpolation and buffering. In the external PID implementation, though, control loops are always expected to run fast according to the selected --syncPeriod, while incoming commands need not to stick to it, and will commonly arrive at a slower rate. Said rate doesn't need any preconfiguration and, though not recommended, may vary. The buffer helps therefore to decouple the strictly regular CAN transmit rate imposed by --syncPeriod from any external source of command targets arriving at a (usually) different pace.

[PeterBowman]

I am introducing a buffer class for storing high-frequency commands.

Implemented and backported to master at https://github.com/roboticslab-uc3m/yarp-devices/commit/3a8f15beb8ee2debda76f15be2cfce5747597de4, also documented at https://github.com/roboticslab-uc3m/teo-developer-manual/commit/8e31a1129a3ee35b0505719dbe949ee4b0d6acc7.

[PeterBowman]

Done at https://github.com/roboticslab-uc3m/yarp-devices/commit/f5009c62a74574e7e4e75aa8aef00fcf71c4070e. A few thing I'd like to check next:

• [x] syncPeriod should be mandatory for embedded TechnosoftIpos, too (https://github.com/roboticslab-uc3m/yarp-devices/commit/635c7f6f8cc4b8d6942ba9dc8abc86285abe0309)
• [x] EncoderRead should perhaps reset internal values on idle; currently patched at https://github.com/roboticslab-uc3m/yarp-devices/commit/7ce9227d1cff55b3468a383a08cc5c815e3a0f9e (https://github.com/roboticslab-uc3m/yarp-devices/commit/f3502732271a01ad0c01cc9e9eb216868bd2b24d)
• [x] for sanity, set reca homs should not block (https://github.com/roboticslab-uc3m/yarp-devices/commit/09295e5bf045e35e66d3e327aef2651b790d682b)
• [x] pidTuning.py should build the staircase signal over the sampling period in order to override the built-in interpolator (https://github.com/roboticslab-uc3m/tools/commit/132ce4c99db1492ecb8c3053da91c66be9dbf0ba)
• [x] some control mode-specific stuff still common to both controllers (https://github.com/roboticslab-uc3m/yarp-devices/commit/d3834216c2a38f26b9e848b30c7cda3e80a029fe)
• [ ] observe position error limit only in stiff interaction mode? perhaps it should be a percentage?
• [x] CSV mode in external PID implementation? (https://github.com/roboticslab-uc3m/yarp-devices/commit/e6ce31b13af5a243887b50e961653c6419cbffb4)
• [x] https://github.com/roboticslab-uc3m/yarp-devices/issues/264
• [x] https://github.com/roboticslab-uc3m/yarp-devices/issues/260
• [x] https://github.com/roboticslab-uc3m/yarp-devices/issues/258
• [ ] https://github.com/roboticslab-uc3m/teo-configuration-files/issues/31