Replace calcResidualForceIgnoringConstraints with calcAcceleration

[itbellix]

I want to modify the current functioning of OpenSimAD so that it can generate functions that capture symbolically the forward dynamics behavior of a model.

To do this, I need to modify Antoine's original code (in particular the generateExternalFunction portion of the code, in utilities.py) such that it receives as inputs the current state of the model x (made of position and velocity for every joints) together with the generalized forces u applied on the model mobilities. The output that I want is x_dot (or at least the second half of it, which is the accelerations of each joint, caused by the generalized forces that are applied to it).

[itbellix]

With this commit (f42dc795bd998f9372be03b057c1e08a0fe15730) I have managed to write my own generateExternalFunction_Acc that should achieve what I describe above. Essentially, the code has been modified such that:

1. it takes as inputs joint states, speeds and generalized forces
2. realizes the model to the required Dynamics stage
3. employs the Simbody function calcAcceleration() to evaluate the model's accelerations (udot)
4. returns the induced accelerations udot on exit

However, the resulting C++ file compiles fine, but throws and error at runtime:

[100%] Linking CXX executable simple_pendulum
terminate called after throwing an instance of 'std::runtime_error'
  what():  Needs to be symbolic
make[2]: *** [CMakeFiles/simple_pendulum.dir/build.make:103: simple_pendulum] Aborted (core dumped)
make[2]: *** Deleting file 'simple_pendulum'
make[1]: *** [CMakeFiles/Makefile2:83: CMakeFiles/simple_pendulum.dir/all] Error 2
make: *** [Makefile:91: all] Error 2

[itbellix]

After discussing with Adam, it looks that the problem is in the way in which the Recorder variables are treated.

Modifying the header recorder.h to expose a couple of functions, I was able to run an inspection to check how the input and output variables of F_generic are managed, in the case in which the functioning OpenSimAD code is used (implementing ID for a simple pendulum model):

[100%] Linking CXX executable simple_pendulum
Checking the id and symbolic properties at the beginning of the function
x id is: 7
x is symbol: 1
u id is: 8
u is symbol: 1
res id is: 9
res is symbol: 0

Checking the id and symbolic properties at the end of the function
x id is: 1321
x is symbol: 1
u id is: 1322
u is symbol: 1
res id is: 1323
res is symbol: 1
[100%] Built target simple_pendulum

The code generates the C++ file, that then I modify to print information about the variables at the beginning and at the end of F_generic, and then the make command is able to successfully build and run the executable file.

By performing the same analysis in my modified pipeline, this is the output:

[100%] Linking CXX executable simple_pendulum

Checking the id and symbolic properties at the beginning of the function
x id is: 7
x is symbol: 1
u id is: 8
u is symbol: 1
res id is: 9
res is symbol: 0

Checking the id and symbolic properties at the end of the function
x id is: 1067
x is symbol: 1
u id is: 1068
u is symbol: 1
res id is: 1069
res is symbol: 0
terminate called after throwing an instance of 'std::runtime_error'
  what():  Needs to be symbolic
make[2]: *** [CMakeFiles/simple_pendulum.dir/build.make:103: simple_pendulum] Aborted (core dumped)
make[2]: *** Deleting file 'simple_pendulum'
make[1]: *** [CMakeFiles/Makefile2:83: CMakeFiles/simple_pendulum.dir/all] Error 2
make: *** [Makefile:91: all] Error 2

It appears that the variable res is not handled correctly, as it is not turned into a valid Recorder (with id>0) during the execution of the modified F_generic function.

[itbellix]

I have tracked down the issue to the line in which the Simbody function is called.

For the functioning version of the code, the call to the function calcResidualForceIgnoringConstraints() produces as an output a valid Recorder variable (which represents the residualMobilityForce):

[100%] Linking CXX executable simple_pendulum
Checking output before Simbody function call
residualMobilities id is: 1001
residualMobilities is symbol: 0
Checking output after Simbody function call
residualMobilities id is: 1290
residualMobilities is symbol: 1
[100%] Built target simple_pendulum

In my modified code, an equivalent call is made to the Simbody function calcAcceleration, but here the outputs (udot) are not returned as a valid Recorder:

[100%] Linking CXX executable simple_pendulum
Checking output before Simbody function call
udot id is: 1064
udot is symbol: 0
Checking output after Simbody function call
udot id is: 1065
udot is symbol: 0
terminate called after throwing an instance of 'std::runtime_error'

[itbellix]

I checked if something changes when the state of the model, in my modified function, is not realized to Dynamics, but rather to Acceleration or simply Velocity. The code returns still the same error (with different ids since more or less variables are allocated because of the different calls) but it does not signal that the Velocity state is not sufficient to actually call the calcAcceleration function. This suggests that actually calcAcceleration is failing silently for some reason, and that is why its outputs are not computed as they should.

Another option still open is that the implementation of calcResidualForceIgnoringConstraints has been modified to work properly with the Recorder class, while calcAcceleration is still fundamentally unable to do so.

[itbellix]

To understand whether calcAcceleration is working properly I have written two simple C++ codes where I do not use any Recorder, but only normal variables.

• in the first code, I feed numerical inputs to calcResidualForceIgnoringConstraint and observe all the variables before and after the function call. In this case, the residualMobilityForces are updated:

  Checking inputs before numeric Simbody function call
  appliedMobilityForces value is~[    (0)]
  appliedBodyForces value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]
  knownUdot value is~[    (1)]
  residualMobilityForces value is~[    (0)]
  
  Checking outputs from numeric Simbody function call
  appliedMobilityForces value is~[    (0)]
  appliedBodyForces value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]
  knownUdot value is~[    (1)]
  residualMobilityForces value is~[    (0.0658706)]

• in the second code, I feed numerical inputs to calcAcceleration, and observe how the variables in input are modified by the function call. The results shows that the output I am interested (udot) do not get updated (even if there is an appliedMobilityForce which is not zero) and the accelerations of the body expressed in ground (A_GB) are updated with some constant value (3.14, which is suspiciously the default value for a newly instantiated Recorder variable):

  Checking inputs before numeric Simbody function call
  appliedMobilityForces value is~[    (1)]
  appliedBodyForces value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]
  udot value is~[    (0)]
  A_GB value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]
  
  Checking output udot after numeric Simbody function call
  appliedMobilityForces value is~[    (1)]
  appliedBodyForces value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]
  udot value is~[    (0)]
  A_GB value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (3.14),    (3.14),    (3.14)],~[    (3.14),    (3.14),    (3.14)]] ~[~[    (3.14),    (3.14),    (3.14)],~[    (3.14),    (3.14),    (3.14)]]]

So, it looks like the behavior of the two functions is actually different (one works, the other does not!), and there appear to be some Recorder shadows even where they should not be. Note that I am still following the building instructions of the original OpenSimAD repository, so the underlying version of OpenSim or Simbody used in the build/execution could very well still be using Recorders even if I am not instantiating them explicitly.

The files used for this comparison can be found here

[itbellix]

Update

I have repeated the test above, but I used calcAccelerationsIgnoringConstraints instead of calcAcceleration. It works, this is the log of the variables:

Checking inputs before numeric Simbody function call
appliedMobilityForces value is~[    (1)]
appliedBodyForces value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]
udot value is~[    (0)]
A_GB value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]

Checking output udot after numeric Simbody function call
appliedMobilityForces value is~[    (1)]
appliedBodyForces value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]]]
udot value is~[    (15.1813)]
A_GB value is~[~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (0)],~[    (0),    (0),    (0)]] ~[~[    (0),    (0),    (15.1813)],~[    (0.00285753),    (3.58839),    (0)]]]

Now, the question that remains is: why are constraints not accounted for in OpenSimAD?

What I observed was that:

• calcAcceleration was not failing if the state was realized only to Velocity stage (as reported above). It was instead always running, but without apparently touching its input variables (numeric or simbolic)
• calcAccelerationIgnoringConstraints instead would throw a runtime error if the state was not realized at least to the Dynamics stage, and would also return the correct (?) value both in case of numeric or symbolic inputs.

Commit: ee3ade0e1d2fd5dfec86402dc0af517147f1c9b8

[itbellix]

In conclusions, if the model does not have constraints, we should probably be good to go!

[adamkewley]

Are joints classed as constraints? I can't remember (probably not?)

Anyway, great investigation! :heart:

[itbellix]

Hey @adamkewley, I am not sure what you mean here... Looking at the inheritance diagram for the Joint class and the Constraint class I would think they are two different unrelated things