I am trying to build up something more complex from as simple of a start as I can. I converted the example2DWalking.m file to python, and made it a torque-drive problem.
The problem converges quite fast, but I get nan for all /forceset. The first tracking simulation does not have this issue though. In spite of this, my extracted ground reaction forces look okay.
Please find attached the output, and the code.
Thanks in advance!
Code: Select all
[info] Loaded model 2D_gait from file D:\softs\github_david\Openpose-to-Opensim\Pose_from_simu_draft\Simulations\Models\2D_model.osim
[info] Adding reserve actuators with an optimal force of 250.0...
[info] Added 10 reserve actuator(s), for each of the following coordinates:
[info] /jointset/groundPelvis/pelvis_tilt
[info] /jointset/groundPelvis/pelvis_tx
[info] /jointset/groundPelvis/pelvis_ty
[info] /jointset/hip_l/hip_flexion_l
[info] /jointset/hip_r/hip_flexion_r
[info] /jointset/knee_l/knee_angle_l
[info] /jointset/knee_r/knee_angle_r
[info] /jointset/ankle_l/ankle_angle_l
[info] /jointset/ankle_r/ankle_angle_r
[info] /jointset/lumbar/lumbar
[info] Loaded model 2D_gait from file D:\softs\github_david\Openpose-to-Opensim\Pose_from_simu_draft\Simulations\Models\2D_model_000.osim
[info] ========================================================================
[info] MocoCasADiSolver starting.
[info] Sat Nov 18 01:44:24 2023
[info] ------------------------------------------------------------------------
Costs: (total: 1)
effort. MocoControlGoal, enabled: true, mode: cost, weight: 10.0
control: /forceset/reserve_jointset_groundPelvis_pelvis_tilt, weight: 1.0
control: /forceset/reserve_jointset_groundPelvis_pelvis_tx, weight: 1.0
control: /forceset/reserve_jointset_groundPelvis_pelvis_ty, weight: 1.0
control: /forceset/reserve_jointset_hip_l_hip_flexion_l, weight: 1.0
control: /forceset/reserve_jointset_hip_r_hip_flexion_r, weight: 1.0
control: /forceset/reserve_jointset_knee_l_knee_angle_l, weight: 1.0
control: /forceset/reserve_jointset_knee_r_knee_angle_r, weight: 1.0
control: /forceset/reserve_jointset_ankle_l_ankle_angle_l, weight: 1.0
control: /forceset/reserve_jointset_ankle_r_ankle_angle_r, weight: 1.0
control: /forceset/reserve_jointset_lumbar_lumbar, weight: 1.0
Endpoint constraints: (total: 2)
symmetryGoal. MocoPeriodicityGoal, enabled: true, mode: endpoint_constraint
state periodicity pairs:
initial: /jointset/groundPelvis/pelvis_tilt/value, final: /jointset/groundPelvis/pelvis_tilt/value
initial: /jointset/groundPelvis/pelvis_tilt/speed, final: /jointset/groundPelvis/pelvis_tilt/speed
initial: /jointset/groundPelvis/pelvis_tx/speed, final: /jointset/groundPelvis/pelvis_tx/speed
initial: /jointset/groundPelvis/pelvis_ty/value, final: /jointset/groundPelvis/pelvis_ty/value
initial: /jointset/groundPelvis/pelvis_ty/speed, final: /jointset/groundPelvis/pelvis_ty/speed
initial: /jointset/hip_l/hip_flexion_l/value, final: /jointset/hip_r/hip_flexion_r/value
initial: /jointset/hip_l/hip_flexion_l/speed, final: /jointset/hip_r/hip_flexion_r/speed
initial: /jointset/hip_r/hip_flexion_r/value, final: /jointset/hip_l/hip_flexion_l/value
initial: /jointset/hip_r/hip_flexion_r/speed, final: /jointset/hip_l/hip_flexion_l/speed
initial: /jointset/knee_l/knee_angle_l/value, final: /jointset/knee_r/knee_angle_r/value
initial: /jointset/knee_l/knee_angle_l/speed, final: /jointset/knee_r/knee_angle_r/speed
initial: /jointset/knee_r/knee_angle_r/value, final: /jointset/knee_l/knee_angle_l/value
initial: /jointset/knee_r/knee_angle_r/speed, final: /jointset/knee_l/knee_angle_l/speed
initial: /jointset/ankle_l/ankle_angle_l/value, final: /jointset/ankle_r/ankle_angle_r/value
initial: /jointset/ankle_l/ankle_angle_l/speed, final: /jointset/ankle_r/ankle_angle_r/speed
initial: /jointset/ankle_r/ankle_angle_r/value, final: /jointset/ankle_l/ankle_angle_l/value
initial: /jointset/ankle_r/ankle_angle_r/speed, final: /jointset/ankle_l/ankle_angle_l/speed
initial: /jointset/lumbar/lumbar/value, final: /jointset/lumbar/lumbar/value
initial: /jointset/lumbar/lumbar/speed, final: /jointset/lumbar/lumbar/speed
control periodicity pairs:
initial: /forceset/reserve_jointset_hip_l_hip_flexion_l, final: /forceset/reserve_jointset_hip_r_hip_flexion_r
initial: /forceset/reserve_jointset_hip_r_hip_flexion_r, final: /forceset/reserve_jointset_hip_l_hip_flexion_l
initial: /forceset/reserve_jointset_knee_l_knee_angle_l, final: /forceset/reserve_jointset_knee_r_knee_angle_r
initial: /forceset/reserve_jointset_knee_r_knee_angle_r, final: /forceset/reserve_jointset_knee_l_knee_angle_l
initial: /forceset/reserve_jointset_ankle_l_ankle_angle_l, final: /forceset/reserve_jointset_ankle_r_ankle_angle_r
initial: /forceset/reserve_jointset_ankle_r_ankle_angle_r, final: /forceset/reserve_jointset_ankle_l_ankle_angle_l
initial: /forceset/reserve_jointset_groundPelvis_pelvis_tilt, final: /forceset/reserve_jointset_groundPelvis_pelvis_tilt
initial: /forceset/reserve_jointset_groundPelvis_pelvis_ty, final: /forceset/reserve_jointset_groundPelvis_pelvis_ty
initial: /forceset/reserve_jointset_lumbar_lumbar, final: /forceset/reserve_jointset_lumbar_lumbar
speed. MocoAverageSpeedGoal, enabled: true, mode: endpoint_constraint
Kinematic constraints: none
Path constraints: none
States: (total: 20)
/jointset/groundPelvis/pelvis_tilt/value. bounds: [-0.349066, -0.174533]
/jointset/groundPelvis/pelvis_tx/value. bounds: [0, 1]
/jointset/groundPelvis/pelvis_ty/value. bounds: [0.75, 1.25]
/jointset/knee_l/knee_angle_l/speed. bounds: [-50, 50]
/jointset/hip_l/hip_flexion_l/value. bounds: [-0.174533, 1.0472]
/jointset/ankle_r/ankle_angle_r/value. bounds: [-0.261799, 0.436332]
/jointset/hip_r/hip_flexion_r/value. bounds: [-0.174533, 1.0472]
/jointset/knee_l/knee_angle_l/value. bounds: [-0.872665, 0]
/jointset/knee_r/knee_angle_r/value. bounds: [-0.872665, 0]
/jointset/knee_r/knee_angle_r/speed. bounds: [-50, 50]
/jointset/ankle_l/ankle_angle_l/value. bounds: [-0.261799, 0.436332]
/jointset/lumbar/lumbar/value. bounds: [0, 0.349066]
/jointset/groundPelvis/pelvis_tilt/speed. bounds: [-50, 50]
/jointset/groundPelvis/pelvis_tx/speed. bounds: [-50, 50]
/jointset/groundPelvis/pelvis_ty/speed. bounds: [-50, 50]
/jointset/hip_l/hip_flexion_l/speed. bounds: [-50, 50]
/jointset/ankle_r/ankle_angle_r/speed. bounds: [-50, 50]
/jointset/hip_r/hip_flexion_r/speed. bounds: [-50, 50]
/jointset/ankle_l/ankle_angle_l/speed. bounds: [-50, 50]
/jointset/lumbar/lumbar/speed. bounds: [-50, 50]
Controls: (total: 10)
/forceset/reserve_jointset_groundPelvis_pelvis_tilt. bounds: [-inf, inf]
/forceset/reserve_jointset_hip_r_hip_flexion_r. bounds: [-inf, inf]
/forceset/reserve_jointset_groundPelvis_pelvis_tx. bounds: [-inf, inf]
/forceset/reserve_jointset_groundPelvis_pelvis_ty. bounds: [-inf, inf]
/forceset/reserve_jointset_hip_l_hip_flexion_l. bounds: [-inf, inf]
/forceset/reserve_jointset_knee_l_knee_angle_l. bounds: [-inf, inf]
/forceset/reserve_jointset_knee_r_knee_angle_r. bounds: [-inf, inf]
/forceset/reserve_jointset_ankle_l_ankle_angle_l. bounds: [-inf, inf]
/forceset/reserve_jointset_ankle_r_ankle_angle_r. bounds: [-inf, inf]
/forceset/reserve_jointset_lumbar_lumbar. bounds: [-inf, inf]
Parameters: none
[info] Number of threads: 12
List of user-set options:
Name Value used
acceptable_compl_inf_tol = 0.0001 yes
acceptable_constr_viol_tol = 0.0001 yes
acceptable_dual_inf_tol = 0.0001 yes
acceptable_tol = 0.0001 yes
compl_inf_tol = 0.0001 yes
constr_viol_tol = 0.0001 yes
dual_inf_tol = 0.0001 yes
hessian_approximation = limited-memory yes
max_iter = 1000 yes
print_user_options = yes yes
tol = 0.0001 yes
******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
Ipopt is released as open source code under the Eclipse Public License (EPL).
For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************
This is Ipopt version 3.12.8, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).
Number of nonzeros in equality constraint Jacobian...: 85769
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 0
Total number of variables............................: 3031
variables with only lower bounds: 0
variables with lower and upper bounds: 2021
variables with only upper bounds: 0
Total number of equality constraints.................: 2529
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
Warning: intermediate_callback is disfunctional in your installation. You will only be able to use stats(). See https://github.com/casadi/casadi/wiki/enableIpoptCallback to enable it.
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 1.4536627e-01 3.69e+00 4.69e-03 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 4.6156915e-01 7.55e+01 1.05e+03 -0.5 2.74e+00 - 9.43e-01 7.66e-01f 1
2 2.0263136e+00 8.70e+01 1.30e+03 0.1 1.66e+00 - 1.00e+00 1.00e+00H 1
3 1.8334886e+00 5.09e+01 5.97e+02 0.1 1.23e+00 - 3.50e-01 5.00e-01f 2
4 1.9776751e+00 1.19e+01 5.32e+02 0.1 8.94e-01 - 1.00e+00 1.00e+00f 1
5 3.6618817e+00 3.17e+00 1.01e+02 0.1 9.61e-01 - 1.00e+00 1.00e+00f 1
6 1.7087051e+00 7.77e+00 2.76e+01 -1.1 1.16e+00 - 9.96e-01 1.00e+00h 1
7 5.5687771e-01 2.35e+01 4.01e+01 -1.8 6.84e-01 - 1.00e+00 1.00e+00h 1
8 4.0238651e-01 3.60e+00 8.29e+00 -3.1 1.31e+00 - 1.00e+00 1.00e+00h 1
9 2.7601157e-01 2.07e+00 5.44e+00 -2.8 6.94e-01 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 2.1717293e-01 4.12e-01 3.36e+00 -3.4 2.90e-01 - 1.00e+00 1.00e+00h 1
11 1.6119098e-01 2.18e+01 2.54e+00 -4.4 9.40e-01 - 1.00e+00 1.00e+00h 1
12 1.8417969e-01 5.86e+00 2.20e+00 -3.9 1.39e+00 - 1.00e+00 8.87e-01H 1
13 1.3498169e-01 1.27e+00 1.10e+00 -4.1 7.63e-01 - 1.00e+00 1.00e+00h 1
14 1.2960389e-01 5.29e-02 2.08e-01 -5.4 5.52e-02 - 1.00e+00 1.00e+00h 1
15 1.1892868e-01 5.04e+00 2.68e-01 -6.3 4.40e-01 - 1.00e+00 1.00e+00h 1
16 1.1245441e-01 4.79e+00 2.94e-01 -4.3 1.16e+00 - 1.00e+00 8.55e-02h 1
17 1.6985026e-01 3.14e+00 3.54e-01 -3.7 6.57e-01 - 1.14e-01 9.93e-01h 1
18 9.8011791e-02 9.84e-01 1.56e-01 -4.3 6.07e-01 - 1.00e+00 1.00e+00h 1
19 9.1188473e-02 3.64e-01 8.51e-02 -5.1 1.15e-01 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 8.8584022e-02 2.54e-01 3.38e-02 -4.5 1.47e-01 - 1.00e+00 1.00e+00h 1
21 8.7843518e-02 7.09e-01 5.27e-02 -4.8 3.36e-01 - 1.00e+00 1.00e+00h 1
22 8.7272286e-02 6.58e-01 7.61e-02 -5.5 4.37e-01 - 3.17e-01 6.73e-01H 1
23 1.0139158e-01 3.30e-01 5.02e-02 -5.8 2.84e-01 - 9.90e-01 1.00e+00H 1
24 8.0335388e-02 2.44e-01 6.57e-02 -5.9 2.91e-01 - 1.00e+00 1.00e+00h 1
25 7.8477111e-02 5.65e-02 6.81e-03 -5.1 9.86e-02 - 1.00e+00 1.00e+00h 1
26 7.7692165e-02 3.02e-01 2.01e-02 -5.9 3.75e-01 - 1.00e+00 1.00e+00h 1
27 7.5792240e-02 3.65e-01 4.33e-02 -4.2 1.49e+00 - 4.01e-01 2.18e-01h 3
28 7.3634520e-02 4.97e-01 7.25e-02 -4.6 1.67e+00 - 2.95e-01 2.22e-01h 3
29 2.2111217e-01 3.70e+00 2.67e-01 -5.2 1.17e+00 - 9.99e-01 1.00e+00H 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 7.2816453e-02 2.40e+00 1.28e-01 -5.3 1.05e+00 - 5.44e-01 1.00e+00h 1
31 7.8223313e-02 3.20e+00 3.00e-01 -5.5 3.26e-01 - 1.00e+00 1.00e+00h 1
32 6.9663547e-02 7.31e-01 1.05e-01 -6.5 1.75e-01 - 1.00e+00 1.00e+00h 1
33 6.9027863e-02 6.17e-02 6.90e-02 -7.9 9.07e-02 - 1.00e+00 1.00e+00h 1
34 6.8457065e-02 1.08e-02 1.57e-02 -9.1 5.62e-02 - 1.00e+00 1.00e+00h 1
35 6.7767402e-02 2.07e-01 8.64e-02 -7.1 5.57e-01 - 1.00e+00 3.10e-01h 2
36 6.7247049e-02 4.28e-01 1.46e-01 -5.1 8.40e-01 - 1.00e+00 3.04e-01h 1
37 6.6332557e-02 6.18e-01 1.85e-01 -5.1 1.12e+00 - 8.24e-01 2.50e-01h 3
38 6.5566855e-02 6.43e-01 1.93e-01 -6.0 1.41e+00 - 7.62e-01 1.60e-01h 1
39 6.4927229e-02 5.63e-01 1.80e-01 -5.3 7.52e-01 - 1.00e+00 1.25e-01h 4
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 6.6385710e-02 2.39e+00 7.48e-02 -6.2 2.67e-01 - 1.00e+00 1.00e+00h 1
41 6.5133685e-02 6.73e-02 3.24e-02 -7.6 2.79e-01 - 1.00e+00 1.00e+00h 1
42 6.2957873e-02 3.70e-02 1.21e-02 -6.4 2.67e-01 - 1.00e+00 1.00e+00h 1
43 6.2202989e-02 1.21e-02 1.01e-02 -6.9 5.66e-02 - 1.00e+00 1.00e+00h 1
44 6.7169018e-02 8.55e-03 1.27e-02 -5.8 2.58e-01 - 7.60e-01 1.00e+00H 1
45 6.1492894e-02 2.21e-02 9.17e-03 -6.2 3.06e-01 - 1.00e+00 1.00e+00h 1
46 6.1178479e-02 1.54e-03 2.55e-03 -7.6 3.02e-02 - 1.00e+00 1.00e+00h 1
47 6.0972548e-02 3.24e-03 1.02e-03 -8.4 3.81e-02 - 1.00e+00 1.00e+00h 1
48 6.0871244e-02 2.43e-01 8.95e-03 -6.4 4.87e-01 - 1.00e+00 4.34e-01h 1
49 9.0944162e-02 1.94e-01 7.34e-02 -6.0 8.63e-01 - 1.21e-01 9.66e-01H 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 6.3748785e-02 6.80e-01 2.22e-02 -6.4 6.19e-01 - 1.00e+00 1.00e+00h 1
51 6.3007126e-02 5.29e-01 1.55e-02 -6.4 3.70e-01 - 5.65e-01 9.00e-01h 1
52 6.0962206e-02 2.36e-01 1.86e-02 -6.4 2.48e-01 - 3.18e-01 1.00e+00h 1
53 6.5611607e-02 1.48e-01 1.48e-02 -6.4 3.38e-01 - 1.00e+00 8.68e-01h 1
54 5.9254349e-02 1.17e-01 7.47e-03 -6.4 3.10e-01 - 1.00e+00 1.00e+00h 1
55 5.8621506e-02 1.22e-02 4.13e-03 -6.6 9.43e-02 - 1.00e+00 1.00e+00h 1
56 5.8273634e-02 9.40e-02 2.21e-03 -7.5 3.52e-02 - 1.00e+00 1.00e+00h 1
57 5.8120499e-02 1.98e-01 2.48e-02 -7.3 7.54e-02 - 4.02e-01 1.00e+00h 1
58 5.7856002e-02 4.23e-01 9.49e-03 -7.6 1.26e-01 - 1.00e+00 1.00e+00h 1
59 5.8128230e-02 6.52e-03 1.19e-02 -6.6 2.73e-01 - 7.41e-01 8.91e-01H 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
60 5.7969074e-02 6.29e-03 1.30e-02 -11.0 2.15e-01 - 4.51e-01 4.65e-02h 1
61 5.6449162e-02 8.84e-01 1.43e-02 -6.7 3.96e-01 - 7.58e-01 8.06e-01h 1
62 6.2000479e-02 5.34e-01 5.75e-02 -6.8 5.19e-01 - 1.75e-01 1.00e+00H 1
63 5.9139916e-02 8.41e-02 3.05e-02 -6.8 3.40e-01 - 5.33e-01 1.00e+00h 1
64 5.6095166e-02 1.23e-01 6.08e-03 -6.8 3.20e-01 - 1.00e+00 1.00e+00h 1
65 5.4535787e-02 1.39e-01 7.07e-03 -7.3 2.07e-01 - 1.00e+00 1.00e+00h 1
66 5.4063605e-02 4.40e-02 5.10e-03 -6.1 1.82e-01 - 7.92e-01 8.58e-01h 1
67 5.4303813e-02 1.03e-01 1.48e-02 -6.4 4.81e-01 - 1.00e+00 1.00e+00H 1
68 5.3491128e-02 5.86e-02 2.03e-02 -7.1 7.42e-01 - 9.65e-01 4.60e-01h 1
69 5.2989959e-02 5.87e-02 2.11e-02 -7.6 6.37e-01 - 8.27e-01 2.92e-01h 2
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
70 5.2418580e-02 1.94e-02 9.86e-03 -7.8 1.40e-01 - 9.89e-01 6.72e-01h 1
71 5.2027221e-02 1.71e-02 2.48e-03 -7.6 9.43e-02 - 1.00e+00 1.00e+00h 1
72 5.1877870e-02 7.78e-02 2.58e-03 -6.9 3.09e-01 - 7.61e-01 7.86e-01h 1
73 5.1721012e-02 1.32e-01 6.91e-03 -5.8 2.67e+00 - 6.65e-02 1.19e-01h 4
74 5.1827452e-02 1.98e-01 1.23e-02 -5.1 4.46e+00 - 5.97e-02 1.18e-01h 4
75 5.1681165e-02 1.88e-01 1.33e-02 -6.3 2.14e+00 - 1.00e+00 8.24e-02h 4
76 6.2653764e-02 3.26e-02 1.89e-02 -6.4 3.38e-01 - 1.00e+00 1.00e+00H 1
77 5.1880495e-02 1.20e-01 8.02e-03 -6.5 3.20e-01 - 1.00e+00 1.00e+00h 1
78 5.4473290e-02 5.74e-03 1.19e-02 -6.5 2.84e-01 - 1.00e+00 1.00e+00H 1
79 5.1478519e-02 1.53e-01 5.77e-03 -6.7 3.78e-01 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
80 6.9053937e-02 1.69e-02 3.07e-02 -6.5 2.85e-01 - 1.00e+00 1.00e+00H 1
81 5.0984046e-02 1.35e-01 6.78e-03 -6.6 2.77e-01 - 1.00e+00 1.00e+00h 1
82 5.0815022e-02 1.13e-03 2.13e-03 -7.1 3.98e-02 - 1.00e+00 1.00e+00h 1
83 5.0785434e-02 7.57e-03 1.18e-03 -7.2 8.52e-02 - 1.00e+00 1.00e+00h 1
84 5.0692824e-02 1.23e-03 7.42e-04 -8.1 3.79e-02 - 1.00e+00 1.00e+00h 1
85 5.0658462e-02 2.18e-03 1.01e-03 -8.5 3.96e-02 - 1.00e+00 1.00e+00h 1
86 5.0608507e-02 2.79e-03 2.07e-03 -7.2 4.57e-02 - 1.00e+00 1.00e+00h 1
87 5.0537453e-02 3.08e-03 8.17e-03 -6.5 2.26e-01 - 4.68e-01 9.55e-02h 1
88 5.0429242e-02 5.01e-03 6.54e-03 -6.4 1.16e+00 - 8.67e-02 5.60e-02h 1
89 5.0427678e-02 1.11e-02 1.26e-02 -7.4 1.09e+00 - 6.69e-02 1.27e-01h 2
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
90 5.0287410e-02 1.20e-02 1.37e-02 -7.6 8.14e-01 - 6.85e-01 9.94e-02h 1
91 5.0089669e-02 3.46e-02 1.42e-02 -7.4 3.90e+00 - 3.33e-02 5.15e-02h 1
92 5.0041185e-02 3.47e-02 1.42e-02 -11.0 3.63e+00 - 2.61e-02 5.47e-03h 1
93 4.9956254e-02 5.25e-02 1.61e-02 -7.9 1.99e+00 - 1.17e-03 6.26e-02h 1
94 4.9906496e-02 5.23e-02 1.67e-02 -8.2 1.84e+00 - 7.01e-02 1.09e-02h 1
95 4.9823508e-02 1.08e-01 2.05e-02 -7.8 7.71e-01 - 1.84e-01 2.99e-01h 1
96 4.9673022e-02 1.13e-01 2.35e-02 -6.7 3.90e+00 - 6.94e-02 1.67e-02h 1
97 4.9499671e-02 1.17e-01 2.72e-02 -7.7 3.68e+00 - 2.60e-02 2.07e-02h 1
98 4.9508035e-02 1.79e-01 7.10e-02 -5.2 2.78e+00 - 6.09e-03 1.14e-01f 3
99 4.9498240e-02 3.18e-01 6.72e-02 -7.1 8.03e+00 - 8.99e-02 5.37e-02H 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
100 4.9301412e-02 3.40e-01 6.71e-02 -11.0 6.40e+00 - 1.09e-01 3.71e-02h 1
101 4.9206973e-02 3.89e-01 6.47e-02 -5.2 4.59e+00 - 1.72e-02 3.71e-02f 4
102 6.8061263e-02 6.23e-01 3.25e-01 -6.1 5.50e-01 - 1.46e-01 9.02e-01H 1
103 5.2281313e-02 2.51e-01 2.77e-01 -6.6 3.40e-01 - 1.00e+00 6.49e-01h 1
104 5.1318432e-02 6.05e-01 2.39e-01 -6.6 4.92e-01 - 9.36e-01 6.95e-01h 1
105 5.0766477e-02 6.75e-01 2.39e-01 -6.6 3.06e-01 - 8.41e-01 5.23e-01h 1
106 4.9097479e-02 5.66e-02 6.17e-02 -6.6 1.55e-01 - 4.66e-02 1.00e+00h 1
107 4.8632998e-02 3.67e-02 2.44e-02 -7.2 8.04e-02 - 9.40e-01 5.75e-01h 1
108 4.8342891e-02 4.57e-02 6.10e-03 -6.9 5.59e-02 - 1.00e+00 1.00e+00h 1
109 4.8348450e-02 2.34e-03 4.04e-03 -7.3 3.38e-02 - 1.00e+00 9.83e-01H 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
110 4.8232600e-02 1.84e-03 1.43e-03 -7.8 1.83e-02 - 1.00e+00 1.00e+00h 1
111 4.8207354e-02 6.00e-04 7.67e-04 -8.2 1.52e-02 - 1.00e+00 1.00e+00h 1
112 4.8164493e-02 2.96e-03 1.50e-03 -8.9 1.88e-02 - 1.00e+00 1.00e+00h 1
113 4.8090019e-02 4.46e-02 5.39e-03 -9.2 2.18e-01 - 1.00e+00 3.03e-01h 1
114 4.8088881e-02 4.45e-02 1.36e-02 -11.0 3.45e-01 - 2.13e-01 1.29e-03h 1
115 4.8062678e-02 4.26e-02 1.30e-02 -7.9 2.85e-01 - 1.24e-01 4.40e-02h 1
116 4.8022224e-02 4.14e-02 1.26e-02 -7.8 6.40e-01 - 1.32e-01 2.88e-02h 1
117 4.7984550e-02 4.09e-02 1.24e-02 -7.0 1.62e+00 - 1.86e-02 1.23e-02h 1
118 4.7941930e-02 3.98e-02 1.20e-02 -7.2 7.78e-01 - 6.61e-02 3.23e-02h 1
119 4.7878671e-02 3.88e-02 1.12e-02 -6.4 1.01e+00 - 6.90e-02 4.16e-02h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
120 4.7873363e-02 3.86e-02 1.12e-02 -11.0 6.32e-01 - 3.08e-02 4.81e-03h 1
121 4.7787700e-02 3.72e-02 1.10e-02 -6.8 4.35e+00 - 1.78e-02 1.87e-02h 1
122 4.7768906e-02 3.71e-02 1.07e-02 -6.7 1.69e+00 - 7.83e-02 5.84e-03h 1
123 4.7709642e-02 3.84e-02 1.00e-02 -6.8 1.31e+00 - 1.17e-02 8.47e-02h 1
124 4.7680911e-02 3.82e-02 9.90e-03 -9.0 2.53e+00 - 2.18e-02 8.30e-03h 1
125 4.7617532e-02 3.74e-02 9.73e-03 -8.5 3.53e+00 - 2.11e-02 1.69e-02h 1
126 4.7604517e-02 3.47e-02 8.75e-03 -6.8 3.19e+00 - 1.44e-01 6.30e-02h 1
127 4.7677665e-02 3.71e-02 1.79e-02 -5.3 1.80e+00 - 6.35e-03 4.96e-02h 5
128 4.7501139e-02 3.18e-02 1.09e-02 -7.1 4.09e-01 - 5.31e-01 4.10e-01h 1
129 4.7690621e-02 3.40e-02 1.03e-02 -5.2 8.26e-01 - 8.61e-02 5.42e-02f 4
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
130 4.7384047e-02 2.24e-03 6.98e-04 -6.3 5.31e-02 - 1.00e+00 1.00e+00h 1
131 4.7360173e-02 2.58e-03 5.90e-04 -6.6 1.89e-02 - 1.00e+00 1.00e+00h 1
132 4.7318588e-02 6.22e-04 6.55e-04 -7.1 4.18e-02 - 1.00e+00 1.00e+00h 1
133 4.7321654e-02 1.98e-03 6.02e-03 -6.3 1.74e-01 - 1.00e+00 2.76e-01h 2
134 4.7412323e-02 7.19e-05 2.32e-03 -6.4 5.26e-02 - 7.76e-01 1.00e+00H 1
135 4.7237805e-02 8.61e-04 2.24e-03 -7.7 4.23e-02 - 1.00e+00 7.12e-01h 1
136 4.7246758e-02 1.01e-05 1.20e-03 -7.1 2.80e-02 - 1.00e+00 1.00e+00H 1
137 4.7196873e-02 7.09e-05 5.83e-03 -7.8 3.09e-02 - 1.00e+00 2.94e-01h 1
138 4.7141307e-02 2.60e-03 1.84e-03 -7.5 1.12e-01 - 1.00e+00 4.76e-01h 2
139 4.7076268e-02 7.66e-03 2.73e-03 -8.0 4.97e-01 - 4.34e-01 1.57e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
140 4.7051576e-02 8.43e-03 5.85e-03 -7.0 6.18e-01 - 8.93e-02 5.24e-02h 5
141 4.7360710e-02 8.02e-04 2.27e-03 -7.3 1.26e-01 - 1.00e+00 1.00e+00H 1
142 4.7035842e-02 2.17e-03 2.77e-03 -7.4 6.43e-02 - 8.04e-01 6.21e-01h 1
143 4.7518686e-02 4.74e-03 4.39e-03 -7.5 7.54e-02 - 1.00e+00 1.00e+00H 1
144 4.6797538e-02 8.23e-02 2.60e-02 -7.6 1.58e-01 - 1.84e-01 1.00e+00h 1
145 4.6896709e-02 6.66e-02 2.05e-02 -7.1 3.33e-01 - 9.19e-01 2.69e-01h 1
146 4.6796704e-02 2.46e-02 7.62e-03 -8.2 1.03e-01 - 3.89e-01 6.30e-01h 1
147 4.6746206e-02 2.30e-02 1.01e-02 -8.1 1.57e-01 - 6.90e-01 6.28e-02h 1
148 4.6692269e-02 1.47e-02 1.28e-02 -6.9 8.99e-02 - 2.65e-01 6.98e-01h 1
149 4.6686861e-02 1.45e-02 1.27e-02 -11.0 1.24e-01 - 1.64e-01 1.53e-02h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
150 4.6843005e-02 8.94e-05 4.74e-03 -6.8 1.71e-01 - 6.79e-01 1.00e+00H 1
151 4.6764553e-02 4.79e-04 1.18e-02 -11.0 3.07e-01 - 5.50e-01 1.45e-01h 1
152 4.7137063e-02 3.87e-05 4.72e-03 -7.0 6.55e-02 - 1.00e+00 1.00e+00H 1
153 4.6524071e-02 5.41e-03 6.95e-04 -7.5 5.25e-02 - 1.00e+00 9.62e-01h 1
154 4.6499241e-02 5.61e-03 8.19e-04 -7.9 6.08e-02 - 1.00e+00 1.00e+00h 1
155 4.6461127e-02 7.41e-03 3.79e-03 -9.2 3.79e-01 - 4.74e-01 2.18e-01h 1
156 4.6425448e-02 8.00e-03 3.23e-03 -7.9 9.71e+00 - 6.63e-02 5.81e-03h 1
157 4.6401887e-02 7.93e-03 1.44e-02 -11.0 1.01e+01 - 4.38e-02 3.80e-03h 1
158 4.6394269e-02 3.34e-02 1.40e-02 -6.4 2.67e+01 - 1.82e-03 1.40e-02h 1
159 4.6389025e-02 3.33e-02 1.40e-02 -11.0 8.07e+00 - 3.72e-02 9.27e-04h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
160 4.6353695e-02 3.76e-01 1.35e-02 -6.3 1.18e+01 - 1.06e-01 2.86e-02h 2
161 4.6290417e-02 3.66e-01 1.27e-02 -7.6 4.81e+00 - 2.65e-01 2.56e-02h 1
162 4.6341577e-02 3.14e-01 2.31e-02 -5.3 2.61e+00 - 2.14e-02 1.43e-01h 1
163 4.6326195e-02 3.13e-01 2.24e-02 -6.9 2.28e+00 - 1.61e-01 2.74e-03h 1
164 4.6486544e-02 1.68e-01 1.66e-02 -6.9 2.79e-01 - 1.00e+00 4.67e-01h 1
165 4.6079935e-02 1.25e-01 1.04e-02 -6.9 8.08e-02 - 3.76e-01 3.88e-01h 1
166 4.5941491e-02 6.06e-02 7.48e-03 -6.4 6.54e-02 - 1.00e+00 1.00e+00f 1
167 4.5857703e-02 1.50e-03 2.47e-03 -6.7 2.35e-02 - 1.00e+00 1.00e+00h 1
168 4.5824240e-02 6.67e-04 2.68e-04 -7.0 1.24e-02 - 1.00e+00 1.00e+00h 1
169 4.5810729e-02 1.71e-04 3.73e-04 -7.1 7.55e-03 - 1.00e+00 9.79e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
170 4.5777110e-02 2.55e-03 1.05e-03 -6.8 3.03e-02 - 1.00e+00 1.00e+00h 1
171 4.5749438e-02 6.06e-03 5.75e-03 -6.6 2.60e-01 - 5.81e-01 1.50e-01h 2
172 4.5749988e-02 5.41e-03 1.46e-02 -7.0 1.30e-01 - 9.96e-01 2.75e-01H 1
173 4.5797296e-02 1.23e-02 5.83e-03 -7.1 6.38e-02 - 1.00e+00 1.00e+00h 1
174 4.5794958e-02 7.21e-03 4.91e-03 -7.2 4.88e-02 - 7.67e-01 7.39e-01h 1
175 4.5693237e-02 1.35e-02 1.44e-03 -7.2 3.82e-02 - 5.17e-01 6.74e-01f 1
176 4.5736632e-02 9.83e-04 1.28e-03 -7.8 2.08e-02 - 1.00e+00 1.00e+00h 1
177 4.5660372e-02 7.93e-04 1.37e-04 -7.8 1.64e-02 - 1.00e+00 9.95e-01h 1
178 4.5654538e-02 1.85e-04 1.01e-03 -7.8 1.28e-02 - 9.39e-01 7.98e-01h 1
179 4.5635220e-02 1.52e-03 1.96e-03 -7.4 9.65e-02 - 7.11e-01 3.96e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
180 4.5605863e-02 8.78e-03 4.89e-03 -6.7 2.13e+00 - 1.18e-01 3.53e-02h 1
181 4.5595881e-02 9.10e-03 4.95e-03 -6.6 1.27e+00 - 1.88e-02 2.72e-02h 4
182 4.5585725e-02 8.49e-03 4.49e-03 -7.4 4.66e-01 - 4.73e-01 8.63e-02h 3
183 4.5581283e-02 1.50e-02 6.47e-03 -6.6 7.75e-01 - 2.23e-01 1.27e-01h 2
184 4.5579238e-02 1.93e-02 7.98e-03 -6.8 1.16e+00 - 3.77e-01 7.47e-02h 2
185 4.5573314e-02 2.91e-02 9.66e-03 -6.9 1.04e+00 - 3.55e-01 1.28e-01h 1
186 5.5481281e-02 8.00e-03 1.15e-01 -6.3 5.75e-01 - 1.01e-01 1.00e+00H 1
187 4.9264247e-02 3.40e-02 7.61e-02 -6.9 5.19e-01 - 5.81e-01 3.55e-01h 1
188 4.8400350e-02 9.10e-02 4.53e-02 -6.9 8.16e-01 - 5.75e-02 2.88e-01f 2
189 4.6960019e-02 7.65e-02 2.05e-02 -6.9 2.27e-01 - 6.33e-01 5.26e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
190 4.6632686e-02 2.36e-02 3.72e-03 -6.9 1.93e-01 - 1.00e+00 9.50e-01h 1
191 4.5551938e-02 4.81e-03 1.28e-01 -6.9 7.25e-02 - 9.51e-02 1.00e+00f 1
192 4.5473585e-02 5.46e-04 5.50e-04 -7.1 3.33e-02 - 1.00e+00 1.00e+00h 1
193 4.5447623e-02 9.32e-05 2.95e-04 -7.5 8.20e-03 - 1.00e+00 1.00e+00h 1
194 4.5430989e-02 4.30e-04 1.54e-03 -7.2 1.66e-02 - 1.00e+00 9.10e-01h 1
195 4.5435088e-02 1.61e-06 6.02e-04 -7.2 2.20e-02 - 1.00e+00 1.00e+00H 1
196 4.5420569e-02 3.75e-05 8.58e-03 -11.0 1.56e-02 - 9.77e-01 4.33e-01h 1
197 4.5408981e-02 7.25e-05 9.88e-05 -7.8 1.17e-02 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 197
(scaled) (unscaled)
Objective...............: 4.5408981029913309e-02 4.5408981029913309e-02
Dual infeasibility......: 9.8764006569054591e-05 9.8764006569054591e-05
Constraint violation....: 4.4577046783799086e-06 7.2468183165641520e-05
Complementarity.........: 1.7646130473460861e-08 1.7646130473460861e-08
Overall NLP error.......: 9.8764006569054591e-05 9.8764006569054591e-05
Number of objective function evaluations = 330
Number of objective gradient evaluations = 198
Number of equality constraint evaluations = 330
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 198
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 0
Total CPU secs in IPOPT (w/o function evaluations) = 13.542
Total CPU secs in NLP function evaluations = 66.006
EXIT: Optimal Solution Found.
nlp : t_proc (avg) t_wall (avg) n_eval
callback_fun | 18.00ms ( 90.91us) 20.03ms (101.17us) 198
nlp_f | 717.00ms ( 2.17ms) 706.89ms ( 2.14ms) 330
nlp_g | 1.61 s ( 4.87ms) 1.62 s ( 4.90ms) 330
nlp_grad_f | 6.70 s ( 33.69ms) 6.71 s ( 33.70ms) 199
nlp_jac_g | 57.29 s (287.88ms) 57.29 s (287.88ms) 199
total | 79.55 s ( 79.55 s) 79.55 s ( 79.55 s) 1
Breakdown of objective (including weights):
effort: 0.045409
[info] Set log level to Info.
[info] ------------------------------------------------------------------------
[info] Elapsed real time: 80 second(s) (1 minute(s), 20 second(s)).
[info] Sat Nov 18 01:45:44 2023
[info] MocoCasADiSolver succeeded!
[info] ========================================================================Code: Select all
def gen_guess():
model = osim.Model(ref_model_path)
if not muscle_driven:
# Remove actuators, muscles, and groups and add reserve actuators
model.updForceSet().clearAndDestroy()
modelProcessor = osim.ModelProcessor(model)
modelProcessor.append(osim.ModOpAddReserves(actuator_force))
model = modelProcessor.process()
model.initSystem()
tableProcessor = osim.TableProcessor(ref_coords_path)
tableProcessor.append(osim.TabOpLowPassFilter(filter_freq))
tableProcessor.append(osim.TabOpUseAbsoluteStateNames())
track = osim.MocoTrack()
track.setName(os.path.join(project_dir, 'Guess', mocoTrackName))
track.setModel(modelProcessor)
track.setStatesReference(tableProcessor)
track.set_states_global_tracking_weight(stateTrackingWeight)
track.set_allow_unused_references(True)
track.set_track_reference_position_derivatives(True)
track.set_apply_tracked_states_to_guess(True)
track.set_initial_time(t0)
track.set_final_time(tf)
study = track.initialize() # save joint angles *and speeds* to mocoTrackName_tracked_states.sto
problem = study.updProblem()
# Symmetry goals
symmetryGoal = osim.MocoPeriodicityGoal('symmetryGoal')
problem.addGoal(symmetryGoal)
# Symmetric coordinate values (except for pelvis_tx) and speeds
for i in range(model.getNumStateVariables()):
currentStateName = model.getStateVariableNames().getitem(i)
if currentStateName.startswith('/jointset'):
if '_r' in currentStateName:
pair = osim.MocoPeriodicityGoalPair(currentStateName, currentStateName.replace('_r', '_l'))
symmetryGoal.addStatePair(pair)
if '_l' in currentStateName:
pair = osim.MocoPeriodicityGoalPair(currentStateName, currentStateName.replace('_l', '_r'))
symmetryGoal.addStatePair(pair)
if not np.array([s in currentStateName for s in ['_r','_l','pelvis_tx/value','/activation']]).any():
symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(currentStateName))
# Symmetric controls: actuator
for i in range(model.getForceSet().getSize()):
currentForceName = '/forceset/' + model.getForceSet().get(i).getName()
if currentForceName.startswith('/forceset/reserve'):
if currentForceName.endswith('_r'):
pair = osim.MocoPeriodicityGoalPair(currentForceName, currentForceName.replace('_r', '_l'))
symmetryGoal.addControlPair(pair)
if currentForceName.endswith('_l'):
pair = osim.MocoPeriodicityGoalPair(currentForceName, currentForceName.replace('_l', '_r'))
symmetryGoal.addControlPair(pair)
symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/forceset/reserve_jointset_groundPelvis_pelvis_tilt'))
symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/forceset/reserve_jointset_groundPelvis_pelvis_ty'))
symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/forceset/reserve_jointset_lumbar_lumbar'))
# Effort goal
effort = osim.MocoControlGoal.safeDownCast(problem.updGoal('control_effort'))
effort.setWeight(controlEffortWeight)
# Contact tracking goal
contactTracking = osim.MocoContactTrackingGoal('contact', GRFTrackingWeight)
contactTracking.setExternalLoadsFile(ref_grf_path)
forceNamesRightFoot = osim.StdVectorString()
forceNamesRightFoot.append('contactHeel_r')
forceNamesRightFoot.append('contactFront_r')
contactTracking.addContactGroup(forceNamesRightFoot, 'Right_GRF')
forceNamesLeftFoot = osim.StdVectorString()
forceNamesLeftFoot.append('contactHeel_l')
forceNamesLeftFoot.append('contactFront_l')
contactTracking.addContactGroup(forceNamesLeftFoot, 'Left_GRF')
contactTracking.setProjection('plane')
contactTracking.setProjectionVector(osim.Vec3(0, 0, 1))
problem.addGoal(contactTracking)
# Bounds
problem.setStateInfo('/jointset/groundPelvis/pelvis_tilt/value', [-20*np.pi/180, -10*np.pi/180])
problem.setStateInfo('/jointset/groundPelvis/pelvis_tx/value', [0, 1])
problem.setStateInfo('/jointset/groundPelvis/pelvis_ty/value', [0.75, 1.25])
problem.setStateInfo('/jointset/hip_l/hip_flexion_l/value', [-10*np.pi/180, 60*np.pi/180])
problem.setStateInfo('/jointset/hip_r/hip_flexion_r/value', [-10*np.pi/180, 60*np.pi/180])
problem.setStateInfo('/jointset/knee_l/knee_angle_l/value', [-50*np.pi/180, 0])
problem.setStateInfo('/jointset/knee_r/knee_angle_r/value', [-50*np.pi/180, 0])
problem.setStateInfo('/jointset/ankle_l/ankle_angle_l/value', [-15*np.pi/180, 25*np.pi/180])
problem.setStateInfo('/jointset/ankle_r/ankle_angle_r/value', [-15*np.pi/180, 25*np.pi/180])
problem.setStateInfo('/jointset/lumbar/lumbar/value', [0, 20*np.pi/180])
# Solve
gaitTrackingSolution = study.solve()
fullStride = osim.createPeriodicTrajectory(gaitTrackingSolution)
fullStride.write(tracked_coord_path)
add_name_to_sto(tracked_coord_path, os.path.basename(tracked_coord_path)[:-4])
fullStride_out = multiple_strides(fullStride, stride_number) # make it 4 strides
tracked_coord_path_morestrides = tracked_coord_path[:-4]+'_morestrides.sto'
fullStride_out.write(tracked_coord_path_morestrides)
add_name_to_sto(tracked_coord_path_morestrides, os.path.basename(tracked_coord_path_morestrides)[:-4])
# Extract ground reaction forces
contact_r = osim.StdVectorString()
contact_l = osim.StdVectorString()
contact_r.append('contactHeel_r')
contact_r.append('contactFront_r')
contact_l.append('contactHeel_l')
contact_l.append('contactFront_l')
externalForcesTableFlat = osim.createExternalLoadsTableForGait(model, fullStride_out, contact_r, contact_l)
tracked_grf_path_morestrides = tracked_grf_path[:-4]+'_morestrides.sto'
osim.STOFileAdapter.write(externalForcesTableFlat, tracked_grf_path_morestrides)
add_name_to_sto(tracked_grf_path_morestrides, os.path.basename(tracked_grf_path_morestrides)[:-4])
def gen_models():
model = osim.Model(ref_model_path)
if not muscle_driven:
model.updForceSet().clearAndDestroy() # Remove actuators, muscles, and groups
modelProcessor = osim.ModelProcessor(model)
modelProcessor.append(osim.ModOpAddReserves(actuator_force))
model = modelProcessor.process()
model.initSystem()
model.printToXML(pred_model_path)
###############
# GEN SIMULS #
###############
def gen_simulations():
study = osim.MocoStudy()
study.setName(mocoPredName)
problem = study.updProblem()
modelProcessor = osim.ModelProcessor(pred_model_path)
problem.setModelProcessor(modelProcessor)
# Symmetry goals
symmetryGoal = osim.MocoPeriodicityGoal('symmetryGoal')
problem.addGoal(symmetryGoal)
model = modelProcessor.process()
model.initSystem()
# Symmetric states: coordinate values (except for pelvis_tx) and speeds
for i in range(model.getNumStateVariables()):
currentStateName = model.getStateVariableNames().getitem(i)
if currentStateName.startswith('/jointset'):
if '_r' in currentStateName:
pair = osim.MocoPeriodicityGoalPair(currentStateName, currentStateName.replace('_r', '_l'))
symmetryGoal.addStatePair(pair)
if '_l' in currentStateName:
pair = osim.MocoPeriodicityGoalPair(currentStateName, currentStateName.replace('_l', '_r'))
symmetryGoal.addStatePair(pair)
if not np.array([s in currentStateName for s in ['_r','_l','pelvis_tx/value','/activation']]).any():
symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(currentStateName))
# Symmetric controls: actuator
for i in range(model.getForceSet().getSize()):
currentForceName = '/forceset/' + model.getForceSet().get(i).getName()
if currentForceName.startswith('/forceset/reserve'):
if currentForceName.endswith('_r'):
pair = osim.MocoPeriodicityGoalPair(currentForceName, currentForceName.replace('_r', '_l'))
symmetryGoal.addControlPair(pair)
if currentForceName.endswith('_l'):
pair = osim.MocoPeriodicityGoalPair(currentForceName, currentForceName.replace('_l', '_r'))
symmetryGoal.addControlPair(pair)
symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/forceset/reserve_jointset_groundPelvis_pelvis_tilt'))
symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/forceset/reserve_jointset_groundPelvis_pelvis_ty'))
symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/forceset/reserve_jointset_lumbar_lumbar'))
# Prescribed average gait speed
speedGoal = osim.MocoAverageSpeedGoal('speed')
problem.addGoal(speedGoal)
speedGoal.set_desired_average_speed(speed)
# Effort over distance
effortGoal = osim.MocoControlGoal('effort', controlEffortWeight)
problem.addGoal(effortGoal)
effortGoal.setExponent(3)
effortGoal.setDivideByDisplacement(True) # Costs less to do long distance
# Bounds
problem.setTimeBounds(0, time_bounds)
problem.setStateInfo('/jointset/groundPelvis/pelvis_tilt/value', pelvis_tilt_bounds)
problem.setStateInfo('/jointset/groundPelvis/pelvis_tx/value', pelvis_tx_bounds)
problem.setStateInfo('/jointset/groundPelvis/pelvis_ty/value', pelvis_ty_bounds)
problem.setStateInfo('/jointset/hip_l/hip_flexion_l/value', hip_flexion_l_bounds)
problem.setStateInfo('/jointset/hip_r/hip_flexion_r/value', hip_flexion_r_bounds)
problem.setStateInfo('/jointset/knee_l/knee_angle_l/value', knee_angle_l_bounds)
problem.setStateInfo('/jointset/knee_r/knee_angle_r/value', knee_angle_r_bounds)
problem.setStateInfo('/jointset/ankle_l/ankle_angle_l/value', ankle_angle_l_bounds)
problem.setStateInfo('/jointset/ankle_r/ankle_angle_r/value', ankle_angle_r_bounds)
problem.setStateInfo('/jointset/lumbar/lumbar/value', lumbar_bounds)
# Configure the solver
solver = study.initCasADiSolver()
solver.set_num_mesh_intervals(num_mesh_intervals)
solver.set_verbosity(2)
solver.set_optim_solver('ipopt')
solver.set_optim_convergence_tolerance(convergence_tolerance)
solver.set_optim_constraint_tolerance(constraint_tolerance)
solver.set_optim_max_iterations(max_iterations)
solver.setGuessFile(tracked_coord_path) # Use tracking solution as initial guess
# Solve
study.printToXML(pred_omoco_path)
# study = osim.MocoStudy(pred_omoco_file)
gaitPredictionSolution = study.solve()
# Save trajectory
fullStride = osim.createPeriodicTrajectory(gaitPredictionSolution)
fullStride_out = multiple_strides(fullStride, stride_number) # make it 4 strides
fullStride_out.write(pred_coord_path)
add_name_to_sto(pred_coord_path, os.path.basename(pred_coord_path)[:-4])
# Extract ground reaction forces
contact_r = osim.StdVectorString()
contact_l = osim.StdVectorString()
contact_r.append('contactHeel_r')
contact_r.append('contactFront_r')
contact_l.append('contactHeel_l')
contact_l.append('contactFront_l')
# externalForcesTableFlat = osim.createExternalLoadsTableForGait(model, fullStride_quadruple, contact_r, contact_l)
externalForcesTableFlat = osim.createExternalLoadsTableForGait(model, fullStride_out, contact_r, contact_l)
osim.STOFileAdapter.write(externalForcesTableFlat, pred_grf_path)
add_name_to_sto(pred_grf_path, os.path.basename(pred_grf_path)[:-4])
if __name__ == '__main__':
gen_guess()
gen_models()
gen_simulations()