Implement cascading PID control

[PeterBowman]

Stemming from #1, this issue is intended to focus mainly on the analysis and implementation of PID control in Gazebo.

---

#1 (comment)

I'm focusing now on PID joint controllers. Real TEO hardware is somewhat tricky:

• As stated earlier, iPOS drives implement cascading control, i.e. position control entails position PID + speed PI + current PI.
• Each drive appears to be configured differently, or at least there are several common variations (see screenshots at https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/45#issuecomment-759550140). Most importantly, current control is always operated in "fast loop". Some joints are configured to operate position/speed in the same manner, while others use a relaxed sampling period ("slow loop"). Slow loop sample times are ten times greater than fast loop ones. At time of writing, sampling periods in slow loop range 0.5-1 ms. Consequently, fast loop periods range 50-100 microseconds.
• The "High resolution speed estimation" checkbox is sometimes marked, sometimes not.
• A feed forward signal is applied to current control. I'm not sure to fully understand this part as it is identified as a "speed" magnitude, while there is another one referring to "acceleration" (both in the feed forward section of the position controller).
• This should be less of a concern, but keep in mind that Gazebo does not understand currents, just torques instead. Known motor constants need to be used to perform conversion between those magnitudes.
• What are the units of iPOS PID coefficients?

Ideally, we'd like to implement a new control type in Gazebo-YARP that would more closely match real robot behavior. Then, similar or exact same values as provided in the iPOS configuration would be reused in this plugin. However, the variable sampling periods are a significant obstacle. Physics in Gazebo are updated each millisecond by default and defined by two elements: <max_step_size> and <real_time_update_rate> (ref). If not provided, Gazebo assumes the following <physics> element:

<world>
  <!-- ... -->
  <physics type="ode">
    <max_step_size>0.001</max_step_size>
    <real_time_update_rate>1000</real_time_update_rate>
  </physics>
</world>

Gazebo's ODE plugin updates physics at a fixed rate of 1 ms, then Gazebo itself will strive to advance steps at 1000 Hz to match real time, i.e. real time factor is 0.001*1000=1.0. If too computationally expensive, simulation time will lag behind real time (real time factor less than 1.0). Info:

• http://gazebosim.org/tutorials?tut=physics_params&cat=physics
• https://answers.gazebosim.org//question/2477/what-does-update-rate-control/
• https://answers.gazebosim.org//question/16053/max_step_size-real_time_factor-and-real_time_update_rate/

I've tried to tighten simulation times and the most I can achieve without degrading the real time factor is 0.2-0.25 ms (200-250 microseconds). It is virtually not possible to achieve a fast control loop of 100 microseconds, much less 50 microseconds, as resource usage is too high.

In short, I'd need to take shortcuts, perhaps forcing all three stages (position PID, speed PI, current PI) to operate at the same "slow loop" sampling period. It is yet to be determined whether that same PID coefficients originating from the iPOS configuration would still be valid in a simulated environment and with the improvements proposed here. Moreover, what if the slow loop period is 0.5 ms instead of 1 ms, but we reuse PID coeffs in a 1 ms loop?

Options:

• Implement cascading PID control. If this turns out to differ significantly from the real PIDs, the simulated ones would need to be tuned (3 per joint, 28 joints total).
• Stick to default joint_pid_gazebo_v1, which is a position PID + speed PID (i.e. 2 per joint), and tune with care.

---

#1 (comment)

As spoken, both @smcdiaz and I agree on Implement cascading PID control would be a great option, even if the parameters would have to be different (hopefully, proportional, and no problem if fully re-tuned). : )

---

#1 (comment)

See roboticslab-uc3m/yarp-devices#253 (comment) regarding sampling rates and slow/fast loop configuration. Besides, MotionChip II Data Sheet (P091.055.MCII.DSH.0704) states in Section 3.5: Special Features:

The MotionChip II uses a fast loop for current/torque control and a slow loop for position/speed sampling. The sampling rates of these loops are synchronized and linked in a fixed ratio with the PWM frequency in order to eliminates the beat-frequency problems. The maximum sampling frequency on the fast loop can be half of the PWM frequency. The PWM frequency and the divider ratios for fast and slow loops are user programmable in a wide range. The maximum values for PWM frequency and sampling rate frequencies depend on the application configuration. (...)

---

#1 (comment)

Just in case, regarding gazebo::common::PID: implementation, reference.

---

#1 (comment)

MotionChip™ II Configuration Setup User Manual provides details on the implementation of the motion controllers in Section 3.5 (see also the description of their TML parameters, especially in Section 4.2.5). There is a correspondence between numerical values and TML parameters in the EasySetUp's "Drive Setup" dialog, as shown below:

All TML parameters are saved in the parameters.cfg file by EasySetUp.

[PeterBowman]

As far as I can tell, torque values used as reference (i.e. commanded via setRefTorque()) at instant t are always equal to the torque measured (i.e. queried via getTorque()) at t+1. It seems to me that there is little sense in trying to close a current/torque loop in simulation since torque feedback is meaningless. In other words, torque is always controlled in an open-loop fashion. Real iPOS drives need to account for a real motor governed by current commands turned into PWM signals. A set of Hall sensors provides current measurements, which indeed come to play in closing the loop for the PI current controller.

[PeterBowman]

iPOS help dialogues:

• EasyMotion Studio - Drive Setup Dialogue.pdf
• EasyMotion Studio - Advanced Drive Setup Dialogue.pdf

Regarding the screenshot attached in #2 (comment) / #1 (comment), note both feedforward terms in Position controller are moved to the Speed controller section if Control mode is configured as Speed instead of Position (this is not our use case). Moreover, the Load speed limit field in Position controller disappears if we select Close only position and current loop (again, not our case). Lastly, there are no feedforward terms present if position/speed controllers are closed using the fast loop.

See also: http://www.technosoft.ro/KB/index.php?/article/AA-15475/0/Advanced-controllers-tuning.html.

In case of really dynamic movements, where the acceleration is rather big and as a result the motor has a rather big following error from the start (which can't be eliminated by increasing the controller gains as this will make the system unstable), an acceleration feed-forward can be added from the Position controllers parameters group box. The value used for the feedforward will be then multiplied with the imposed acceleration and the result (scaled to IU - internal units) will be added to the current reference (which is the output of the speed controller - if active - otherwise the output of the speed controller). The idea behind the feedforward is to avoid having to wait for an error to occur, so that the PID can increase the current reference enough until the motor starts moving, but to already generate a current command from the moment the reference starts. Hence the presence of the feedforward will help the PID which will have to compensate a smaller error (please keep in mind that the feedforward is actually not related to the controllers and even if you set it from the same tuning wizard, its effect is present and constant regardless of the values of the gains of the controller).

The same idea applies also to the speed feed-forward - the value of the feedforward is multiplied with the speed reference and the scaled result is added either to current command or to the speed command (depending on the system configuration or user setting).

Remark: While the purpose of the acceleration / speed feedforward is to help the controllers, setting values too big for these parameters will actually have the adverse effect - it will hinder the controller by creating an unwanted faster movement/response which will in the end increase the error (it is true that without the feedforward the error will be positive - the motor will lag the reference, but if the feedforward is too big the error will be negative - the motor will be leading the reference, so from the controller point of view it will still be an error which will have to be corrected and which will reduce the stability limits of the system).

[PeterBowman]

From MotionChip™ II Configuration Setup User Manual:

[PeterBowman]

WIP: teo.ods

[PeterBowman]

I am going to close this as WONTFIX. The point of going all the way down to the basics of how iPOS controllers work was to replicate them as closely as possible in Gazebo. There are several challenges that make this very unlikely to happen (triple cascading PID, stiction, no model available for an EC motor, different sampling times...). Instead, it makes more sense to work with what we have now and improve it via proper configuration.

I have introduced a pidTuning.py script at roboticslab-uc3m/tools@a63896d which allows to modify the PID coefficients (via IPidControl). The results of a step response are plotted for visual inspection and analysis. It also features a "staircase" input to emulate the actual behavior of a longer, fixed-velocity trajectory.

Commit 4807410 introduced the new coefficients in the model. Besides, I am also adding the "externalwrench" Gazebo/YARP plugin in the main SDF file to emulate the behavior of the leg limbs while standing on one foot (Fz = 540 N).

PS: I had to change the NWS state publisher port period from 10 ms to 1 ms (here).