Reset JTC PID's to zero on_activate()

[MarqRazz]

When running the JTC in velocity control mode I noticed that the joints receive an impulse on reactivation after is has thrown a previous fault in motion. In the below example you can see this joint gets an impulse of -4 rad/sec down to zero as I reactivate the JTC. The impulse causes my hardware to throw another fault.

This change adds a reset() call to each joints PID controller so that the old command values are zero'd out when (re)activating.

[bmagyar]

great catch, thank you! I would normally ask for a test but this is not only pretty hard to capture in a unit test but also clearly required when going through a deactivate/activate cycle.

[saikishor]

Makes total sense.

Thanks for fixing it @MarqRazz

[christophfroehlich]

In your case this makes totally sense. But should we make this optional? See the save_i_term parameter of the PidROS class. I could think of an application, where the current behavior could be the desired one.

[MarqRazz]

In your case this makes totally sense. But should we make this optional? See the save_i_term parameter of the PidROS class. I could think of an application, where the current behavior could be the desired one.

I don't think the old behavior would ever be correct. It's applying control between steps with an unknown and large amount of time. I can see use cases where you want to calculate control values before you forward them to hardware (i.e. a chained controller g-comp situation) but that should not require the JTC keep the old memory values for the PID that was running last and shutdown in an unknown way.

[christophfroehlich]

Then I haven't understood the issue. Is the PID called with the passed time since deactivation? What I had in mind is something like a model-free gravity compensation in effort control mode, or leaking hydraulic valves in velocity control mode, where the integral term should be retained after re-activation. I don't have a usecase for that, I was just thinking if there might be one.

[MarqRazz]

Then I haven't understood the issue. Is the PID called with the passed time since deactivation?

The PID is called with duration, but the value can be seconds to minuets old and the internal derivative and integral terms terms it keeps can be from a motion that failed because of over torque or speed or all kinds of situations that you don't want propagated to reactivate.
Keeping the old values around sounds hard to manage correctly and not have your robot do bad things when reactivating. Was it holding an item when it faulted? Is it still holding it on activate? Did the arm crash into something and it pushes on the object every time you reactivate causing it to fault again and go into a failure loop?

This is the line of code that is causing the impulse with the old values. When it re-activates state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint] are both zero for all of the joints but the old terms for derivative and integration are out of date.

[christophfroehlich]

This is the line of code that is causing the impulse with the old values. When it re-activates state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint] are both zero for all of the joints but the old terms for derivative and integration are out of date.

There is no "old term for derivative", the velocity error is directly multiplied by the D-gain
https://github.com/ros-controls/control_toolbox/blob/c46e3c5b56d62c30e30e42d3658821fbf909dd3e/control_toolbox/src/pid.cpp#L336
https://github.com/ros-controls/control_toolbox/blob/c46e3c5b56d62c30e30e42d3658821fbf909dd3e/control_toolbox/src/pid.cpp#L349

However, I totally understand your point with the integral action, I just think we could add a parameter save_i_term and use this overload
https://github.com/ros-controls/control_toolbox/blob/c46e3c5b56d62c30e30e42d3658821fbf909dd3e/control_toolbox/src/pid.cpp#L129-L144
so that users can decide on the behavior here. If you don't see a use case here, we also have to discuss if the save_i_term parameter of the PID class makes sense ;)
We also use this in the pid_controller plugin btw

ros2_controllers/pid_controller/src/pid_controller.yaml

Lines 128 to 132 in b21a7e1

	save_i_term: {
	type: bool,
	default_value: true,
	description: "Indicating if integral term is retained after re-activation"
	}

[MarqRazz]

I just think we could add a parameter save_i_term and use this overload

This is going to depend on how the hardware shuts down so it will require users to know each time whether or not it's safe to apply.

If you don't see a use case here, we also have to discuss if the save_i_term parameter of the PID class makes sense

I do see the use case of adding a pause functionality around the active state but think that on_activate is assuming the entry point to powering and enabling control of the hardware.

At Sarcos we made an Exo-Skeleton robot and it could pause control (while holding a load) and then the user could come back and resume from that state. If the hardware ever faulted or deactivated we were required to reactivate the controllers and could not un-pause from that state.

[bmagyar]

@christophfroehlich , I think we should discuss that new save_i_term use separately. This is a bugfix for the standard use-case.