How to get contact forces

[ManuelZ]

How can I get the contact forces when using GeneralContact? I have already enabled computeContactForces but I'm not sure how to get the "respective LTG-vector" or how to extract the forces. There are many GetObjectLTG functions. There is also GetSystemODE2RhsContactForces.

[jgerstmayr]

Contact forces cannot be retrieved directly, at least not currently.
However, there is a GeneralContact function called "GetSystemODE2RhsContactForces()", which allows you to see the contribution of contact forces to the global force vector.
Say, if you know your global coordinate indices of a planar rigid body node, say 21,22, and 23, you could call GetSystemODE2RhsContactForces() and this would give you all contact forces (21 and 22) and the torque due to friction (23) acting on this body.

[ManuelZ]

Thank you; I'm now able to retrieve some forces. However, when I check the forces at the end of my script, it seems I'm only accessing the final computed values. Is this correct?

The documentation for GetSystemODE2RhsContactForces mentions that "the contact forces vector is computed in each contact iteration." I would like to capture the contact forces at each contact iteration during the simulation. Would using SetPreStepUserFunction to store the forces during each iteration be the correct approach?

[jgerstmayr]

SetPreStepUserFunction is called only in every step, not in every iteration of the (implicit) solver.
GetSystemODE2RhsContactForces computes the solution in every iteration.
There will be a new function SetPreNewtonResidualUserFunction() in version 1.9.15 which is called in every iteration. As a workaround, you can put a user function e.g. into an otherwise inactive load which is then called upon every iteration and can handle special tasks.

[ManuelZ]

I'm having trouble understanding what would be the consequence of calling GetSystemODE2RhsContactForces from inside a PreStepUserFunction.

Wouldn't I be getting the contact forces calculated at the last iteration of the last step?
Or is it that I would be calculating contact forces at a moment when I won't get converged results?

This is what I'm currently doing:

def get_contact_forces(mbs, obj):
    # Get object list of global coordinate indices for ODE2 coordinates
    ltg_obj = mbs.systemData.GetObjectLTGODE2(obj.body)
    contact_forces = gContact.GetSystemODE2RhsContactForces()
    return contact_forces[ltg_obj]

def controller_uf(mbs, t):
    if contact_config.rolling_method == ContactType.contact_ANCF_cables:
        mbs.variables["wheel_left_forces"].append(get_contact_forces(mbs, wheel_left))
        mbs.variables["wheel_right_forces"].append(get_contact_forces(mbs, wheel_right))

    ...
    return True

mbs.SetPreStepUserFunction(controller_uf)

[jgerstmayr]

Sure, SetPreStepUserFunction would be called at the beginning of the next step.
There are several alternatives:

1. use
   mbs.SetPostNewtonUserFunction(...)
   which is called after every Newton iteration. If you have only one discontinuous step, then it is what you seek. You can check the behavior by printing time t of the user function, to see how often and when it is called.

2. use the solver's user function capabilities (solver is the solver object usually in mbs.sys['dynamicSolver']):

def UserFinishStep(solver, mbs, simulationSettings):
    solver.FinishStep(mainSystem, simulationSettings)
    ... do some extra work here to read out contact forces
    #returns  nothing

solver.SetUserFunctionFinishStep(mainSystem, UserFinishStep)

See the documentation for the interface of userFunctions (I just noticed that the solver argument is missing in the docu). There is a test model explicitLieGroupIntegratorPythonTest.py which uses such user functions.

[ManuelZ]

Thank you very much. I've explored the first alternative and I think I understand it better.

I'll leave here my notes for future reference, for myself or anyone else.

For the case of a two-wheeled vehicle falling into a pothole: I've noticed that when a wheel looses contact with the ground and then touches the floor, there is more than one discontinuous iteration for that step. I'm not sure when else that happens or why does it happen, but at least I can see it first ocurring at the moment of impact.

In the following graph I've marked in red circles the points in time where there is more than one contact force value for the values extracted with a PostNewtonUserFunction.

In the first red circle it can be seen a difference of 76 N between the two discontinuous iterations and a difference of 71 N between the second discontinuous iteration value and the pre-step value.

In the orange circle I can see a difference of 341 N betweeen the pre-step and post-newton values.

Since the stiffness and damping values that I'm using for the contact are arbitrary, the actual values don't really mean anything. But I guess that the difference between pre-step and post-newton values could be significative in some situation.

[jgerstmayr]

This is correctly showing what PostNewtonUserFunction does.
You should not interpret it as a difference in force, but a difference in time (horizontal shift, not vertical).
The error in PostNewton itself can be adjusted in the discontinuous settings of the solver.

If you only like to retrieve values for plotting, you could use a SensorUserFunction, which takes the final values at the end of steps. I forgot to mention that in the previous answer.

[ManuelZ]

Thank you! Yeah I see the horizontal shift now.

For reference:

def contact_force_sensor_user_function(mbs, t, sensorNumbers, factors, configuration):
    ltg_obj = mbs.systemData.GetObjectLTGODE2(int(factors[0]))
    contact_forces = gContact.GetSystemODE2RhsContactForces()
    return contact_forces[ltg_obj]

sensor_contact_force_left_wheel = mbs.AddSensor(
    SensorUserFunction(
        name="Left wheel contact force",
        storeInternal=True,
        sensorUserFunction=contact_force_sensor_user_function,
        factors=[wheel_left.body],
    )
)

PlotSensor(
    mbs,
    sensorNumbers=[sensor_contact_force_left_wheel],
    components=[exu.plot.componentNorm],
    title="Wheels contact forces norm",
    yLabel="Contact force [N]",
    labels=["Left wheel"],
)