How to correctly create a controller

[ManuelZ]

Hi, I have questions regarding how to create a controller. The summary would be this:

• What is the difference between using loadVectorUserFunction and SetPreStepUserFunction for creating the controller of a force?
• What if I'm making the load dependent on the system state?

Context

• I am trying to recreate this mechanism in 2D:
  

• There is an object at the top (in red) to which both large bars are connected, and this object is connected through a prismatic joint (GenericJoint with constrainedAxes=[0, 1, 0, 0, 0, 1]) to the platform (the top empty red rectangle). There is also an actuator composed by two objects connected through a prismatic joint, one of which has an associated LoadForceVector.
  

• The controller should output the force required to move the platform vertically at certain target speed.

Question 1

I would like to know the difference between using SetPreStepUserFunction and loadVectorUserFunction for creating a controller for the actuator.

• When I use a loadVectorUserFunction, the simulation runs super slow. It runs as if I would have set h=0.0001 even if I have the number of steps defined as:

tEnd = 4
h = 0.01
simulationSettings.timeIntegration.numberOfSteps = int(tEnd / h)

def controller(mbs, t, load_vector):
    vx, vy, vz = mbs.GetSensorValues(platform.sensor_velocity)
    force = clip(CONTROLLER_P * (CONTROLLER_SET_VEL - vy), -MAX_FORCE, MAX_FORCE)
    return [force, 0, 0]

And the output looks like this:

• When I use SetPreStepUserFunction with the same h=0.01, the platform can't be hold in place and just falls.

def controller_uf(mbs, t):
    vx, vy, vz = mbs.GetSensorValues(platform.sensor_velocity)
    load = clip(CONTROLLER_P * (CONTROLLER_SET_VEL - vy), -MAX_FORCE, MAX_FORCE)
    mbs.SetLoadParameter(actuator.load_index, "loadVector", [load, 0, 0])
    return True

• When I change h to h=0.001, the result looks like this:
  

• When I change h to h = 0.0001, the result looks like this:
  
  (notice the max force)

• When I change h to h = 0.00001, the result looks like this, and the force resembles what I got using the loadVectorUserFunction way (at least regarding the force decreasing linearly from around 16000 N).
  

All this using the GeneralizedAlpha solver.

Question(s) 2

I read that making a load dependent on the system state is not recommended (in this comment).

Making loads dependent on the system state (e.g., by reading out the current position of a marker, body or node) is not recommended, as loads are not "differentiated", that is they are not considered in the system jacobian. Therefore the solver would get into trouble. User functions in connectors are considered in the jacobian, so this is the right way to go.
Most general force/torque laws can be implemented with the RigidBodySpringDamper, as it provides the full rigid body information on both sides of markers. But this should not be necessary for the above question.

I am making my controllers (through loadVectorUserFunction or SetPreStepUserFunction) dependent on the state of the platform, so I guess the "trouble" that you mentioned there is what I'm seeing in my experiments, right?

You suggested (in a different context) implementing the control law in RigidBodySpringDamper. Should I connect the two bodies of my actuator with a RigidBodySpringDamper?

Is there a place in the documentation where I can learn more about this? I don't think I've come across explicit examples on this topic.

Thanks!

EDIT:
Claryfying note about the graphs above:
The third graph (h = 0.0001) looks weird because the sensorsWritePeriod was too large. I fixed it by setting it to have the same period h as the h used in numberOfSteps = int(tEnd / h).

[jgerstmayr]

the difference between the two choices are:
loadVectorUserFunction: is called in every iteration of the solver - in particular for implicit solvers. This means that this is the most accurate solution. However, for large P-control values, the solver does not know the strong coupling of controller output to sensor values, which can be added by using AddODE2LoadDependencies(...), see https://exudyn.readthedocs.io/en/latest/docs/RST/cInterface/SystemData.html
SetPreStepUserFunction: this is a weak coupling, which is similar to a real controller which acts every delta t (the step size or whatever you implement in SetPreStepUserFunction. This may lead to controller instabilities like in real world.

There are several examples for serial robots which use controllers.

Using RigidBodySpringDamper or any other spring: then you can directly couple the P and D-values (stiffness and damping) to control inputs. Do not forget to set both offset and offset_t for set values for the controller.

[jgerstmayr]

Just to add one thing:
load or object user functions should be treated carefully with respect to discontinuities, such as
clip(CONTROLLER_P * (CONTROLLER_SET_VEL - vy), -MAX_FORCE, MAX_FORCE)
Every function which is included in loads, spring-dampers or objects get part of the Newton iteration. The Newton method does not work well in case of discontinuities - which is the reason for the solver to switch to very small time steps.
Workarounds: 1) use the PreStepUserFunction, which is not part of the Newton-iteration, or 2) add a NodeGenericData variable which is then used to take care of the switching (switching should be done in the PostStepUserFunction). However, the latter is only needed in rare cases, see the final comment below.

The simpler functionality based on the PreStepUserFunction should always be sufficient for simulation of controllers, if your simulation step-size is in the range of the controller rate/delay (usually, controllers run around 1ms - some slower and some a little bit faster). If this way of implementation of the controller becomes unstable, it indicates that also the real system would be unstable. A potential problem could pop up if the CONTROLLER_P is not the value which couples the velocity with the actuator force (by the way, I see this as the D-value as it uses the velocity as input and not the position).

[ManuelZ]

Thank you very much.

I'm using SetPreStepUserFunction now, since it sounds like the simplest solution and there I don't have to:

• Add any AddODE2LoadDependencies to make the solver know of the coupling between the controller output and sensor values.
• Be careful with discontinuities by adding a NodeGenericData.
• Think of other ways of solving the problem, like with RigidBodySpringDamper

Now I can really focus on creating a good controller.