├── requirements.txt
├── .gitignore
├── InverseDynamics
    └── SetupID.xml
├── main.py
├── README.md
├── LICENSE
└── utilities.py


/requirements.txt:
--------------------------------------------------------------------------------
1 | numpy
2 | casadi
3 | pandas
4 | cmake
5 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Byte-compiled / optimized / DLL files
 2 | __pycache__/
 3 | *.log
 4 | *dummyData.sto
 5 | *.DS_Store
 6 | 
 7 | examples/*
 8 | !examples/Hamner_modified.osim
 9 | !examples/LaiArnold_modified.osim
10 | 
11 | opensimAD-install/*


--------------------------------------------------------------------------------
/InverseDynamics/SetupID.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8" ?>
 2 | <OpenSimDocument Version="40000">
 3 | 	<InverseDynamicsTool name="DummyModel">
 4 | 		<!--Directory used for writing results.-->
 5 | 		<results_directory />
 6 | 		<!--Directory for input files-->
 7 | 		<input_directory />
 8 | 		<!--Name of the .osim file used to construct a model.-->
 9 | 		<model_file>../subject1_scaled.osim</model_file>
10 | 		<!--Time range over which the inverse dynamics problem is solved.-->
11 | 		<time_range> 0.01 0.1</time_range>
12 | 		<!--List of forces by individual or grouping name (e.g. All, actuators, muscles, ...) to be excluded when computing model dynamics. 'All' also excludes external loads added via 'external_loads_file'.-->
13 | 		<forces_to_exclude> Muscles</forces_to_exclude>
14 | 		<!--XML file (.xml) containing the external loads applied to the model as a set of ExternalForce(s).-->
15 | 		<external_loads_file />
16 | 		<!--The name of the file containing coordinate data. Can be a motion (.mot) or a states (.sto) file.-->
17 | 		<coordinates_file>dummy_motion.mot</coordinates_file>
18 | 		<!--Low-pass cut-off frequency for filtering the coordinates_file data (currently does not apply to states_file or speeds_file). A negative value results in no filtering. The default value is -1.0, so no filtering.-->
19 | 		<lowpass_cutoff_frequency_for_coordinates>-1</lowpass_cutoff_frequency_for_coordinates>
20 | 		<!--Name of the storage file (.sto) to which the generalized forces are written.-->
21 | 		<output_gen_force_file>ID_OSIM.sto</output_gen_force_file>
22 | 		<!--List of joints (keyword All, for all joints) to report body forces acting at the joint frame expressed in ground.-->
23 | 		<joints_to_report_body_forces />
24 | 		<!--Name of the storage file (.sto) to which the body forces at specified joints are written.-->
25 | 		<output_body_forces_file>body_forces_at_joints.sto</output_body_forces_file>
26 | 	</InverseDynamicsTool>
27 | </OpenSimDocument>
28 | 


--------------------------------------------------------------------------------
/main.py:
--------------------------------------------------------------------------------
 1 | '''
 2 |     Author: Antoine Falisse    
 3 | 
 4 |     This script uses OpenSimAD to generate a CasADi external function.
 5 |     
 6 |     Given an OpenSim model provided as an .osim file, this script generates a
 7 |     C++ file with a function F building the musculoskeletal model
 8 |     programmatically and running, among other, inverse dynamics. The C++ file
 9 |     is then compiled as an application which is run to generate the expression
10 |     graph underlying the function F. From this expression graph, CasADi can
11 |     generate C code containing the function F and its Jacobian in a format
12 |     understandable by CasADi. This code is finally compiled as a dynamically
13 |     linked library that can be imported when formulating trajectory
14 |     optimization problems with CasADi.
15 |     
16 |     The function F takes as:
17 |         - INPUTS: 
18 |             - joint positions and velocities (intertwined)
19 |             - joint accelerations
20 |         - OUTPUTS:
21 |             - joint torques
22 |             - ground reaction forces
23 |             - ground reaction moments
24 |             - body origins
25 |             
26 |     You can adjust the script generateExternalFunction to modify the inputs or
27 |     outputs.
28 |             
29 |     This script also saves a dictionnary F_map with the indices of the
30 |     outputs of F. E.g., the left hip flexion index is given by 
31 |     F_map['residuals']['hip_flexion_l'].
32 |             
33 |     See concrete example of how the function F can be used here:
34 |         https://github.com/antoinefalisse/predsim_tutorial    
35 | '''
36 | 
37 | import os
38 | from utilities import generateExternalFunction
39 | 
40 | pathMain = os.getcwd()
41 | 
42 | # %% User inputs.
43 | # Provide path to the directory where you want to save your results.
44 | pathModelFolder = os.path.join(pathMain, 'examples')
45 | # Provide path to OpenSim model.
46 | modelName = 'Hamner_modified'
47 | pathOpenSimModel = os.path.join(pathModelFolder, modelName + '.osim')
48 | # Provide path to the InverseDynamics folder.
49 | # To verify that what we did is correct, we compare torques returned by the
50 | # external function given some input data to torques returned by OpenSim's ID
51 | # tool given the same input data and the original .osim file. If the two sets
52 | # of resulting torques differ, it means something went wrong when generating
53 | # the external function.
54 | pathID =  os.path.join(pathMain, 'InverseDynamics')
55 | 
56 | # %% Optional user inputs.
57 | # Output file name (default is F).
58 | outputFilename = modelName
59 | 
60 | # %% Generate external function.
61 | generateExternalFunction(pathOpenSimModel, pathModelFolder, pathID,
62 |                          outputFilename=outputFilename)
63 | 
64 | # %% Example (not recommended).
65 | # You can also directly provide a cpp file and use the built-in utilities to
66 | # build the corresponding dll. Note that with this approach, you will not get
67 | # the F_map output.
68 | # from utilities import buildExternalFunction
69 | # nCoordinates = 31
70 | # buildExternalFunction(outputFilename, pathModelFolder, 3*nCoordinates, 
71 | #                       compiler=compiler)
72 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # SimbodyAD - OpenSimAD
 2 | Libraries for SimbodyAD and OpenSimAD - Simbody and OpenSim with support for Algorithmic Differentiation.
 3 | 
 4 | ## How to generate an external function for use with CasADi?
 5 | OpenSimAD is used to formulate trajectory optimization problems with OpenSim musculoskeletal models. To leverage the benefits of algorithmic differentiation, we use [CasADi external functions](https://web.casadi.org/docs/#casadi-s-external-function). In our case, the external functions typically take as inputs the multi-body model states (joint positions and speeds) and controls (joint accelerations) and return the joint torques after solving inverse dynamics. The external functions can then be called when formulating trajectory optimization problems (e.g., https://github.com/antoinefalisse/3dpredictsim and https://github.com/antoinefalisse/predictsim_mtp).
 6 | 
 7 | Here we provide code and examples to generate external functions automatically given an OpenSim musculoskeletal model (.osim file). Visit https://github.com/antoinefalisse/predsim_tutorial for a tutorial about how to use these external functions when formulating and solving trajectory optimization problems.
 8 | 
 9 | ### Install requirements
10 | 1. Third-party packages
11 | 	- **Windows only**: Install [Visual Studio](https://visualstudio.microsoft.com/downloads/)
12 | 		- The Community variant is sufficient and is free for everyone.
13 | 		- During the installation, select the *workload Desktop Development with C++*.
14 | 		- The code was tested with the 2017, 2019, and 2022 Community editions.
15 | 	- **Linux only**: Install OpenBLAS libraries
16 | 		- `sudo apt-get install libopenblas-base`
17 | 2. Conda environment
18 | 	- Install [Anaconda](https://www.anaconda.com/)
19 | 	- Open Anaconda prompt
20 | 	- Create environment (python 3.9 recommended): `conda create -n opensim-ad python=3.9`
21 | 	- Activate environment: `conda activate opensim-ad`
22 | 	- Install OpenSim: `conda install -c opensim-org opensim=4.4=py39np120`
23 | 		- Test that OpenSim was successfully installed:
24 | 			- Start python: `python`
25 | 			- Import OpenSim: `import opensim`
26 | 				- If you don't get any error message at this point, you should be good to go.
27 | 			- You can also double check which version you installed : `opensim.GetVersion()`
28 | 			- Exit python: `quit()`
29 | 		- Visit this [webpage](https://simtk-confluence.stanford.edu:8443/display/OpenSim/Conda+Package) for more details about the OpenSim conda package.
30 | 	- (Optional): Install an IDE such as Spyder: `conda install spyder`
31 | 	- Clone the repository to your machine: 
32 | 		- Navigate to the directory where you want to download the code: eg. `cd Documents`. Make sure there are no spaces in this path.
33 | 		- Clone the repository: `git clone https://github.com/antoinefalisse/opensimAD.git`
34 | 		- Navigate to the directory: `cd opensimAD`
35 | 	- Install required packages: `python -m pip install -r requirements.txt`
36 | 
37 | ### Examples
38 |   - run `main.py`
39 |       - You should get as output a few files in the example folder. Among them: `Hamner_modified.cpp`, `Hamner_modified_map.npy`, and `Hamner_modified.dll` (Windows) or `Hamner_modified.so` (Linux) or `Hamner_modified.dylib` (macOS). The .cpp file contains the source code of the external function, the .dll/.so/.dylib file is the [dynamically linked library](https://web.casadi.org/docs/#casadi-s-external-function) that can be called when formulating your trajectory optimization problem, the .npy file is a dictionnary that describes the outputs of the external function (names and indices).
40 |       - More details in the comments of `main.py` about what inputs are necessary and optional.
41 |   - Evaluate the external function and its Jacobian.
42 |       - Now that you have generated the external function, you can evaluate it with numerical inputs, use it in optimization problems (see example [here](https://github.com/antoinefalisse/predsim_tutorial)), get an expression for its Jacobian, etc. Here is example code:
43 | ```
44 | # Import CasADi and numpy.
45 | import casadi as ca
46 | import numpy as np
47 | # Load external function.
48 | F = ca.external('F', 'examples/Hamner_modified.dll')
49 | # Evaluate the function with numerical inputs.
50 | inputs = np.ones(93,)
51 | out = F(inputs).full()
52 | # Get the Jacobian of the function.
53 | F_jac = F.jacobian()
54 | # Evaluate the Jacobian with numerical inputs.
55 | inputs1 = np.ones(93,)
56 | inputs2 = np.ones(103,)
57 | out = F_jac(inputs1, inputs2).full()
58 | ```
59 | 
60 | ### Limitations
61 |   - Not all OpenSim models are supported:
62 |     - Your model **should not have locked joints**. Please replace them with weld joints (locked joints would technically require having kinematic constraints, which is possible but makes the problem more complicated).
63 |     - **Constraints will be ignored** (eg, coupling constraints).
64 |     - **SimmSplines are not supported**, as their implementation in OpenSim is not really compatible with algorithmic differentiation. See how we replaced the splines of the [LaiArnold_modifed model](https://github.com/antoinefalisse/opensimAD/blob/main/examples/LaiArnold_modified.osim#L3564) with polynomials.
65 |   - OpenSimAD does not support all features of OpenSim. **Make sure you verify what you are doing**. We have only used OpenSimAD for specific applications.
66 | 
67 | ## Tutorial
68 |   - You can find [here a tutorial](https://github.com/antoinefalisse/predsim_tutorial) describing how to generate a predictive simulation of walking. The tutorial describes all the steps required, including the use of OpenSimAD to general external functions for use when formulating the trajectory optimization problem underlying the predictive simulation. 
69 | 
70 | ## Citation
71 | Please cite this paper in your publications if OpenSimAD helps your research:
72 |   - Falisse A, Serrancolí G, et al. (2019) Algorithmic differentiation improves the computational efficiency of OpenSim-based trajectory optimization of human movement. PLoS ONE 14(10): e0217730. https://doi.org/10.1371/journal.pone.0217730
73 | 
74 | Please cite this paper in your publications if you used OpenSimAD for simulations of human walking:
75 |   - Falisse A, et al. (2019) Rapid predictive simulations with complex musculoskeletal models suggest that diverse healthy and pathological human gaits can emerge from similar control strategies. J. R. Soc. Interface.162019040220190402. http://doi.org/10.1098/rsif.2019.0402
76 | 
77 | ## Source code
78 | The OpenSimAD libraries were compiled from [here](https://github.com/antoinefalisse/opensim-core/tree/AD-recorder-py).
79 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 |                                  Apache License
  2 |                            Version 2.0, January 2004
  3 |                         http://www.apache.org/licenses/
  4 | 
  5 |    TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
  6 | 
  7 |    1. Definitions.
  8 | 
  9 |       "License" shall mean the terms and conditions for use, reproduction,
 10 |       and distribution as defined by Sections 1 through 9 of this document.
 11 | 
 12 |       "Licensor" shall mean the copyright owner or entity authorized by
 13 |       the copyright owner that is granting the License.
 14 | 
 15 |       "Legal Entity" shall mean the union of the acting entity and all
 16 |       other entities that control, are controlled by, or are under common
 17 |       control with that entity. For the purposes of this definition,
 18 |       "control" means (i) the power, direct or indirect, to cause the
 19 |       direction or management of such entity, whether by contract or
 20 |       otherwise, or (ii) ownership of fifty percent (50%) or more of the
 21 |       outstanding shares, or (iii) beneficial ownership of such entity.
 22 | 
 23 |       "You" (or "Your") shall mean an individual or Legal Entity
 24 |       exercising permissions granted by this License.
 25 | 
 26 |       "Source" form shall mean the preferred form for making modifications,
 27 |       including but not limited to software source code, documentation
 28 |       source, and configuration files.
 29 | 
 30 |       "Object" form shall mean any form resulting from mechanical
 31 |       transformation or translation of a Source form, including but
 32 |       not limited to compiled object code, generated documentation,
 33 |       and conversions to other media types.
 34 | 
 35 |       "Work" shall mean the work of authorship, whether in Source or
 36 |       Object form, made available under the License, as indicated by a
 37 |       copyright notice that is included in or attached to the work
 38 |       (an example is provided in the Appendix below).
 39 | 
 40 |       "Derivative Works" shall mean any work, whether in Source or Object
 41 |       form, that is based on (or derived from) the Work and for which the
 42 |       editorial revisions, annotations, elaborations, or other modifications
 43 |       represent, as a whole, an original work of authorship. For the purposes
 44 |       of this License, Derivative Works shall not include works that remain
 45 |       separable from, or merely link (or bind by name) to the interfaces of,
 46 |       the Work and Derivative Works thereof.
 47 | 
 48 |       "Contribution" shall mean any work of authorship, including
 49 |       the original version of the Work and any modifications or additions
 50 |       to that Work or Derivative Works thereof, that is intentionally
 51 |       submitted to Licensor for inclusion in the Work by the copyright owner
 52 |       or by an individual or Legal Entity authorized to submit on behalf of
 53 |       the copyright owner. For the purposes of this definition, "submitted"
 54 |       means any form of electronic, verbal, or written communication sent
 55 |       to the Licensor or its representatives, including but not limited to
 56 |       communication on electronic mailing lists, source code control systems,
 57 |       and issue tracking systems that are managed by, or on behalf of, the
 58 |       Licensor for the purpose of discussing and improving the Work, but
 59 |       excluding communication that is conspicuously marked or otherwise
 60 |       designated in writing by the copyright owner as "Not a Contribution."
 61 | 
 62 |       "Contributor" shall mean Licensor and any individual or Legal Entity
 63 |       on behalf of whom a Contribution has been received by Licensor and
 64 |       subsequently incorporated within the Work.
 65 | 
 66 |    2. Grant of Copyright License. Subject to the terms and conditions of
 67 |       this License, each Contributor hereby grants to You a perpetual,
 68 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 69 |       copyright license to reproduce, prepare Derivative Works of,
 70 |       publicly display, publicly perform, sublicense, and distribute the
 71 |       Work and such Derivative Works in Source or Object form.
 72 | 
 73 |    3. Grant of Patent License. Subject to the terms and conditions of
 74 |       this License, each Contributor hereby grants to You a perpetual,
 75 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 76 |       (except as stated in this section) patent license to make, have made,
 77 |       use, offer to sell, sell, import, and otherwise transfer the Work,
 78 |       where such license applies only to those patent claims licensable
 79 |       by such Contributor that are necessarily infringed by their
 80 |       Contribution(s) alone or by combination of their Contribution(s)
 81 |       with the Work to which such Contribution(s) was submitted. If You
 82 |       institute patent litigation against any entity (including a
 83 |       cross-claim or counterclaim in a lawsuit) alleging that the Work
 84 |       or a Contribution incorporated within the Work constitutes direct
 85 |       or contributory patent infringement, then any patent licenses
 86 |       granted to You under this License for that Work shall terminate
 87 |       as of the date such litigation is filed.
 88 | 
 89 |    4. Redistribution. You may reproduce and distribute copies of the
 90 |       Work or Derivative Works thereof in any medium, with or without
 91 |       modifications, and in Source or Object form, provided that You
 92 |       meet the following conditions:
 93 | 
 94 |       (a) You must give any other recipients of the Work or
 95 |           Derivative Works a copy of this License; and
 96 | 
 97 |       (b) You must cause any modified files to carry prominent notices
 98 |           stating that You changed the files; and
 99 | 
100 |       (c) You must retain, in the Source form of any Derivative Works
101 |           that You distribute, all copyright, patent, trademark, and
102 |           attribution notices from the Source form of the Work,
103 |           excluding those notices that do not pertain to any part of
104 |           the Derivative Works; and
105 | 
106 |       (d) If the Work includes a "NOTICE" text file as part of its
107 |           distribution, then any Derivative Works that You distribute must
108 |           include a readable copy of the attribution notices contained
109 |           within such NOTICE file, excluding those notices that do not
110 |           pertain to any part of the Derivative Works, in at least one
111 |           of the following places: within a NOTICE text file distributed
112 |           as part of the Derivative Works; within the Source form or
113 |           documentation, if provided along with the Derivative Works; or,
114 |           within a display generated by the Derivative Works, if and
115 |           wherever such third-party notices normally appear. The contents
116 |           of the NOTICE file are for informational purposes only and
117 |           do not modify the License. You may add Your own attribution
118 |           notices within Derivative Works that You distribute, alongside
119 |           or as an addendum to the NOTICE text from the Work, provided
120 |           that such additional attribution notices cannot be construed
121 |           as modifying the License.
122 | 
123 |       You may add Your own copyright statement to Your modifications and
124 |       may provide additional or different license terms and conditions
125 |       for use, reproduction, or distribution of Your modifications, or
126 |       for any such Derivative Works as a whole, provided Your use,
127 |       reproduction, and distribution of the Work otherwise complies with
128 |       the conditions stated in this License.
129 | 
130 |    5. Submission of Contributions. Unless You explicitly state otherwise,
131 |       any Contribution intentionally submitted for inclusion in the Work
132 |       by You to the Licensor shall be under the terms and conditions of
133 |       this License, without any additional terms or conditions.
134 |       Notwithstanding the above, nothing herein shall supersede or modify
135 |       the terms of any separate license agreement you may have executed
136 |       with Licensor regarding such Contributions.
137 | 
138 |    6. Trademarks. This License does not grant permission to use the trade
139 |       names, trademarks, service marks, or product names of the Licensor,
140 |       except as required for reasonable and customary use in describing the
141 |       origin of the Work and reproducing the content of the NOTICE file.
142 | 
143 |    7. Disclaimer of Warranty. Unless required by applicable law or
144 |       agreed to in writing, Licensor provides the Work (and each
145 |       Contributor provides its Contributions) on an "AS IS" BASIS,
146 |       WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 |       implied, including, without limitation, any warranties or conditions
148 |       of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 |       PARTICULAR PURPOSE. You are solely responsible for determining the
150 |       appropriateness of using or redistributing the Work and assume any
151 |       risks associated with Your exercise of permissions under this License.
152 | 
153 |    8. Limitation of Liability. In no event and under no legal theory,
154 |       whether in tort (including negligence), contract, or otherwise,
155 |       unless required by applicable law (such as deliberate and grossly
156 |       negligent acts) or agreed to in writing, shall any Contributor be
157 |       liable to You for damages, including any direct, indirect, special,
158 |       incidental, or consequential damages of any character arising as a
159 |       result of this License or out of the use or inability to use the
160 |       Work (including but not limited to damages for loss of goodwill,
161 |       work stoppage, computer failure or malfunction, or any and all
162 |       other commercial damages or losses), even if such Contributor
163 |       has been advised of the possibility of such damages.
164 | 
165 |    9. Accepting Warranty or Additional Liability. While redistributing
166 |       the Work or Derivative Works thereof, You may choose to offer,
167 |       and charge a fee for, acceptance of support, warranty, indemnity,
168 |       or other liability obligations and/or rights consistent with this
169 |       License. However, in accepting such obligations, You may act only
170 |       on Your own behalf and on Your sole responsibility, not on behalf
171 |       of any other Contributor, and only if You agree to indemnify,
172 |       defend, and hold each Contributor harmless for any liability
173 |       incurred by, or claims asserted against, such Contributor by reason
174 |       of your accepting any such warranty or additional liability.
175 | 
176 |    END OF TERMS AND CONDITIONS
177 | 
178 |    APPENDIX: How to apply the Apache License to your work.
179 | 
180 |       To apply the Apache License to your work, attach the following
181 |       boilerplate notice, with the fields enclosed by brackets "[]"
182 |       replaced with your own identifying information. (Don't include
183 |       the brackets!)  The text should be enclosed in the appropriate
184 |       comment syntax for the file format. We also recommend that a
185 |       file or class name and description of purpose be included on the
186 |       same "printed page" as the copyright notice for easier
187 |       identification within third-party archives.
188 | 
189 |    Copyright [yyyy] [name of copyright owner]
190 | 
191 |    Licensed under the Apache License, Version 2.0 (the "License");
192 |    you may not use this file except in compliance with the License.
193 |    You may obtain a copy of the License at
194 | 
195 |        http://www.apache.org/licenses/LICENSE-2.0
196 | 
197 |    Unless required by applicable law or agreed to in writing, software
198 |    distributed under the License is distributed on an "AS IS" BASIS,
199 |    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 |    See the License for the specific language governing permissions and
201 |    limitations under the License.
202 | 


--------------------------------------------------------------------------------
/utilities.py:
--------------------------------------------------------------------------------
  1 | import os
  2 | import sys
  3 | import opensim
  4 | import numpy as np
  5 | import casadi as ca
  6 | import shutil
  7 | import importlib
  8 | import pandas as pd
  9 | import platform
 10 | import urllib.request
 11 | import zipfile
 12 | 
 13 | def generateExternalFunction(pathOpenSimModel, outputDir, pathID,
 14 |                              outputFilename='F'):
 15 |     
 16 |     # %% Paths.
 17 |     os.makedirs(outputDir, exist_ok=True)
 18 |     pathOutputFile = os.path.join(outputDir, outputFilename + ".cpp")
 19 |     pathOutputMap = os.path.join(outputDir, outputFilename + "_map.npy")
 20 |     
 21 |     # %% Generate external Function (.cpp file).
 22 |     opensim.Logger.setLevelString('error')
 23 |     model = opensim.Model(pathOpenSimModel)
 24 |     model.initSystem()
 25 |     bodySet = model.getBodySet()
 26 |     
 27 |     nBodies = 0
 28 |     for i in range(bodySet.getSize()):        
 29 |         c_body = bodySet.get(i)
 30 |         c_body_name = c_body.getName()  
 31 |         if (c_body_name == 'patella_l' or c_body_name == 'patella_r'):
 32 |             continue
 33 |         nBodies += 1
 34 |     
 35 |     jointSet = model.get_JointSet()
 36 |     nJoints = jointSet.getSize()
 37 |     geometrySet = model.get_ContactGeometrySet()
 38 |     forceSet = model.get_ForceSet()
 39 |     coordinateSet = model.getCoordinateSet()
 40 |     nCoordinates = coordinateSet.getSize()
 41 |     coordinates = []
 42 |     for coor in range(nCoordinates):
 43 |         coordinates.append(coordinateSet.get(coor).getName())
 44 |     sides = ['r', 'l']
 45 |     for side in sides:
 46 |         # We do not include the coordinates from the patellofemoral joints,
 47 |         # since they only influence muscle paths, which we approximate using
 48 |         # polynomials offline.
 49 |         if 'knee_angle_{}_beta'.format(side) in coordinates:
 50 |             nCoordinates -= 1
 51 |             nJoints -= 1
 52 |         elif 'knee_angle_beta_{}'.format(side) in coordinates:
 53 |             nCoordinates -= 1
 54 |             nJoints -= 1        
 55 |     
 56 |     nContacts = 0
 57 |     for i in range(forceSet.getSize()):        
 58 |         c_force_elt = forceSet.get(i)        
 59 |         if c_force_elt.getConcreteClassName() == "SmoothSphereHalfSpaceForce":  
 60 |             nContacts += 1
 61 |     
 62 |     with open(pathOutputFile, "w") as f:
 63 |         
 64 |         # TODO: only include those that are necessary (model-specific).
 65 |         f.write('#include <OpenSim/Simulation/Model/Model.h>\n')
 66 |         f.write('#include <OpenSim/Simulation/SimbodyEngine/PinJoint.h>\n')
 67 |         f.write('#include <OpenSim/Simulation/SimbodyEngine/WeldJoint.h>\n')
 68 |         f.write('#include <OpenSim/Simulation/SimbodyEngine/PlanarJoint.h>\n')
 69 |         f.write('#include <OpenSim/Simulation/SimbodyEngine/Joint.h>\n')
 70 |         f.write('#include <OpenSim/Simulation/SimbodyEngine/SpatialTransform.h>\n')
 71 |         f.write('#include <OpenSim/Simulation/SimbodyEngine/CustomJoint.h>\n')
 72 |         f.write('#include <OpenSim/Common/LinearFunction.h>\n')
 73 |         f.write('#include <OpenSim/Common/PolynomialFunction.h>\n')
 74 |         f.write('#include <OpenSim/Common/MultiplierFunction.h>\n')
 75 |         f.write('#include <OpenSim/Common/Constant.h>\n')
 76 |         f.write('#include <OpenSim/Simulation/Model/SmoothSphereHalfSpaceForce.h>\n')
 77 |         f.write('#include <OpenSim/Simulation/SimulationUtilities.h>\n')
 78 |         f.write('#include "SimTKcommon/internal/recorder.h"\n\n')
 79 |         
 80 |         f.write('#include <iostream>\n')
 81 |         f.write('#include <iterator>\n')
 82 |         f.write('#include <random>\n')
 83 |         f.write('#include <cassert>\n')
 84 |         f.write('#include <algorithm>\n')
 85 |         f.write('#include <vector>\n')
 86 |         f.write('#include <fstream>\n\n')
 87 |         
 88 |         f.write('using namespace SimTK;\n')
 89 |         f.write('using namespace OpenSim;\n\n')
 90 |     
 91 |         f.write('constexpr int n_in = 2; \n')
 92 |         f.write('constexpr int n_out = 1; \n')
 93 |         
 94 |         f.write('constexpr int nCoordinates = %i; \n' % nCoordinates)
 95 |         f.write('constexpr int NX = nCoordinates*2; \n')
 96 |         f.write('constexpr int NU = nCoordinates; \n')
 97 |         
 98 |         # Residuals (joint torques), 3D GRFs, GRMs, and body origins.
 99 |         nOutputs = nCoordinates + 3*(2+2+nBodies)
100 |         f.write('constexpr int NR = %i; \n\n' % (nOutputs))
101 |     
102 |         f.write('template<typename T> \n')
103 |         f.write('T value(const Recorder& e) { return e; }; \n')
104 |         f.write('template<> \n')
105 |         f.write('double value(const Recorder& e) { return e.getValue(); }; \n\n')
106 |         
107 |         f.write('template<typename T>\n')
108 |         f.write('int F_generic(const T** arg, T** res) {\n\n')
109 |         
110 |         # Model
111 |         f.write('\t// Definition of model.\n')
112 |         f.write('\tOpenSim::Model* model;\n')
113 |         f.write('\tmodel = new OpenSim::Model();\n\n')
114 |         
115 |         # Bodies
116 |         f.write('\t// Definition of bodies.\n')
117 |         for i in range(bodySet.getSize()):        
118 |             c_body = bodySet.get(i)
119 |             c_body_name = c_body.getName()            
120 |             if (c_body_name == 'patella_l' or c_body_name == 'patella_r'):
121 |                 continue            
122 |             c_body_mass = c_body.get_mass()
123 |             c_body_mass_center = c_body.get_mass_center().to_numpy()
124 |             c_body_inertia = c_body.get_inertia()
125 |             c_body_inertia_vec3 = np.array([c_body_inertia.get(0), c_body_inertia.get(1), c_body_inertia.get(2)])        
126 |             f.write('\tOpenSim::Body* %s;\n' % c_body_name)
127 |             f.write('\t%s = new OpenSim::Body(\"%s\", %.20f, Vec3(%.20f, %.20f, %.20f), Inertia(%.20f, %.20f, %.20f, 0., 0., 0.));\n' % (c_body_name, c_body_name, c_body_mass, c_body_mass_center[0], c_body_mass_center[1], c_body_mass_center[2], c_body_inertia_vec3[0], c_body_inertia_vec3[1], c_body_inertia_vec3[2]))
128 |             f.write('\tmodel->addBody(%s);\n' % (c_body_name))
129 |             f.write('\n')
130 |         
131 |         # Joints
132 |         f.write('\t// Definition of joints.\n')
133 |         for i in range(jointSet.getSize()): 
134 |             c_joint = jointSet.get(i)
135 |             c_joint_type = c_joint.getConcreteClassName()
136 |             
137 |             c_joint_name = c_joint.getName()
138 |             if (c_joint_name == 'patellofemoral_l' or 
139 |                 c_joint_name == 'patellofemoral_r'):
140 |                 continue
141 |             
142 |             parent_frame = c_joint.get_frames(0)
143 |             parent_frame_name = parent_frame.getParentFrame().getName()
144 |             parent_frame_trans = parent_frame.get_translation().to_numpy()
145 |             parent_frame_or = parent_frame.get_orientation().to_numpy()
146 |             
147 |             child_frame = c_joint.get_frames(1)
148 |             child_frame_name = child_frame.getParentFrame().getName()
149 |             child_frame_trans = child_frame.get_translation().to_numpy()
150 |             child_frame_or = child_frame.get_orientation().to_numpy()
151 |             
152 |             # Custom joints
153 |             if c_joint_type == "CustomJoint":
154 | 
155 |                 f.write('\tSpatialTransform st_%s;\n' % c_joint.getName())
156 | 
157 |                 cObj = opensim.CustomJoint.safeDownCast(c_joint)
158 |                 spatialtransform = cObj.get_SpatialTransform()
159 | 
160 |                 for iCoord in range(6):
161 |                     if iCoord == 0:
162 |                         dofSel = spatialtransform.get_rotation1()
163 |                     elif iCoord == 1:
164 |                         dofSel = spatialtransform.get_rotation2()
165 |                     elif iCoord == 2:
166 |                         dofSel = spatialtransform.get_rotation3()
167 |                     elif iCoord == 3:
168 |                         dofSel = spatialtransform.get_translation1()
169 |                     elif iCoord == 4:
170 |                         dofSel = spatialtransform.get_translation2()
171 |                     elif iCoord == 5:
172 |                         dofSel = spatialtransform.get_translation3()
173 |                     coord = iCoord
174 | 
175 |                     # Transform axis.
176 |                     dofSel_axis = dofSel.get_axis().to_numpy()
177 |                     dofSel_f = dofSel.get_function()
178 |                     if dofSel_f.getConcreteClassName() == 'LinearFunction':
179 |                         dofSel_f_obj = opensim.LinearFunction.safeDownCast(dofSel_f)
180 |                         dofSel_f_slope = dofSel_f_obj.getSlope()
181 |                         dofSel_f_intercept = dofSel_f_obj.getIntercept()
182 |                         #c_coord = c_joint.get_coordinates(coord)
183 |                         c_coord_name = dofSel.get_coordinates(0)
184 |                         f.write('\tst_%s[%i].setCoordinateNames(OpenSim::Array<std::string>(\"%s\", 1, 1));\n' % (
185 |                         c_joint.getName(), coord, c_coord_name))
186 |                         f.write('\tst_%s[%i].setFunction(new LinearFunction(%.4f, %.4f));\n' % (
187 |                         c_joint.getName(), coord, dofSel_f_slope, dofSel_f_intercept))
188 |                     elif dofSel_f.getConcreteClassName() == 'PolynomialFunction':
189 |                         f.write('\tst_%s[%i].setCoordinateNames(OpenSim::Array<std::string>(\"%s\", 1, 1));\n' % (
190 |                         c_joint.getName(), coord, c_coord_name))
191 |                         dofSel_f_obj = opensim.PolynomialFunction.safeDownCast(dofSel_f)
192 |                         dofSel_f_coeffs = dofSel_f_obj.getCoefficients().to_numpy()
193 |                         c_nCoeffs = dofSel_f_coeffs.shape[0]
194 |                         if c_nCoeffs == 2:
195 |                             f.write('\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f}; \n' % (
196 |                             c_joint.getName(), coord, c_nCoeffs, dofSel_f_coeffs[0], dofSel_f_coeffs[1]))
197 |                         elif c_nCoeffs == 3:
198 |                             f.write('\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f, %.20f}; \n' % (
199 |                             c_joint.getName(), coord, c_nCoeffs, dofSel_f_coeffs[0], dofSel_f_coeffs[1], dofSel_f_coeffs[2]))
200 |                         elif c_nCoeffs == 4:
201 |                             f.write('\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f, %.20f, %.20f}; \n' % (
202 |                             c_joint.getName(), coord, c_nCoeffs, dofSel_f_coeffs[0], dofSel_f_coeffs[1], dofSel_f_coeffs[2],
203 |                             dofSel_f_coeffs[3]))
204 |                         elif c_nCoeffs == 5:
205 |                             f.write(
206 |                                 '\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f, %.20f, %.20f, %.20f}; \n' % (
207 |                                 c_joint.getName(), coord, c_nCoeffs, dofSel_f_coeffs[0], dofSel_f_coeffs[1], dofSel_f_coeffs[2],
208 |                                 dofSel_f_coeffs[3], dofSel_f_coeffs[4]))
209 |                         elif c_nCoeffs == 7:
210 |                             f.write(
211 |                                 '\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f, %.20f, %.20f, %.20f, %.20f, %.20f}; \n' % (
212 |                                 c_joint.getName(), coord, c_nCoeffs, dofSel_f_coeffs[0], dofSel_f_coeffs[1], dofSel_f_coeffs[2],
213 |                                 dofSel_f_coeffs[3], dofSel_f_coeffs[4], dofSel_f_coeffs[5], dofSel_f_coeffs[6]))
214 |                         else:
215 |                             raise ValueError("TODO")
216 |                         f.write('\tVector st_%s_%i_coeffs_vec(%i); \n' % (c_joint.getName(), coord, c_nCoeffs))
217 |                         f.write('\tfor (int i = 0; i < %i; ++i) st_%s_%i_coeffs_vec[i] = st_%s_%i_coeffs[i]; \n' % (
218 |                         c_nCoeffs, c_joint.getName(), coord, c_joint.getName(), coord))
219 |                         f.write('\tst_%s[%i].setFunction(new PolynomialFunction(st_%s_%i_coeffs_vec));\n' % (
220 |                         c_joint.getName(), coord, c_joint.getName(), coord))
221 |                     elif dofSel_f.getConcreteClassName() == 'MultiplierFunction':
222 |                         dofSel_f_obj = opensim.MultiplierFunction.safeDownCast(dofSel_f)
223 |                         dofSel_f_obj_scale = dofSel_f_obj.getScale()
224 |                         dofSel_f_obj_f = dofSel_f_obj.getFunction()
225 |                         dofSel_f_obj_f_name = dofSel_f_obj_f.getConcreteClassName()
226 |                         if dofSel_f_obj_f_name == 'Constant':
227 |                             dofSel_f_obj_f_obj = opensim.Constant.safeDownCast(dofSel_f_obj_f)
228 |                             dofSel_f_obj_f_obj_value = dofSel_f_obj_f_obj.getValue()
229 |                             f.write('\tst_%s[%i].setFunction(new MultiplierFunction(new Constant(%.20f), %.20f));\n' % (
230 |                             c_joint.getName(), coord, dofSel_f_obj_f_obj_value, dofSel_f_obj_scale))
231 |                         elif dofSel_f_obj_f_name == 'PolynomialFunction':
232 |                             f.write('\tst_%s[%i].setCoordinateNames(OpenSim::Array<std::string>(\"%s\", 1, 1));\n' % (
233 |                             c_joint.getName(), coord, c_coord_name))
234 |                             dofSel_f_obj_f_obj = opensim.PolynomialFunction.safeDownCast(dofSel_f_obj_f)
235 |                             dofSel_f_obj_f_coeffs = dofSel_f_obj_f_obj.getCoefficients().to_numpy()
236 |                             c_nCoeffs = dofSel_f_obj_f_coeffs.shape[0]
237 |                             if c_nCoeffs == 2:
238 |                                 f.write('\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f}; \n' % (
239 |                                 c_joint.getName(), coord, c_nCoeffs, dofSel_f_obj_f_coeffs[0], dofSel_f_obj_f_coeffs[1]))
240 |                             elif c_nCoeffs == 3:
241 |                                 f.write('\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f, %.20f}; \n' % (
242 |                                 c_joint.getName(), coord, c_nCoeffs, dofSel_f_obj_f_coeffs[0], dofSel_f_obj_f_coeffs[1],
243 |                                 dofSel_f_obj_f_coeffs[2]))
244 |                             elif c_nCoeffs == 4:
245 |                                 f.write('\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f, %.20f, %.20f}; \n' % (
246 |                                 c_joint.getName(), coord, c_nCoeffs, dofSel_f_obj_f_coeffs[0], dofSel_f_obj_f_coeffs[1],
247 |                                 dofSel_f_obj_f_coeffs[2], dofSel_f_obj_f_coeffs[3]))
248 |                             elif c_nCoeffs == 5:
249 |                                 f.write(
250 |                                     '\tosim_double_adouble st_%s_%i_coeffs[%i] = {%.20f, %.20f, %.20f, %.20f, %.20f}; \n' % (
251 |                                     c_joint.getName(), coord, c_nCoeffs, dofSel_f_obj_f_coeffs[0], dofSel_f_obj_f_coeffs[1],
252 |                                     dofSel_f_obj_f_coeffs[2], dofSel_f_obj_f_coeffs[3], dofSel_f_obj_f_coeffs[4]))
253 |                             else:
254 |                                 raise ValueError("TODO")
255 |                             f.write('\tVector st_%s_%i_coeffs_vec(%i); \n' % (c_joint.getName(), coord, c_nCoeffs))
256 |                             f.write('\tfor (int i = 0; i < %i; ++i) st_%s_%i_coeffs_vec[i] = st_%s_%i_coeffs[i]; \n' % (
257 |                             c_nCoeffs, c_joint.getName(), coord, c_joint.getName(), coord))
258 |                             f.write(
259 |                                 '\tst_%s[%i].setFunction(new MultiplierFunction(new PolynomialFunction(st_%s_%i_coeffs_vec), %.20f));\n' % (
260 |                                 c_joint.getName(), coord, c_joint.getName(), coord, dofSel_f_obj_scale))
261 |                         else:
262 |                             raise ValueError("Not supported")
263 |                     elif dofSel_f.getConcreteClassName() == 'Constant':
264 |                         dofSel_f_obj = opensim.Constant.safeDownCast(dofSel_f)
265 |                         dofSel_f_obj_value = dofSel_f_obj.getValue()
266 |                         f.write('\tst_%s[%i].setFunction(new Constant(%.20f));\n' % (
267 |                         c_joint.getName(), coord, dofSel_f_obj_value))
268 |                     else:
269 |                         raise ValueError(dofSel_f.getConcreteClassName() +" Not supported")
270 |                     f.write('\tst_%s[%i].setAxis(Vec3(%.20f, %.20f, %.20f));\n' % (
271 |                     c_joint.getName(), coord, dofSel_axis[0], dofSel_axis[1], dofSel_axis[2]))
272 |                 
273 |                 
274 |                 # Joint.
275 |                 f.write('\tOpenSim::%s* %s;\n' % (c_joint_type, c_joint.getName()))
276 |                 if parent_frame_name == "ground":
277 |                     f.write('\t%s = new OpenSim::%s(\"%s\", model->getGround(), Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f), *%s, Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f), st_%s);\n' % (c_joint.getName(), c_joint_type, c_joint.getName(), parent_frame_trans[0], parent_frame_trans[1], parent_frame_trans[2], parent_frame_or[0], parent_frame_or[1], parent_frame_or[2], child_frame_name, child_frame_trans[0], child_frame_trans[1], child_frame_trans[2], child_frame_or[0], child_frame_or[1], child_frame_or[2], c_joint.getName()))     
278 |                 else:
279 |                     f.write('\t%s = new OpenSim::%s(\"%s\", *%s, Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f), *%s, Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f), st_%s);\n' % (c_joint.getName(), c_joint_type, c_joint.getName(), parent_frame_name, parent_frame_trans[0], parent_frame_trans[1], parent_frame_trans[2], parent_frame_or[0], parent_frame_or[1], parent_frame_or[2], child_frame_name, child_frame_trans[0], child_frame_trans[1], child_frame_trans[2], child_frame_or[0], child_frame_or[1], child_frame_or[2], c_joint.getName()))
280 |                 
281 |             elif c_joint_type == 'PinJoint' or c_joint_type == 'WeldJoint' or c_joint_type == 'PlanarJoint':
282 |                 f.write('\tOpenSim::%s* %s;\n' % (c_joint_type, c_joint.getName()))
283 |                 if parent_frame_name == "ground":
284 |                     f.write('\t%s = new OpenSim::%s(\"%s\", model->getGround(), Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f), *%s, Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f));\n' % (c_joint.getName(), c_joint_type, c_joint.getName(), parent_frame_trans[0], parent_frame_trans[1], parent_frame_trans[2], parent_frame_or[0], parent_frame_or[1], parent_frame_or[2], child_frame_name, child_frame_trans[0], child_frame_trans[1], child_frame_trans[2], child_frame_or[0], child_frame_or[1], child_frame_or[2]))     
285 |                 else:
286 |                     f.write('\t%s = new OpenSim::%s(\"%s\", *%s, Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f), *%s, Vec3(%.20f, %.20f, %.20f), Vec3(%.20f, %.20f, %.20f));\n' % (c_joint.getName(), c_joint_type, c_joint.getName(), parent_frame_name, parent_frame_trans[0], parent_frame_trans[1], parent_frame_trans[2], parent_frame_or[0], parent_frame_or[1], parent_frame_or[2], child_frame_name, child_frame_trans[0], child_frame_trans[1], child_frame_trans[2], child_frame_or[0], child_frame_or[1], child_frame_or[2])) 
287 |             else:
288 |                 raise ValueError("TODO: joint type not yet supported")
289 |             f.write('\tmodel->addJoint(%s);\n' % (c_joint.getName()))
290 |             f.write('\n')  
291 |                 
292 |         # Contacts
293 |         f.write('\t// Definition of contacts.\n')   
294 |         for i in range(forceSet.getSize()):        
295 |             c_force_elt = forceSet.get(i)        
296 |             if c_force_elt.getConcreteClassName() == "SmoothSphereHalfSpaceForce":            
297 |                 c_force_elt_obj =  opensim.SmoothSphereHalfSpaceForce.safeDownCast(c_force_elt) 	
298 |                 
299 |                 socket0Name = c_force_elt.getSocketNames()[0]
300 |                 socket0 = c_force_elt.getSocket(socket0Name)
301 |                 socket0_obj = socket0.getConnecteeAsObject()
302 |                 socket0_objName = socket0_obj.getName()            
303 |                 geo0 = geometrySet.get(socket0_objName)
304 |                 geo0_loc = geo0.get_location().to_numpy()
305 |                 geo0_or = geo0.get_orientation().to_numpy()
306 |                 geo0_frameName = geo0.getFrame().getName()
307 |                 
308 |                 socket1Name = c_force_elt.getSocketNames()[1]
309 |                 socket1 = c_force_elt.getSocket(socket1Name)
310 |                 socket1_obj = socket1.getConnecteeAsObject()
311 |                 socket1_objName = socket1_obj.getName()            
312 |                 geo1 = geometrySet.get(socket1_objName)
313 |                 geo1_loc = geo1.get_location().to_numpy()
314 |                 # geo1_or = geo1.get_orientation().to_numpy()
315 |                 geo1_frameName = geo1.getFrame().getName()
316 |                 obj = opensim.ContactSphere.safeDownCast(geo1) 	
317 |                 geo1_radius = obj.getRadius()            
318 |                 
319 |                 f.write('\tOpenSim::%s* %s;\n' % (c_force_elt.getConcreteClassName(), c_force_elt.getName()))
320 |                 if geo0_frameName == "ground":
321 |                     f.write('\t%s = new %s(\"%s\", *%s, model->getGround());\n' % (c_force_elt.getName(), c_force_elt.getConcreteClassName(), c_force_elt.getName(), geo1_frameName))
322 |                 else:
323 |                     f.write('\t%s = new %s(\"%s\", *%s, *%s);\n' % (c_force_elt.getName(), c_force_elt.getConcreteClassName(), c_force_elt.getName(), geo1_frameName, geo0_frameName))
324 |                     
325 |                 f.write('\tVec3 %s_location(%.20f, %.20f, %.20f);\n' % (c_force_elt.getName(), geo1_loc[0], geo1_loc[1], geo1_loc[2]))
326 |                 f.write('\t%s->set_contact_sphere_location(%s_location);\n' % (c_force_elt.getName(), c_force_elt.getName()))
327 |                 f.write('\tdouble %s_radius = (%.20f);\n' % (c_force_elt.getName(), geo1_radius))
328 |                 f.write('\t%s->set_contact_sphere_radius(%s_radius );\n' % (c_force_elt.getName(), c_force_elt.getName()))
329 |                 f.write('\t%s->set_contact_half_space_location(Vec3(%.20f, %.20f, %.20f));\n' % (c_force_elt.getName(), geo0_loc[0], geo0_loc[1], geo0_loc[2]))
330 |                 f.write('\t%s->set_contact_half_space_orientation(Vec3(%.20f, %.20f, %.20f));\n' % (c_force_elt.getName(), geo0_or[0], geo0_or[1], geo0_or[2]))
331 |                 
332 |                 f.write('\t%s->set_stiffness(%.20f);\n' % (c_force_elt.getName(), c_force_elt_obj.get_stiffness()))
333 |                 f.write('\t%s->set_dissipation(%.20f);\n' % (c_force_elt.getName(), c_force_elt_obj.get_dissipation()))
334 |                 f.write('\t%s->set_static_friction(%.20f);\n' % (c_force_elt.getName(), c_force_elt_obj.get_static_friction()))
335 |                 f.write('\t%s->set_dynamic_friction(%.20f);\n' % (c_force_elt.getName(), c_force_elt_obj.get_dynamic_friction()))
336 |                 f.write('\t%s->set_viscous_friction(%.20f);\n' % (c_force_elt.getName(), c_force_elt_obj.get_viscous_friction()))
337 |                 f.write('\t%s->set_transition_velocity(%.20f);\n' % (c_force_elt.getName(), c_force_elt_obj.get_transition_velocity()))
338 |                 
339 |                 f.write('\t%s->connectSocket_sphere_frame(*%s);\n' % (c_force_elt.getName(), geo1_frameName))
340 |                 if geo0_frameName == "ground":
341 |                     f.write('\t%s->connectSocket_half_space_frame(model->getGround());\n' % (c_force_elt.getName()))                
342 |                 else:
343 |                     f.write('\t%s->connectSocket_half_space_frame(*%s);\n' % (c_force_elt.getName(), geo0_frameName))
344 |                 f.write('\tmodel->addComponent(%s);\n' % (c_force_elt.getName()))
345 |                 f.write('\n')
346 |                 
347 |         f.write('\t// Initialize system.\n')
348 |         f.write('\tSimTK::State* state;\n')
349 |         f.write('\tstate = new State(model->initSystem());\n\n')
350 |     
351 |         f.write('\t// Read inputs.\n')
352 |         f.write('\tstd::vector<T> x(arg[0], arg[0] + NX);\n')
353 |         f.write('\tstd::vector<T> u(arg[1], arg[1] + NU);\n\n')
354 |         
355 |         f.write('\t// States and controls.\n')
356 |         f.write('\tT ua[NU];\n')
357 |         f.write('\tVector QsUs(NX);\n')
358 |         f.write('\t/// States\n')
359 |         f.write('\tfor (int i = 0; i < NX; ++i) QsUs[i] = x[i];\n') 	
360 |         f.write('\t/// Controls\n')
361 |         f.write('\t/// OpenSim and Simbody have different state orders.\n')
362 |         f.write('\tauto indicesOSInSimbody = getIndicesOpenSimInSimbody(*model);\n')
363 |         f.write('\tfor (int i = 0; i < NU; ++i) ua[i] = u[indicesOSInSimbody[i]];\n\n')
364 |     
365 |         f.write('\t// Set state variables and realize.\n')
366 |         f.write('\tmodel->setStateVariableValues(*state, QsUs);\n')
367 |         f.write('\tmodel->realizeVelocity(*state);\n\n')
368 |         
369 |         f.write('\t// Compute residual forces.\n')
370 |         f.write('\t/// Set appliedMobilityForces (# mobilities).\n')
371 |         f.write('\tVector appliedMobilityForces(nCoordinates);\n')
372 |         f.write('\tappliedMobilityForces.setToZero();\n')
373 |         f.write('\t/// Set appliedBodyForces (# bodies + ground).\n')
374 |         f.write('\tVector_<SpatialVec> appliedBodyForces;\n')
375 |         f.write('\tint nbodies = model->getBodySet().getSize() + 1;\n')
376 |         f.write('\tappliedBodyForces.resize(nbodies);\n')
377 |         f.write('\tappliedBodyForces.setToZero();\n')
378 |         f.write('\t/// Set gravity.\n')
379 |         f.write('\tVec3 gravity(0);\n')
380 |         f.write('\tgravity[1] = %.20f;\n' % model.get_gravity()[1])
381 |         f.write('\t/// Add weights to appliedBodyForces.\n')
382 |         f.write('\tfor (int i = 0; i < model->getBodySet().getSize(); ++i) {\n')
383 |         f.write('\t\tmodel->getMatterSubsystem().addInStationForce(*state,\n')
384 |         f.write('\t\tmodel->getBodySet().get(i).getMobilizedBodyIndex(),\n')
385 |         f.write('\t\tmodel->getBodySet().get(i).getMassCenter(),\n')
386 |         f.write('\t\tmodel->getBodySet().get(i).getMass()*gravity, appliedBodyForces);\n')
387 |         f.write('\t}\n')    
388 |         f.write('\t/// Add contact forces to appliedBodyForces.\n')
389 |         
390 |         count = 0
391 |         for i in range(forceSet.getSize()):        
392 |             c_force_elt = forceSet.get(i)     
393 |             
394 |             if c_force_elt.getConcreteClassName() == "SmoothSphereHalfSpaceForce":
395 |                 c_force_elt_name = c_force_elt.getName()    
396 |                 
397 |                 f.write('\tArray<osim_double_adouble> Force_%s = %s->getRecordValues(*state);\n' % (str(count), c_force_elt_name))
398 |                 f.write('\tSpatialVec GRF_%s;\n' % (str(count)))           
399 |                 
400 |                 f.write('\tGRF_%s[0] = Vec3(Force_%s[3], Force_%s[4], Force_%s[5]);\n' % (str(count), str(count), str(count), str(count)))
401 |                 f.write('\tGRF_%s[1] = Vec3(Force_%s[0], Force_%s[1], Force_%s[2]);\n' % (str(count), str(count), str(count), str(count)))
402 |                 
403 |                 socket1Name = c_force_elt.getSocketNames()[1]
404 |                 socket1 = c_force_elt.getSocket(socket1Name)
405 |                 socket1_obj = socket1.getConnecteeAsObject()
406 |                 socket1_objName = socket1_obj.getName()            
407 |                 geo1 = geometrySet.get(socket1_objName)
408 |                 geo1_frameName = geo1.getFrame().getName()
409 |                 
410 |                 f.write('\tint c_idx_%s = model->getBodySet().get("%s").getMobilizedBodyIndex();\n' % (str(count), geo1_frameName))            
411 |                 f.write('\tappliedBodyForces[c_idx_%s] += GRF_%s;\n' % (str(count), str(count)))
412 |                 count += 1
413 |                 f.write('\n')
414 |                 
415 |         f.write('\t/// knownUdot.\n')
416 |         f.write('\tVector knownUdot(nCoordinates);\n')
417 |         f.write('\tknownUdot.setToZero();\n')
418 |         f.write('\tfor (int i = 0; i < nCoordinates; ++i) knownUdot[i] = ua[i];\n')
419 |         f.write('\t/// Calculate residual forces.\n')
420 |         f.write('\tVector residualMobilityForces(nCoordinates);\n')
421 |         f.write('\tresidualMobilityForces.setToZero();\n')
422 |         f.write('\tmodel->getMatterSubsystem().calcResidualForceIgnoringConstraints(*state,\n')
423 |         f.write('\t\t\tappliedMobilityForces, appliedBodyForces, knownUdot, residualMobilityForces);\n\n')
424 |         
425 |         # Get body origins.
426 |         f.write('\t/// Body origins.\n')
427 |         for i in range(bodySet.getSize()):        
428 |             c_body = bodySet.get(i)
429 |             c_body_name = c_body.getName()            
430 |             if (c_body_name == 'patella_l' or c_body_name == 'patella_r'):
431 |                 continue            
432 |             f.write('\tVec3 %s_or = %s->getPositionInGround(*state);\n' % (c_body_name, c_body_name))
433 |         f.write('\n')
434 |             
435 |         # Get GRFs.
436 |         f.write('\t/// Ground reaction forces.\n')
437 |         f.write('\tVec3 GRF_r(0), GRF_l(0);\n')
438 |         count = 0
439 |         for i in range(forceSet.getSize()):        
440 |             c_force_elt = forceSet.get(i)  
441 |             if c_force_elt.getConcreteClassName() == "SmoothSphereHalfSpaceForce":
442 |                 c_force_elt_name = c_force_elt.getName() 
443 |                 if c_force_elt_name[-2:] == "_r":
444 |                     f.write('\tGRF_r += GRF_%s[1];\n'  % (str(count)))
445 |                 elif c_force_elt_name[-2:] == "_l":
446 |                     f.write('\tGRF_l += GRF_%s[1];\n'  % (str(count)))
447 |                 else:
448 |                     raise ValueError("Cannot identify contact side")
449 |                 count += 1
450 |         f.write('\n')
451 |             
452 |         # Get GRMs.
453 |         f.write('\t/// Ground reaction moments.\n')
454 |         f.write('\tVec3 GRM_r(0), GRM_l(0);\n')
455 |         f.write('\tVec3 normal(0, 1, 0);\n\n')
456 |         count = 0
457 |         geo1_frameNames = []
458 |         for i in range(forceSet.getSize()):        
459 |             c_force_elt = forceSet.get(i)  
460 |             if c_force_elt.getConcreteClassName() == "SmoothSphereHalfSpaceForce":
461 |                 c_force_elt_name = c_force_elt.getName() 
462 |                 socket1Name = c_force_elt.getSocketNames()[1]
463 |                 socket1 = c_force_elt.getSocket(socket1Name)
464 |                 socket1_obj = socket1.getConnecteeAsObject()
465 |                 socket1_objName = socket1_obj.getName()            
466 |                 geo1 = geometrySet.get(socket1_objName)
467 |                 geo1_frameName = geo1.getFrame().getName() 
468 |                 
469 |                 if not geo1_frameName in geo1_frameNames:
470 |                     f.write('\tSimTK::Transform TR_GB_%s = %s->getMobilizedBody().getBodyTransform(*state);\n' % (geo1_frameName, geo1_frameName))    
471 |                     geo1_frameNames.append(geo1_frameName)
472 |                     
473 |                 f.write('\tVec3 %s_location_G = %s->findStationLocationInGround(*state, %s_location);\n' % (c_force_elt_name, geo1_frameName, c_force_elt_name))                
474 |                 f.write('\tVec3 %s_locationCP_G = %s_location_G - %s_radius * normal;\n' % (c_force_elt_name, c_force_elt_name, c_force_elt_name))
475 |                 f.write('\tVec3 locationCP_G_adj_%i = %s_locationCP_G - 0.5*%s_locationCP_G[1] * normal;\n' % (count, c_force_elt_name, c_force_elt_name))
476 |                 f.write('\tVec3 %s_locationCP_B = model->getGround().findStationLocationInAnotherFrame(*state, locationCP_G_adj_%i, *%s);\n' % (c_force_elt_name, count, geo1_frameName))
477 |                 f.write('\tVec3 GRM_%i = (TR_GB_%s*%s_locationCP_B) %% GRF_%s[1];\n' % (count, geo1_frameName, c_force_elt_name, str(count)))
478 |                 
479 |                 if c_force_elt_name[-2:] == "_r":
480 |                     f.write('\tGRM_r += GRM_%i;\n'  % (count))   
481 |                 elif c_force_elt_name[-2:] == "_l": 
482 |                     f.write('\tGRM_l += GRM_%i;\n'  % (count))   
483 |                 else:
484 |                     raise ValueError("Cannot identify contact side")
485 |                 f.write('\n')                   
486 |                 count += 1
487 |         
488 |         # Save dict pointing to which elements are returned by F and in which
489 |         # order, such as to facilitate using F when formulating problem.
490 |         F_map = {}
491 |         
492 |         f.write('\t/// Outputs.\n')        
493 |         # Export residuals (joint torques).
494 |         f.write('\t/// Residual forces (OpenSim and Simbody have different state orders).\n')
495 |         f.write('\tauto indicesSimbodyInOS = getIndicesSimbodyInOpenSim(*model);\n')
496 |         f.write('\tfor (int i = 0; i < NU; ++i) res[0][i] =\n')
497 |         f.write('\t\t\tvalue<T>(residualMobilityForces[indicesSimbodyInOS[i]]);\n')
498 |         F_map['residuals'] = {}
499 |         count = 0
500 |         for coordinate in coordinates:
501 |             if 'beta' in coordinate:
502 |                 continue
503 |             F_map['residuals'][coordinate] = count 
504 |             count += 1
505 |         count_acc = nCoordinates
506 |         
507 |         # Export GRFs
508 |         f.write('\t/// Ground reaction forces.\n')
509 |         f.write('\tfor (int i = 0; i < 3; ++i) res[0][i + %i] = value<T>(GRF_r[i]);\n' % (count_acc))
510 |         f.write('\tfor (int i = 0; i < 3; ++i) res[0][i + %i] = value<T>(GRF_l[i]);\n' % (count_acc+3))
511 |         F_map['GRFs'] = {}      
512 |         F_map['GRFs']['right'] = range(count_acc, count_acc+3)
513 |         F_map['GRFs']['left'] = range(count_acc+3, count_acc+6)
514 |         count_acc += 6
515 |         
516 |         # Export GRMs
517 |         f.write('\t/// Ground reaction moments.\n')
518 |         f.write('\tfor (int i = 0; i < 3; ++i) res[0][i + %i] = value<T>(GRM_r[i]);\n' % (count_acc))
519 |         f.write('\tfor (int i = 0; i < 3; ++i) res[0][i + %i] = value<T>(GRM_l[i]);\n' % (count_acc+3))
520 |         F_map['GRMs'] = {}
521 |         F_map['GRMs']['right'] = range(count_acc, count_acc+3)
522 |         F_map['GRMs']['left'] = range(count_acc+3, count_acc+6)
523 |         count_acc += 6
524 |         
525 |         # Export body origins.        
526 |         f.write('\t/// Body origins.\n')
527 |         F_map['body_origins'] = {}
528 |         count = 0
529 |         for i in range(bodySet.getSize()):        
530 |             c_body = bodySet.get(i)
531 |             c_body_name = c_body.getName()
532 |             if (c_body_name == 'patella_l' or c_body_name == 'patella_r'):
533 |                 continue
534 |             f.write('\tfor (int i = 0; i < 3; ++i) res[0][i + %i] = value<T>(%s_or[i]);\n' % (count_acc+count*3, c_body_name))
535 |             F_map['body_origins'][c_body_name] = range(count_acc+count*3, count_acc+count*3+3)
536 |             count += 1
537 |         count_acc += 3*count
538 |             
539 |         f.write('\n')
540 |         f.write('\treturn 0;\n')
541 |         f.write('}\n\n')
542 |         
543 |         f.write('int main() {\n')
544 |         f.write('\tRecorder x[NX];\n')
545 |         f.write('\tRecorder u[NU];\n')
546 |         f.write('\tRecorder tau[NR];\n')
547 |         f.write('\tfor (int i = 0; i < NX; ++i) x[i] <<= 0;\n')
548 |         f.write('\tfor (int i = 0; i < NU; ++i) u[i] <<= 0;\n')
549 |         f.write('\tconst Recorder* Recorder_arg[n_in] = { x,u };\n')
550 |         f.write('\tRecorder* Recorder_res[n_out] = { tau };\n')
551 |         f.write('\tF_generic<Recorder>(Recorder_arg, Recorder_res);\n')
552 |         f.write('\tdouble res[NR];\n')
553 |         f.write('\tfor (int i = 0; i < NR; ++i) Recorder_res[0][i] >>= res[i];\n')
554 |         f.write('\tRecorder::stop_recording();\n')
555 |         f.write('\treturn 0;\n')
556 |         f.write('}\n')
557 |         
558 |         # Save dict
559 |         np.save(pathOutputMap, F_map)
560 |         
561 |     # %% Build external Function.
562 |     buildExternalFunction(outputFilename, outputDir, 3*nCoordinates)
563 |         
564 |     # %% Torque verification test.
565 |     # Delete previous saved dummy motion if needed.
566 |     if os.path.exists(os.path.join(pathID, "dummyData.sto")):
567 |         os.remove(os.path.join(pathID, "dummyData.sto"))
568 | 
569 |     # Create a dummy motion for ID.
570 |     nCoordinatesAll = coordinateSet.getSize()
571 |     dummyDataa = np.zeros((10, nCoordinatesAll + 1))
572 |     for coor in range(nCoordinatesAll):
573 |         dummyDataa[:, coor + 1] = np.random.rand()*0.05
574 |     dummyDataa[:, 0] = np.linspace(0.01, 0.1, 10)
575 |     labelsDummy = []
576 |     labelsDummy.append("time")
577 |     for coor in range(nCoordinatesAll):
578 |         labelsDummy.append(coordinateSet.get(coor).getName())
579 |     numpy2storage(labelsDummy, dummyDataa, os.path.join(pathID,
580 |                                                         "dummyData.sto"))
581 | 
582 |     # Solve inverse dynamics.
583 |     pathGenericIDSetupFile = os.path.join(pathID, "SetupID.xml")
584 |     idTool = opensim.InverseDynamicsTool(pathGenericIDSetupFile)
585 |     idTool.setName("ID_withOsimAndIDTool")
586 |     idTool.setModelFileName(pathOpenSimModel)
587 |     idTool.setResultsDir(outputDir)
588 |     idTool.setCoordinatesFileName(os.path.join(pathID, "dummyData.sto"))
589 |     idTool.setOutputGenForceFileName("ID_withOsimAndIDTool.sto")       
590 |     pathSetupID = os.path.join(outputDir, "SetupID.xml")
591 |     idTool.printToXML(pathSetupID)
592 |     idTool.run()
593 |     
594 |     # Extract torques from .osim + ID tool.    
595 |     headers = []    
596 |     for coord in range(nCoordinatesAll):                
597 |         if (coordinateSet.get(coord).getName() == "pelvis_tx" or 
598 |             coordinateSet.get(coord).getName() == "pelvis_ty" or 
599 |             coordinateSet.get(coord).getName() == "pelvis_tz" or
600 |             coordinateSet.get(coord).getName() == "knee_angle_r_beta" or 
601 |             coordinateSet.get(coord).getName() == "knee_angle_l_beta" or
602 |             coordinateSet.get(coord).getName() == "knee_angle_beta_r" or 
603 |             coordinateSet.get(coord).getName() == "knee_angle_beta_l"):
604 |             suffix_header = "_force"
605 |         else:
606 |             suffix_header = "_moment"
607 |         headers.append(coordinateSet.get(coord).getName() + suffix_header)
608 |         
609 |     from utilities import storage2df    
610 |     ID_osim_df = storage2df(os.path.join(outputDir,
611 |                                   "ID_withOsimAndIDTool.sto"), headers)
612 |     ID_osim = np.zeros((nCoordinates))
613 |     count = 0
614 |     for coordinate in coordinates:
615 |         if (coordinate == "pelvis_tx" or 
616 |             coordinate == "pelvis_ty" or 
617 |             coordinate == "pelvis_tz"):
618 |             suffix_header = "_force"
619 |         else:
620 |             suffix_header = "_moment"
621 |         if 'beta' in coordinate:
622 |             continue                
623 |         ID_osim[count] = ID_osim_df.iloc[0][coordinate + suffix_header]
624 |         count += 1
625 |     
626 |     # Extract torques from external function.
627 |     os_system = platform.system()
628 |     if os_system == 'Windows':
629 |         F = ca.external('F', os.path.join(outputDir, 
630 |                                           outputFilename + '.dll'))
631 |     elif os_system == 'Linux':
632 |         F = ca.external('F', os.path.join(outputDir, 
633 |                                           outputFilename + '.so'))
634 |     elif os_system == 'Darwin':
635 |         F = ca.external('F', os.path.join(outputDir, 
636 |                                           outputFilename + '.dylib')) 
637 |     DefaultPos = storage2df(os.path.join(pathID,
638 |                                          "dummyData.sto"), coordinates)
639 |     vecInput = np.zeros((nCoordinates * 3, 1))    
640 |     coordinates_sel = []
641 |     for coord in coordinates:
642 |         if 'beta' in coord:
643 |             continue
644 |         coordinates_sel.append(coord)        
645 |     idxCoord4F = [coordinates_sel.index(coord) 
646 |                   for coord in list(F_map['residuals'].keys())]
647 |     for c, coor in enumerate(coordinates_sel):        
648 |         vecInput[idxCoord4F[c] * 2] = DefaultPos.iloc[0][coor]
649 |     ID_F = (F(vecInput)).full().flatten()[:nCoordinates]
650 |     
651 |     # Verify torques from external match torques from .osim + ID tool.
652 |     # TODO: weird that check had to be relaxed
653 |     diff_ID = np.abs(ID_osim - ID_F)
654 |     for i in range(diff_ID.shape[0]):
655 |         if diff_ID[i] > 1e-6:            
656 |             diff_ID_perc = diff_ID[i]/np.abs(ID_osim[i])*100            
657 |             if diff_ID_perc > 0.1:
658 |                 raise ValueError("Torque verification test failed")    
659 |     # assert(np.max(np.abs(ID_osim - ID_F)) < 1e-6), (
660 |     #     "Torque verification test failed")
661 |     print('Torque verification test passed')
662 | 
663 | # %% Generate c-code with external function (and its Jacobian).
664 | def generateF(dim):
665 |     import foo
666 |     importlib.reload(foo)
667 |     cg = ca.CodeGenerator('foo_jac')
668 |     arg = ca.SX.sym('arg', dim)
669 |     y,_,_ = foo.foo(arg)
670 |     F = ca.Function('F',[arg],[y])
671 |     cg.add(F)
672 |     cg.add(F.jacobian())
673 |     cg.generate()
674 | 
675 | # %% Build/compile external function.
676 | def buildExternalFunction(filename, CPP_DIR, nInputs):       
677 |     
678 |     # %% Part 1: build expression graph (i.e., generate foo.py).
679 |     pathMain = os.getcwd()
680 |     pathBuildExpressionGraph = os.path.join(pathMain, 'buildExpressionGraph')
681 |     pathBuild = os.path.join(pathMain, 'build-ExpressionGraph' + filename)
682 |     os.makedirs(pathBuild, exist_ok=True)
683 |     OpenSimAD_DIR = os.path.join(pathMain, 'opensimAD-install')
684 |     os.makedirs(OpenSimAD_DIR, exist_ok=True)
685 |     os_system = platform.system()
686 |     
687 |     if os_system == 'Windows':
688 |         pathBuildExpressionGraphOS = os.path.join(pathBuildExpressionGraph, 'windows') 
689 |         OpenSimADOS_DIR = os.path.join(OpenSimAD_DIR, 'windows')        
690 |         BIN_DIR = os.path.join(OpenSimADOS_DIR, 'bin')
691 |         SDK_DIR = os.path.join(OpenSimADOS_DIR, 'sdk')
692 |         # Download libraries if not existing locally.
693 |         if not os.path.exists(BIN_DIR):
694 |             url = 'https://sourceforge.net/projects/opensimad/files/windows.zip'
695 |             zipfilename = 'windows.zip'                
696 |             download_file(url, zipfilename)
697 |             with zipfile.ZipFile('windows.zip', 'r') as zip_ref:
698 |                 zip_ref.extractall(OpenSimAD_DIR)
699 |             os.remove('windows.zip')
700 |         cmd1 = 'cmake "' + pathBuildExpressionGraphOS + '"  -A x64 -DTARGET_NAME:STRING="' + filename + '" -DSDK_DIR:PATH="' + SDK_DIR + '" -DCPP_DIR:PATH="' + CPP_DIR + '"'
701 |         cmd2 = "cmake --build . --config RelWithDebInfo"
702 |         
703 |     elif os_system == 'Linux':
704 |         pathBuildExpressionGraphOS = os.path.join(pathBuildExpressionGraph, 'linux')
705 |         OpenSimADOS_DIR = os.path.join(OpenSimAD_DIR, 'linux')
706 |         # Download libraries if not existing locally.
707 |         if not os.path.exists(os.path.join(OpenSimAD_DIR, 'linux', 'lib')):
708 |             url = 'https://sourceforge.net/projects/opensimad/files/linux.tar.gz'
709 |             zipfilename = 'linux.tar.gz'                
710 |             download_file(url, zipfilename)
711 |             cmd_tar = 'tar -xf linux.tar.gz -C "{}"'.format(OpenSimAD_DIR)
712 |             os.system(cmd_tar)
713 |             os.remove('linux.tar.gz')
714 |         cmd1 = 'cmake "' + pathBuildExpressionGraphOS + '" -DTARGET_NAME:STRING="' + filename + '" -DSDK_DIR:PATH="' + OpenSimADOS_DIR + '" -DCPP_DIR:PATH="' + CPP_DIR + '"'
715 |         cmd2 = "make"
716 |         BIN_DIR = pathBuild
717 |         
718 |     elif os_system == 'Darwin':
719 |         pathBuildExpressionGraphOS = os.path.join(pathBuildExpressionGraph, 'macOS')
720 |         OpenSimADOS_DIR = os.path.join(OpenSimAD_DIR, 'macOS')
721 |         # Download libraries if not existing locally.
722 |         if not os.path.exists(os.path.join(OpenSimAD_DIR, 'macOS', 'lib')):
723 |             url = 'https://sourceforge.net/projects/opensimad/files/macOS.tgz'
724 |             zipfilename = 'macOS.tgz'                
725 |             download_file(url, zipfilename)
726 |             cmd_tar = 'tar -xf macOS.tgz -C "{}"'.format(OpenSimAD_DIR)
727 |             os.system(cmd_tar)
728 |             os.remove('macOS.tgz')
729 |         cmd1 = 'cmake "' + pathBuildExpressionGraphOS + '" -DTARGET_NAME:STRING="' + filename + '" -DSDK_DIR:PATH="' + OpenSimADOS_DIR + '" -DCPP_DIR:PATH="' + CPP_DIR + '"'
730 |         cmd2 = "make"
731 |         BIN_DIR = pathBuild
732 |     
733 |     os.chdir(pathBuild)    
734 |     os.system(cmd1)    
735 |     os.system(cmd2)
736 |     
737 |     if os_system == 'Windows':
738 |         os.chdir(BIN_DIR)
739 |         path_EXE = os.path.join(pathBuild, 'RelWithDebInfo', filename + '.exe')
740 |         cmd2w = '"{}"'.format(path_EXE)
741 |         os.system(cmd2w)
742 |     
743 |     # %% Part 2: build external function (i.e., build .dll).
744 |     fooName = "foo.py"
745 |     pathBuildExternalFunction = os.path.join(pathMain, 'buildExternalFunction')
746 |     path_external_filename_foo = os.path.join(BIN_DIR, fooName)
747 |     path_external_functions_filename_build = os.path.join(pathMain, 'build-ExternalFunction' + filename)
748 |     path_external_functions_filename_install = os.path.join(pathMain, 'install-ExternalFunction' + filename)
749 |     os.makedirs(path_external_functions_filename_build, exist_ok=True) 
750 |     os.makedirs(path_external_functions_filename_install, exist_ok=True)
751 |     shutil.copy2(path_external_filename_foo, pathBuildExternalFunction)
752 |     
753 |     sys.path.append(pathBuildExternalFunction)
754 |     os.chdir(pathBuildExternalFunction)
755 |     
756 |     generateF(nInputs)
757 |     
758 |     if os_system == 'Windows':
759 |         cmd3 = 'cmake "' + pathBuildExternalFunction + '" -A x64 -DTARGET_NAME:STRING="' + filename + '" -DINSTALL_DIR:PATH="' + path_external_functions_filename_install + '"'
760 |         cmd4 = "cmake --build . --config RelWithDebInfo --target install"
761 |     elif os_system == 'Linux':
762 |         cmd3 = 'cmake "' + pathBuildExternalFunction + '" -DTARGET_NAME:STRING="' + filename + '" -DINSTALL_DIR:PATH="' + path_external_functions_filename_install + '"'
763 |         cmd4 = "make install"
764 |     elif os_system == 'Darwin':
765 |         cmd3 = 'cmake "' + pathBuildExternalFunction + '" -DTARGET_NAME:STRING="' + filename + '" -DINSTALL_DIR:PATH="' + path_external_functions_filename_install + '"'
766 |         cmd4 = "make install"
767 |     
768 |     os.chdir(path_external_functions_filename_build)
769 |     os.system(cmd3)
770 |     os.system(cmd4)    
771 |     os.chdir(pathMain)
772 |     
773 |     if os_system == 'Windows':
774 |         shutil.copy2(os.path.join(path_external_functions_filename_install, 'bin', filename + '.dll'), CPP_DIR)
775 |     elif os_system == 'Linux':
776 |         shutil.copy2(os.path.join(path_external_functions_filename_install, 'lib', 'lib' + filename + '.so'), CPP_DIR)
777 |         os.rename(os.path.join(CPP_DIR, 'lib' + filename + '.so'), os.path.join(CPP_DIR, filename + '.so'))
778 |     elif os_system == 'Darwin':
779 |         shutil.copy2(os.path.join(path_external_functions_filename_install, 'lib', 'lib' + filename + '.dylib'), CPP_DIR)
780 |         os.rename(os.path.join(CPP_DIR, 'lib' + filename + '.dylib'), os.path.join(CPP_DIR, filename + '.dylib'))
781 |     
782 |     os.remove(os.path.join(pathBuildExternalFunction, "foo_jac.c"))
783 |     os.remove(os.path.join(pathBuildExternalFunction, fooName))
784 |     os.remove(path_external_filename_foo)
785 |     shutil.rmtree(pathBuild)
786 |     shutil.rmtree(path_external_functions_filename_install)
787 |     shutil.rmtree(path_external_functions_filename_build)    
788 | 
789 | # %% From storage file to numpy array.
790 | def storage2numpy(storage_file, excess_header_entries=0):
791 |     """Returns the data from a storage file in a numpy format. Skips all lines
792 |     up to and including the line that says 'endheader'.
793 |     Parameters
794 |     ----------
795 |     storage_file : str
796 |         Path to an OpenSim Storage (.sto) file.
797 |     Returns
798 |     -------
799 |     data : np.ndarray (or numpy structure array or something?)
800 |         Contains all columns from the storage file, indexable by column name.
801 |     excess_header_entries : int, optional
802 |         If the header row has more names in it than there are data columns.
803 |         We'll ignore this many header row entries from the end of the header
804 |         row. This argument allows for a hacky fix to an issue that arises from
805 |         Static Optimization '.sto' outputs.
806 |     Examples
807 |     --------
808 |     Columns from the storage file can be obtained as follows:
809 |         >>> data = storage2numpy('<filename>')
810 |         >>> data['ground_force_vy']
811 |     """
812 |     # What's the line number of the line containing 'endheader'?
813 |     f = open(storage_file, 'r')
814 | 
815 |     header_line = False
816 |     for i, line in enumerate(f):
817 |         if header_line:
818 |             column_names = line.split()
819 |             break
820 |         if line.count('endheader') != 0:
821 |             line_number_of_line_containing_endheader = i + 1
822 |             header_line = True
823 |     f.close()
824 | 
825 |     # With this information, go get the data.
826 |     if excess_header_entries == 0:
827 |         names = True
828 |         skip_header = line_number_of_line_containing_endheader
829 |     else:
830 |         names = column_names[:-excess_header_entries]
831 |         skip_header = line_number_of_line_containing_endheader + 1
832 |     data = np.genfromtxt(storage_file, names=names,
833 |             skip_header=skip_header)
834 | 
835 |     return data
836 | 
837 | # %% From storage file to DataFrame.
838 | def storage2df(storage_file, headers):
839 |     # Extract data
840 |     data = storage2numpy(storage_file)
841 |     out = pd.DataFrame(data=data['time'], columns=['time'])    
842 |     for count, header in enumerate(headers):
843 |         out.insert(count + 1, header, data[header])    
844 |     
845 |     return out
846 | 
847 | # %% From numpy array to storage file.
848 | def numpy2storage(labels, data, storage_file):
849 |     assert data.shape[1] == len(labels), "# labels doesn't match columns"
850 |     assert labels[0] == "time"
851 | 
852 |     f = open(storage_file, 'w')
853 |     f.write('name %s\n' % storage_file)
854 |     f.write('datacolumns %d\n' % data.shape[1])
855 |     f.write('datarows %d\n' % data.shape[0])
856 |     f.write('range %f %f\n' % (np.min(data[:, 0]), np.max(data[:, 0])))
857 |     f.write('endheader \n')
858 | 
859 |     for i in range(len(labels)):
860 |         f.write('%s\t' % labels[i])
861 |     f.write('\n')
862 | 
863 |     for i in range(data.shape[0]):
864 |         for j in range(data.shape[1]):
865 |             f.write('%20.8f\t' % data[i, j])
866 |         f.write('\n')
867 | 
868 |     f.close()
869 |     
870 | # %% Download file given url.
871 | def download_file(url, file_name):
872 |     with urllib.request.urlopen(url) as response, open(file_name, 'wb') as out_file:
873 |         shutil.copyfileobj(response, out_file)
874 | 


--------------------------------------------------------------------------------