Cascade PID Controller Implementation #554

ilge · 2020-07-15T01:54:52Z

Introducing a cascaded PI mode that also refactors the existing one. The new mode overloads the use of gainprm and is activated by setting user="1" in the general type actuator definition.

Currently, the cascaded controller supports PI mode for both the position and the velocity controllers and therefore requires Kp, Ti and max_clamp_integral parameters to be specified for both. Additionally, an exponential moving average smoothing is applied to the velocity component for added stability, and the controller is gravity compensated via the qfrc_bias term.

These are provided as part of gainprm in the following order: gainprm="Kp_pos Ti_pos max_clamp_pos Kp_vel Ti_vel max_clamp_vel smooth_factor max_vel", where smooth_factor denotes the EMA smoothing factor and max_vel is a velocity constraint applied to the velocity setpoint.

…vs ID custom controllers based on the actuator_user field that is defined per actuator. `user="1"` would trigger activating an ID controller where the gainprm are interpreted as Kp, Td, ema_smoothing_term, respectively.

bayesian

understood what's going on now, we shall extend unit tests to compare pid and cascaded pid to check the expected benefits.

bayesian · 2020-07-15T18:11:19Z

mujoco_py/mjpid.pyx

+        # Save errors
+        d.userdata[id * NUM_USER_DATA_PER_ACT + IDX_CAS_LAST_ERROR] = pos_output.errors.error
+        d.userdata[id * NUM_USER_DATA_PER_ACT + IDX_CAS_INTEGRAL_ERROR] = pos_output.errors.integral_error
+        des_vel = Kp_cas * (pos_output.errors.error + pos_output.errors.integral_error)


why des_vel only has PI no D terms? also I don't fully understand the physical meaning of des_vel now (it should be the force with "Newton" unit in PID), but on in the cascaed PID, it seems to mean velocity now.

In this setup of cascaded PID, we're only using the PI components. There are effectively two PI controllers. First controller tracks position and outputs a velocity. Second controller tracks the velocity reference from the first controller and outputs a torque.

I also had the question: theoretically, if we needed/wanted, could these be PIDs, or is there a reason that's a bad idea? (also gainprm limitation is there, so we'd move all params to actuator_user, which is a bigger refactor)

also I think that des_vel is just the same as pos_output.otput

good point. I've updated this line as such

mujoco_py/mjpid.pyx

bayesian · 2020-07-15T18:33:57Z

mujoco_py/tests/test_pid.py

+    EMA smoothing constant is set to 0.97, and velocity limit is 3 rad/s
+    """
+    random.seed(30)
+    xml = MODEL_XML.format(actuator=CASCADED_PID_ACTUATOR, nuser_actuator=1)


shall we add some tests to compare cascaded pid with regular pid to check the expected benefits?? e.g. the max velocity should be smaller or something.

added one for the cascaded PID (that checks force limit, however, qvel limit applies as a constraint between the two controllers, so I can't check for that directly). I can also extend the existing PID test (now that I set seed, we could ideally test for trajectories, but didn't want to do that here since it's quite a lot of data)

what is the max_torque for pid only controller here? can we just run both PID and cascaded PID in the test, and compare some metrics? max_torque check here is applied through "force range" which is shared with regular PID controller. it would be great to see in the unit tests the advantages of cascaded PID controls here.

adding @rubendsa: can we set a PID with only PI terms set, and compared against cascaded with extra velocity constraints, is that a unit test we could add?

@ilge Yeah, we can run that test. In this context with both loops running at the same frequency, the biggest gain we have with this cascaded approach is having the ability to apply velocity limits independent of torque constraints.

I couldn't find a good way to test PIDs that have similar Kp/Ti terms for the position control as per for the cascade case, the position controller part is acting to trace a velocity setpoint ant it'll inherently have a very different transfer function compared to the single PID. let me know if the individual tests don't look sufficient, though.

mujoco_py/mjpid.pyx

…s used

mujoco_py/mjpid.pyx

bayesian · 2020-07-15T18:54:10Z

mujoco_py/mjpid.pyx

-        integral_error_term = integral_error / integral_time_const
+    vel_output = _pid(parameters=PIDParameters(
+        dt_seconds=dt_in_sec,
+        setpoint=smoothed_vel_setpoint,


this seems a very complex controller: PI output from the first PID, then exp moving avg, then second PID. I am a bit concern about ema about the pid output, would it introduce the "undesired" behavior of the controller?

The biggest issue with the EMA is the delay it introduces on the original position reference signal tracking, other than that, the performance quite good.

it is perfectly fine and beneficial for have EMA on controller inputs (like pos, vels, etc), but here we are applying EMA to the first PID output. is it common in cascaded PID controller?

mujoco_py/mjpid.pyx

arthurpetron · 2020-07-15T21:24:15Z

mujoco_py/mjpid.pyx

+
+    cdef double integral_error_term = 0.0
+    if parameters.integral_time_const != 0:
+        integral_error_term = integral_error / parameters.integral_time_const


Are you sure you don't want to clamp after integral_error / parameters.integral_time_const ?

I'm not changing the implementation in this PR. Maybe @bayesian has context?

integral_time_const is a constant, clamp before or after multiplied by a constant is equivalent, it only affects the clamp range. It is likely this logic is from our c++ controller.

Except that one is easier to think about than the other.

arthurpetron · 2020-07-15T21:53:02Z

understood what's going on now, we shall extend unit tests to compare pid and cascaded pid to check the expected benefits.

@bayesian The benefit is that you are able to control the velocity of the robot. This is not possible without a cascaded controller. The test should be "does it exist or not." I'm not sure what else you want to test.

bayesian · 2020-07-15T23:12:47Z

mujoco_py/mjpid.pyx

+
+    cdef double integral_error_term = 0.0
+    if parameters.integral_time_const != 0:
+        integral_error_term = integral_error / parameters.integral_time_const


integral_time_const is a constant, clamp before or after multiplied by a constant is equivalent, it only affects the clamp range. It is likely this logic is from our c++ controller.

ilge and others added 8 commits July 7, 2020 14:10

renaming

e6a5f75

Clean

8411ef9

Cast to int

fa26418

wip cascade

912ee99

Enabled PID and cascaded PI control

66b1038

refactored PIDs

367e795

fix id

dd3b77b

ilge changed the title ~~Ilge ruben/cascade~~ Cascade PID Controller Implementation Jul 15, 2020

add some comments

563c4b6

ilge requested a review from bayesian July 15, 2020 02:16

ilge added 2 commits July 15, 2020 10:22

some more formatting

896ec80

test tweaks and fix to apply gravity compensation pre-clamping

b270ea3

ilge marked this pull request as ready for review July 15, 2020 18:15

updated cascade documentation

440017e

bayesian requested changes Jul 15, 2020

View reviewed changes

raul-openai reviewed Jul 15, 2020

View reviewed changes

mujoco_py/mjpid.pyx Outdated Show resolved Hide resolved

rm unused error term, move effort limit definition closer to where it…

24568e0

…s used

bayesian reviewed Jul 15, 2020

View reviewed changes

mujoco_py/mjpid.pyx Outdated Show resolved Hide resolved

remove unused memory allocation

086f30d

arthurpetron reviewed Jul 15, 2020

View reviewed changes

arthurpetron approved these changes Jul 15, 2020

View reviewed changes

ilge mentioned this pull request Jul 15, 2020

Inverse Dynamics Controller #551

Closed

simplify des_vel definition

a392d85

bayesian approved these changes Jul 15, 2020

View reviewed changes

ilge merged commit fe8373d into master Jul 16, 2020

Cascade PID Controller Implementation #554

Cascade PID Controller Implementation #554

Uh oh!

Conversation

ilge commented Jul 15, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

bayesian left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

rubendsa Jul 15, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bayesian Jul 15, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ilge Jul 15, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bayesian Jul 15, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bayesian Jul 15, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bayesian Jul 15, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

arthurpetron commented Jul 15, 2020

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

6 participants

ilge commented Jul 15, 2020 •

edited

Loading

bayesian left a comment •

edited

Loading

rubendsa Jul 15, 2020 •

edited

Loading

bayesian Jul 15, 2020 •

edited

Loading

ilge Jul 15, 2020 •

edited

Loading

bayesian Jul 15, 2020 •

edited

Loading

bayesian Jul 15, 2020 •

edited

Loading

bayesian Jul 15, 2020 •

edited

Loading